klampt.plan.kinetrajopt.kinetrajopt module

Implementation of TrajOpt algorithm in klampt.

Classes:

TrajOptSettings(**kw)	Defines settings of the KineTrajOpt solver.
JointObjectInfo(linkid, objid, theta, qid, ...)	Information contraining joint-object collision.
SweepJointObjectInfo(linkid, objid, theta1, ...)	Information for sweeping joint object collision
KineTrajOpt(world, robot[, q0, qf, config, ...])	An implementation of the trajopt library by Josh Schulman, authored by Gao Tang.

Exceptions:

QPException

class klampt.plan.kinetrajopt.kinetrajopt.TrajOptSettings(**kw)

Bases: object

Defines settings of the KineTrajOpt solver.

class klampt.plan.kinetrajopt.kinetrajopt.JointObjectInfo(linkid, objid, theta, qid, jacobian, normal, distance)

Bases: object

Information contraining joint-object collision.

Parameters:

• linkid – int, the id of link with collision, just for bookkeeping

• objid – int, the id of world object, just for bookkeeping

• theta – ndarray, configuration, the configuration of robot at this collision

• qid – int, this is used to locate which index theta is at

• jacobian – ndarray, the jacobian matrix of the interesting point

• normal – ndarray, the normal direction of intervention

• distance – float, the distance for this collision pair

class klampt.plan.kinetrajopt.kinetrajopt.SweepJointObjectInfo(linkid, objid, theta1, theta2, qid1, alpha, jacobian1, jacobian2, normal, distance)

Bases: object

Information for sweeping joint object collision

Parameters:

• linkid – int, the id of link with collision

• objid – int, the id of object

• theta1 – ndarray, configuration at first step

• theta2 – ndarray, configuration at second step

• qid – int, index of theta1, the next one is qid + 1

• alpha – float, the split between two points

• jacobian1 – ndarray, the jacobian matrix of p0 at theta1

• jacobian2 – ndarray, the jacobian matrix of p1 at theta2

• normal – ndarray, the normal direction from object to link hull

• distance – float, the distance between object and convex hull

exception klampt.plan.kinetrajopt.kinetrajopt.QPException

Bases: Exception

class klampt.plan.kinetrajopt.kinetrajopt.KineTrajOpt(world, robot, q0=None, qf=None, config=None, link_index=None, geom_index=None, obs_index=None, mounted=None, constrs=None, losses=None)

Bases: object

An implementation of the trajopt library by Josh Schulman, authored by Gao Tang.

Parameters:

• world – WorldModel, the world which contains obstacle information.

• robot – RobotModel, the robot whose trajectory has to be optimized.

• q0 – arr-like, if not None, it gives the initial configuration of the robot. Its length equals number of joints being optimized. See link_index for details

• qf – arr-like, if not None, it gives the final configuration of the robot. Its length equals q0

• config – TrajOptSettings, it sets some hyperparameters of the solver.

• link_index – arr-like, if not None, it is the links whose configurations are optimized. It can have smaller length than robot.numLinks()

• geom_index – arr-like, if not None, it is the links whose geometries are considered for collision.

• obs_index – arr-like, if not None, it is the index of terrains considered as obstacles

• mounted – list of (int, se3, convexhull), this means other geometries rigidly mounted on link with given index

• constrs – ConstrContainer, if not None, it contains the constraints the trajectory has to satisfy.

• losses – list of CostInterface, if not None, it contains all the loss functions. If None, we optimize sum (q_i - q_{i-1}) ** 2

Methods:

set_q0(q0)	Sets a fixed initial configuration.
set_qf(qf)	Sets a fixed final configuration.
add_pose_constraint(index, linkid, pose)	Add constraint such that at index of the trajectory, the robot link linkid is at pose
add_position_constraint(index, linkid, ...)	Add constraint such that at index of the trajectory, the robot link linkid's local position at world is world_pos
add_orientation_constraint(index, linkid, R)	Add constraint such that the link is at some fixed orientation
add_direction_constraint(index, linkid, ...)	Add constraint such that one link's local direction aligns with some world direction
optimize(theta0)	Given an initial trajectory, use trajopt algorithm to update it.
compute_costs(thetas, point_collisions, ...)	Just compute the cost function at current solution
collision_satisfy(point_collisions, ...)	Check if at current solution, all collision avoidance constraints are satisfied
constrs_satisfy(thetas, ctol)	Check if the constraints are satisfied
solve_qp_with_tr_size(tr_size)	Given trust region size, we solve the qp
build_qp(point_collisions, sweep_collisions, ...)	Create the qp problem to be solved.
create_geometry_cache(robot, link_index, ...)	Populates the internal geometry cache with ConvexHull versions of the link and terrain geometries.
distance_with_one_link(q, geom_order)
all_linkgeom_transforms(thetas)	Compute transform matrix for all active links for all configurations.
find_collision_pair(thetas, dcheck)	Given current solution, find all possible collision pairs.

set_q0(q0)

Sets a fixed initial configuration.

set_qf(qf)

Sets a fixed final configuration.

add_pose_constraint(index, linkid, pose)

Add constraint such that at index of the trajectory, the robot link linkid is at pose

add_position_constraint(index, linkid, lcl_pos, world_pos)

Add constraint such that at index of the trajectory, the robot link linkid’s local position at world is world_pos

add_orientation_constraint(index, linkid, R)

Add constraint such that the link is at some fixed orientation

add_direction_constraint(index, linkid, lcl_dir, world_dir)

Add constraint such that one link’s local direction aligns with some world direction

optimize(theta0)

Given an initial trajectory, use trajopt algorithm to update it.

Parameters:

theta0 (ndarray) – should have shape (N,dof) where N is grid size and dof is the number of links being optimized. It can also have size N*dof, in which case it will be internally reshaped to the proper dimension.

Returns:

The result of optimization. Contains keys:

• ’success’: True if successful, False otherwise

• ’sol’: the solution trajectory, in the same form as theta0.

• ’cost’: the cost of the solution trajectory.

Return type:

dict

compute_costs(thetas, point_collisions, sweep_collisions, d_safe)

Just compute the cost function at current solution

collision_satisfy(point_collisions, sweep_collisions, ctol, dsafe)

Check if at current solution, all collision avoidance constraints are satisfied

constrs_satisfy(thetas, ctol)

Check if the constraints are satisfied

solve_qp_with_tr_size(tr_size)

Given trust region size, we solve the qp

build_qp(point_collisions, sweep_collisions, N, theta0, mu, d_safe)

Create the qp problem to be solved.

point_collisions is an Iterable of JointObjectInfo storing all point collision sweep_collisions is an Iterable of SweepJointObjectInfo storing all sweeping collisions theta0 is the trajectory at the last step mu is the penalty parameter to constraints warm means only trust region size is updated and we can reuse

create_geometry_cache(robot, link_index, geom_index)

Populates the internal geometry cache with ConvexHull versions of the link and terrain geometries.

distance_with_one_link(q, geom_order)

all_linkgeom_transforms(thetas)

Compute transform matrix for all active links for all configurations. Return a list (for each configuration) of list (for each active link) of (R, t) tuples

find_collision_pair(thetas, dcheck)

Given current solution, find all possible collision pairs.