klampt.plan.kinetrajopt.kinetrajopt module

Implementation of TrajOpt algorithm in klampt.

Classes:

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

Exceptions:

QPException

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.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:

add_direction_constraint(index, linkid, …)	Add constraint such that one link’s local direction aligns with some world direction
add_orientation_constraint(index, linkid, R)	Add constraint such that the link is at some fixed orientation
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
all_linkgeom_transforms(thetas)	Compute transform matrix for all active links for all configurations.
build_qp(point_collisions, sweep_collisions, …)	Create the qp problem to be solved.
collision_satisfy(point_collisions, …)	Check if at current solution, all collision avoidance constraints are satisfied
compute_costs(thetas, point_collisions, …)	Just compute the cost function at current solution
constrs_satisfy(thetas, ctol)	Check if the constraints are satisfied
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)
find_collision_pair(thetas, dcheck)	Given current solution, find all possible collision pairs.
optimize(theta0)	Given an initial trajectory, use trajopt algorithm to update it.
set_q0(q0)	Sets a fixed initial configuration.
set_qf(qf)	Sets a fixed final configuration.
solve_qp_with_tr_size(tr_size)	Given trust region size, we solve the qp

add_direction_constraint(index, linkid, lcl_dir, world_dir)

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

add_orientation_constraint(index, linkid, R)

Add constraint such that the link is at some fixed orientation

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

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

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

collision_satisfy(point_collisions, sweep_collisions, ctol, dsafe)

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

compute_costs(thetas, point_collisions, sweep_collisions, d_safe)

Just compute the cost function at current solution

constrs_satisfy(thetas, ctol)

Check if the constraints are satisfied

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)

find_collision_pair(thetas, dcheck)

Given current solution, find all possible collision pairs.

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

set_q0(q0)

Sets a fixed initial configuration.

set_qf(qf)

Sets a fixed final configuration.

solve_qp_with_tr_size(tr_size)

Given trust region size, we solve the qp

exception klampt.plan.kinetrajopt.kinetrajopt.QPException

Bases: Exception

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

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

Bases: object

Defines settings of the KineTrajOpt solver.