Drake
drake::manipulation::planner Namespace Reference

Namespaces

 detail
 
 rbt
 

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

Functions

 GTEST_TEST (ConstraintRelaxingIkTest, SolveIkFromFk)
 
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 Eigen::Ref< const VectorX< double >> &q_current, const Eigen::Ref< const VectorX< double >> &v_current, const Eigen::Ref< const VectorX< double >> &V, const Eigen::Ref< const MatrixX< double >> &J, const DifferentialInverseKinematicsParameters &parameters)
 Computes a generalized velocity v_next, via the following MathematicalProgram: More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const multibody::MultibodyPlant< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_WE_desired, const multibody::Frame< 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...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const multibody::MultibodyPlant< double > &robot, const systems::Context< double > &context, const Isometry3< double > &X_WE_desired, const multibody::Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 A wrapper over DoDifferentialInverseKinematics(robot, context, V_WE_desired, frame_E, params) that tracks frame E's pose in the world frame. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const multibody::MultibodyTree< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_WE_desired, const multibody::Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 Deprecated: Please use the MultibodyPlant version. More...
 
DifferentialInverseKinematicsResult DoDifferentialInverseKinematics (const multibody::MultibodyTree< double > &robot, const systems::Context< double > &context, const Isometry3< double > &X_WE_desired, const multibody::Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
 Deprecated: Please use the MultibodyPlant version. More...
 

Enumeration Type Documentation

◆ DifferentialInverseKinematicsStatus

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.

◆ InterpolatorType

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

◆ ComputePoseDiffInCommonFrame()

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.

◆ DoDifferentialInverseKinematics() [1/5]

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

Computes a generalized velocity v_next, via the following MathematicalProgram:

min_{v_next,alpha} 100 * | alpha - |V| |^2 // iff J.rows() < J.cols(), then

  • | q_current + v_next*dt - q_nominal |^2

s.t. J*v_next = alpha * V / |V| // J*v_next has the same direction as V joint_lim_min <= q_current + v_next*dt <= joint_lim_max joint_vel_lim_min <= v_next <= joint_vel_lim_max joint_accel_lim_min <= (v_next - v_current)/dt <= joint_accel_lim_max for all i > J.rows(), -unconstrained_vel_lim <= S.col(i)' v_next <= unconstrained_vel_lim where J = UΣS' is the SVD, with the singular values in decreasing order. Note that the constraint is imposed on each column independently.

and any additional linear constraints added via AddLinearVelocityConstraint() in the DifferentialInverseKinematicsParameters. where J.rows() == V.size() and J.cols() == v_current.size() == q_current.size() == v_next.size(). V can have any size, with each element representing a constraint on the solution (6 constraints specifying an end-effector pose is typical, but not required).

Intuitively, this finds a v_next such that J*v_next is in the same direction as V, and the difference between |V| and |J * v_next| is minimized while all constraints in parameters are satisfied as well. If the problem is redundant, a secondary objective to minimize |q_current + v_next * 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.

◆ DoDifferentialInverseKinematics() [2/5]

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const multibody::MultibodyPlant< double > &  robot,
const systems::Context< double > &  context,
const Vector6< double > &  V_WE_desired,
const multibody::Frame< 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 context. 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 (specified in frame E) in parameters, and only selecting the non zero elements. J is computed similarly.

Parameters
robotA MultibodyPlant model.
contextMust be the Context of the MultibodyPlant. Contains 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.

◆ DoDifferentialInverseKinematics() [3/5]

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const multibody::MultibodyPlant< double > &  robot,
const systems::Context< double > &  context,
const Isometry3< double > &  X_WE_desired,
const multibody::Frame< double > &  frame_E,
const DifferentialInverseKinematicsParameters parameters 
)

A wrapper over DoDifferentialInverseKinematics(robot, context, 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 context, and dt is taken from parameters.

Parameters
robotA MultibodyPlant model.
contextMust be the Context of the MultibodyPlant. Contains 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.

◆ DoDifferentialInverseKinematics() [4/5]

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const multibody::MultibodyTree< double > &  robot,
const systems::Context< double > &  context,
const Vector6< double > &  V_WE_desired,
const multibody::Frame< double > &  frame_E,
const DifferentialInverseKinematicsParameters parameters 
)

Deprecated: Please use the MultibodyPlant version.

◆ DoDifferentialInverseKinematics() [5/5]

DifferentialInverseKinematicsResult DoDifferentialInverseKinematics ( const multibody::MultibodyTree< double > &  robot,
const systems::Context< double > &  context,
const Isometry3< double > &  X_WE_desired,
const multibody::Frame< double > &  frame_E,
const DifferentialInverseKinematicsParameters parameters 
)

Deprecated: Please use the MultibodyPlant version.

◆ GTEST_TEST()

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

◆ operator<<()

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