klampt.model.cartesian_trajectory module

Reliable Cartesian interpolation functions between arbitrary task space points. Also defines a convenient “bump” function that modify joint-space paths to achieve a task-space 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 joint-space 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 world-space 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:

set_cartesian_constraints(x, constraints, solver)	For x a workspace parameter setting (obtained via config.getConfig(constraints)), a set of constraints, and a IKSolver object, modifies the constraints and the solver so that the solver is setup to match the workspace parameter setting x.
solve_cartesian(x, constraints, solver)	For x a workspace parameter setting (obtained via config.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).
cartesian_interpolate_linear(robot, a, b, ...)	Resolves a continuous robot trajectory that interpolates between two cartesian points for specified IK constraints.
cartesian_interpolate_bisect(robot, a, b, ...)	Resolves a continuous robot trajectory that interpolates between two cartesian points for a single link of a robot.
cartesian_path_interpolate(robot, path, ...)	Resolves a continuous robot trajectory that follows a cartesian path for one or more links of a robot.
cartesian_bump(robot, js_path, constraints, ...)	Given the robot and a reference joint space trajectory, "bumps" the trajectory in Cartesian space using a given relative transform (or transform path).
cartesian_move_to(robot, constraints[, ...])	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)

For x a workspace parameter setting (obtained via config.getConfig(constraints)), a set of constraints, and a IKSolver 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)

For x a workspace parameter setting (obtained via config.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)

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)

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 the maximize 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)

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, not HermiteTrajectory. If a single link is provided, an SE3Trajectory can be provided (but not SE3HermiteTrajectory.)

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)

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 world-space 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 end-effector 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)

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]