klampt.model.cartesian_trajectory module
Reliable Cartesian interpolation functions between arbitrary task space points. Also defines a convenient “bump” function that modify jointspace paths to achieve a taskspace displacement.
Utilities for Cartesian IK solving, interpolation, and path adjustment.
To generate a path that moves to a Cartesian target, the
cartesian_move_to()
function is the most convenient.
To interpolate between two Cartesian task space points,
cartesian_interpolate_linear()
and cartesian_interpolate_bisect()
are approximately equivalent.
To interpolate along a Cartesian task space path, the
cartesian_path_interpolate()
function implements two methods for solving
for a feasible robot path. A pointwise approach moves greedily along the path,
while a roadmap approach gives a probabilistically complete solver for the
redundancy resolution.
To move an existing jointspace path by a cartesian offset, the
cartesian_bump()
is used. This is useful for adapting motion primitives
to new sensor data.
All the classes in this module use a unified representation of the workspace of
one or more IKObjective
constraints, which is retrieved by
config.getConfig(constraints)
. For example, the workspace of a position
only constraint is its 3D worldspace target coordinates. The workspace of a
fixed transform constraint is the 12D concatenation of the rotation matrix and
translation vector. See the config
module for details.
Functions:

For 

For 

Resolves a continuous robot trajectory that interpolates between two cartesian points for specified IK constraints. 

Resolves a continuous robot trajectory that interpolates between two cartesian points for a single link of a robot. 

Resolves a continuous robot trajectory that follows a cartesian path for one or more links of a robot. 

Given the robot and a reference joint space trajectory, "bumps" the trajectory in Cartesian space using a given relative transform (or transform path). 

A convenience function that generates a path that performs a linear cartesian interpolation starting from the robot's current configuration and ending at the desired IK constraints. 
 klampt.model.cartesian_trajectory.set_cartesian_constraints(x, constraints, solver)[source]
For
x
a workspace parameter setting (obtained viaconfig.getConfig(constraints)
), a set of constraints, and aIKSolver
object, modifies the constraints and the solver so that the solver is setup to match the workspace parameter setting x. Return type:
None
 klampt.model.cartesian_trajectory.solve_cartesian(x, constraints, solver)[source]
For
x
a workspace parameter setting (obtained viaconfig.getConfig(constraints)
), a set of constraints, and a IKSolver object, returns True if the solver can find a solution, starting from the robot’s current configuration). Returns True if successful. Return type:
bool
 klampt.model.cartesian_trajectory.cartesian_interpolate_linear(robot, a, b, constraints, startConfig='robot', endConfig=None, delta=0.01, solver=None, feasibilityTest=None, maximize=False)[source]
Resolves a continuous robot trajectory that interpolates between two cartesian points for specified IK constraints. The output path is only a kinematic resolution, and has time domain [0,1].
Differs from
cartesian_interpolate_bisect()
in that the solver moves incrementally along the path from a to b in linear fashion. Return type:
Optional
[RobotTrajectory
] Parameters:
robot – the RobotModel or SubRobotModel.
a (list of floats) – start point of the Cartesian trajectory. Assumed derived from config.getConfig(constraints)
b (list of floats) – start point of the Cartesian trajectory. Assumed derived from config.getConfig(constraints)
constraints –
one or more link indices, link names, or
IKObjective
objects specifying the Cartesian task. Interpreted as follows:int or str: the specified link’s entire pose is constrained
IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory.
list of int, list of str, or list of IKObjective: concatenates the specified constraints together
startConfig (optional) – either ‘robot’ (configuration taken from the robot), a configuration, or None (any configuration)
endConfig (optional) – same type as startConfig.
delta (float, optional) – the maximum configuration space distance between points on the output path
solver (IKSolver, optional) – if provided, an IKSolver configured with the desired parameters for IK constraint solving.
feasibilityTest (function, optional) – a function f(q) that returns false when a configuration q is infeasible
maximize (bool, optional) – if true, goes as far as possible along the path.
 Returns:
A configuration space path that interpolates the Cartesian path, or None if no solution can be found.
 klampt.model.cartesian_trajectory.cartesian_interpolate_bisect(robot, a, b, constraints, startConfig='robot', endConfig=None, delta=0.01, solver=None, feasibilityTest=None, maximize=False, growthTol=10)[source]
Resolves a continuous robot trajectory that interpolates between two cartesian points for a single link of a robot. Note that the output path is only a kinematic resolution, and has time domain [0,1].
Differs from
cartesian_interpolate_linear()
in that the solver creates the path from a to b using bisection. This function may be more capable, but doesn’t accept themaximize
argument in case the query doesn’t have a solution. Return type:
Optional
[RobotTrajectory
] Parameters:
robot (RobotModel or SubRobotModel) – the robot.
a (list of floats) – start point of the Cartesian trajectory. Assumed derived from config.getConfig(constraints)
b (list of floats) – start point of the Cartesian trajectory. Assumed derived from config.getConfig(constraints)
constraints –
one or more link indices, link names, or
IKObjective
objects specifying the Cartesian task space. Interpreted as follows:int or str: the specified link’s entire pose is constrained
IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory.
list of int, list of str, or list of IKObjective: concatenates the specified constraints together
startConfig (optional) – either ‘robot’ (configuration taken from the robot), a configuration, or None (any configuration)
endConfig (optional) – same type as startConfig.
delta (float, optional) – the maximum configuration space distance between points on the output path
solver (IKSolver, optional) – if provided, an IKSolver configured with the desired parameters for IK constraint solving.
feasibilityTest (function, optional) – a function f(q) that returns false when a configuration q is infeasible
growthTol (float, optional) – the end path can be at most
growthTol
times the length of the length between the start and goal configurations.
 Returns:
A configuration space path that interpolates the Cartesian path, or None if no solution can be found.
 klampt.model.cartesian_trajectory.cartesian_path_interpolate(robot, path, constraints, startConfig='robot', endConfig=None, delta=0.01, method='any', solver=None, feasibilityTest=None, numSamples=1000, maximize=False)[source]
Resolves a continuous robot trajectory that follows a cartesian path for one or more links of a robot. :rtype:
Optional
[RobotTrajectory
]Note
The output path is only a kinematic resolution, and may not respect the robot’s velocity / acceleration limits.
Note
Only compatible with
Trajectory
, notHermiteTrajectory
. If a single link is provided, anSE3Trajectory
can be provided (but notSE3HermiteTrajectory
.) Parameters:
robot (RobotModel or SubRobotModel) – the robot.
path (Trajectory or list of milestones) – a cartesian path for the parameters of the the given constraints. If only milestones are given, the milestones are spaced 1s apart in time.
constraints –
one or more link indices, link names, or or
IKObjective
objects specifying the Cartesian task space. Interpreted as follows:int or str: the specified link’s entire pose is constrained
IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory.
list of int, list of str, or list of IKObjective: concatenates the specified constraints together
startConfig (optional) – either ‘robot’ (configuration taken from the robot), a configuration, or None (any configuration)
endConfig (optional) – same type as startConfig.
delta (float, optional) – the maximum configuration space distance between points on the output path
method (str) – method used. Can be ‘any’, ‘pointwise’, or ‘roadmap’.
solver (IKSolver, optional) – if provided, an IKSolver configured with the desired parameters for IK constraint solving.
feasibilityTest (function, optional) – a function f(q) that returns False when a configuration q is infeasible
numSamples (int, optional) – if ‘roadmap’ or ‘any’ method is used, the # of configuration space samples that are used.
maximize (bool, optional) – if true, goes as far as possible along the path.
 Returns:
A configuration space path that interpolates the Cartesian path, or None if no solution can be found.
 klampt.model.cartesian_trajectory.cartesian_bump(robot, js_path, constraints, bump_paths, delta=0.01, solver=None, ee_relative=False, maximize=False, closest=False, maxDeviation=None)[source]
Given the robot and a reference joint space trajectory, “bumps” the trajectory in Cartesian space using a given relative transform (or transform path). The movement in joint space is approximately minimized to follow the bumped Cartesian path.
For example, to translate the motion of an end effector by [dx,dy,dz] in world coordinates, call:
cartesian_bump(robot,traj,ik.fixed_objective(link),se3.from_translation([dx,dy,dz]))
 Return type:
Optional
[RobotTrajectory
] Parameters:
robot (RobotModel or SubRobotModel) – the robot for which the bump is applied.
js_path (Trajectory or RobotTrajectory) – the reference joint space Trajectory of the robot.
constraints –
one or more link indices, link names, or
IKObjective
objects specifying the Cartesian task space. giving the manner in which the Cartesian space is defined. Interpreted as follows:int or str: the specified link’s entire pose is constrained
IKObjective: the links, constraint types, local positions, and local axes are used as constraints. The world space elements are considered temporary and will change to match the Cartesian trajectory.
list of int, list of str, or list of IKObjective: concatenates the specified constraints together
bump_paths – one or more transforms or transform paths specifying the worldspace relative “bump” of each cartesian goal. One bump per constraint must be given as input. Each bump can either be a static klampt.se3 element or a SE3Trajectory.
delta (float, optional) – the maximum configuration space distance between points on the output path
method – method used. Can be ‘any’, ‘pointwise’, or ‘roadmap’.
solver (IKSolver, optional) – if provided, an IKSolver configured with the desired parameters for IK constraint solving.
ee_relative (bool, optional) – if False (default), bumps are given in world coordinates. If True, bumps are given in endeffector local coordinates.
maximize (bool, optional) – if true, goes as far as possible along the path.
closest (bool, optional) – if not resolved and this is True, the function returns the robot trajectory whose cartesian targets get as close as possible (locally) to the bumped trajectory
maxDeviation (list of floats, optional) – if not None, this is a vector giving the max joint space distance by which each active joint is allowed to move from js_path.
 Returns:
The bumped trajectory, or None if no path can be found.
 klampt.model.cartesian_trajectory.cartesian_move_to(robot, constraints, delta=0.01, solver=None, feasibilityTest=None, maximize=False)[source]
A convenience function that generates a path that performs a linear cartesian interpolation starting from the robot’s current configuration and ending at the desired IK constraints.
This is a bit more convenient than
cartesian_interpolate_linear()
since you only need to pass in the target objective, rather than the start and end Cartesian parameters as well.Usage:
path = cartesian_move_to(robot,goal)
Other arguments are equivalent to those in cartesian_interpolate_linear.
 Return type:
Optional
[RobotTrajectory
]