Drake
drake::manipulation::planner Namespace Reference

Classes

class  ConstraintRelaxingIk
 A wrapper class around the IK planner. More...
 
class  DifferentialInverseKinematicsParameters
 Contains parameters for differential inverse kinematics. More...
 
struct  DifferentialInverseKinematicsResult
 
class  RobotPlanInterpolator
 This class implements a source of joint positions for a robot. More...
 

Enumerations

enum  DifferentialInverseKinematicsStatus { kSolutionFound, kNoSolutionFound, kStuck }
 
enum  InterpolatorType { ZeroOrderHold, FirstOrderHold, Pchip, Cubic }
 This enum specifies the type of interpolator to use in constructing the piece-wise polynomial. More...
 

Functions

std::ostream & operator<< (std::ostream &os, const DifferentialInverseKinematicsStatus value)
 
Vector6< doubleComputePoseDiffInCommonFrame (const Isometry3< double > &X_C0, const Isometry3< double > &X_C1)
 Computes the pose "difference" between pose1 and pose0 s.t. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const VectorX< double > q_current, const VectorX< double > &v_current, const VectorX< double > &V, const MatrixX< double > &J, const DifferentialInverseKinematicsParameters &parameters)
 Computes a generalized velocity v, s.t. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const RigidBodyTree< double > &robot, const KinematicsCache< double > &cache, const Isometry3< double > &X_WE_desired, const RigidBodyFrame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(robot, cache, V_WE_desired, frame_E, params) that tracks frame E's pose in the world frame. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const RigidBodyTree< double > &robot, const KinematicsCache< double > &cache, const Vector6< double > &V_WE_desired, const RigidBodyFrame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(q_current, v_current, V, J, params) that tracks frame E's spatial velocity. More...
 
 GTEST_TEST (ConstraintRelaxingIkTest, SolveIkFromFk)
 

Enumeration Type Documentation

Enumerator
kSolutionFound 

Found the optimal solution.

kNoSolutionFound 

Solver unable to find a solution.

kStuck 

Unable to follow the desired velocity direction likely due to constraints.

enum InterpolatorType
strong

This enum specifies the type of interpolator to use in constructing the piece-wise polynomial.

Enumerator
ZeroOrderHold 
FirstOrderHold 
Pchip 
Cubic 

Function Documentation

Vector6< double > ComputePoseDiffInCommonFrame ( const Isometry3< double > &  X_C0,
const Isometry3< double > &  X_C1 
)

Computes the pose "difference" between pose1 and pose0 s.t.

the linear part equals p_C1 - p_C0, and the angular part equals R_C1 * R_C0.inv(), where p and R stand for the position and rotation parts, and C is the common frame.

Here is the call graph for this function:

Here is the caller graph for this function:

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const VectorX< double q_current,
const VectorX< double > &  v_current,
const VectorX< double > &  V,
const MatrixX< double > &  J,
const DifferentialInverseKinematicsParameters parameters 
)

Computes a generalized velocity v, s.t.

J * v has the same direction as V, and the difference between |V| and |J * v| is minimized while all constraints in parameters are satisfied as well. If the problem is redundant, a secondary objective to minimize |q_current + v * dt - q_nominal| is added to the problem. It is possible that the solver is unable to find such a generalized velocity while not violating the constraints, in which case, status will be set to kStuck in the returned DifferentialInverseKinematicsResult.

Parameters
q_currentThe current generalized position.
v_currentThe current generalized position.
VDesired spatial velocity. It must have the same number of rows as J.
JGeometric Jacobian. It must have the same number of rows as V. J * v need to represent the same spatial velocity as V.
parametersCollection of various problem specific constraints and constants.
Returns
If the solver successfully finds a solution, joint_velocities will be set to v, otherwise it will be nullopt.

Here is the call graph for this function:

Here is the caller graph for this function:

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const RigidBodyTree< double > &  robot,
const KinematicsCache< double > &  cache,
const Isometry3< double > &  X_WE_desired,
const RigidBodyFrame< double > &  frame_E,
const DifferentialInverseKinematicsParameters parameters 
)

A wrapper over DoDifferentialInverseKinematics(robot, cache, V_WE_desired, frame_E, params) that tracks frame E's pose in the world frame.

q_current and v_current are taken from cache. V_WE is computed by ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) / dt, where X_WE is computed from cache, and dt is taken from parameters.

Parameters
robotRobot model.
cacheKinematiCache built from the current generalized position and velocity.
X_WE_desiredDesired pose of frame_E in the world frame.
frame_EEnd effector frame.
parametersCollection of various problem specific constraints and constants.
Returns
If the solver successfully finds a solution, joint_velocities will be set to v, otherwise it will be nullopt.

Here is the call graph for this function:

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const RigidBodyTree< double > &  robot,
const KinematicsCache< double > &  cache,
const Vector6< double > &  V_WE_desired,
const RigidBodyFrame< double > &  frame_E,
const DifferentialInverseKinematicsParameters parameters 
)

A wrapper over DoDifferentialInverseKinematics(q_current, v_current, V, J, params) that tracks frame E's spatial velocity.

q_current and v_current are taken from cache. V is computed by first transforming V_WE to V_WE_E, then taking the element-wise product between V_WE_E and the gains in parameters, and only selecting the non zero elements. J is computed similarly.

Parameters
robotKinematic tree.
cacheKinematic cache build from the current generalized position and velocity.
V_WE_desiredDesired world frame spatial velocity of frame_E.
frame_EEnd effector frame.
parametersCollection of various problem specific constraints and constants.
Returns
If the solver successfully finds a solution, joint_velocities will be set to v, otherwise it will be nullopt.

Here is the call graph for this function:

drake::manipulation::planner::GTEST_TEST ( ConstraintRelaxingIkTest  ,
SolveIkFromFk   
)

Here is the call graph for this function:

std::ostream & operator<< ( std::ostream &  os,
const DifferentialInverseKinematicsStatus  value 
)