Drake
Planning

Detailed Description

A collection of algorithms for finding configurations and/or trajectories of dynamical systems.

Many planning algorithms make heavy use of solvers::MathematicalProgram and the numerous Costs and Constraints. There are also some useful classes in Manipulation.

Classes

class  ConstraintRelaxingIk
 A wrapper class around the IK planner. More...
 
class  GlobalInverseKinematics
 Solves the inverse kinematics problem as a mixed integer convex optimization problem. More...
 
class  InverseKinematics
 Solves an inverse kinematics (IK) problem on a MultibodyPlant, to find the postures of the robot satisfying certain constraints. More...
 
class  StaticEquilibriumProblem
 Finds the static equilibrium pose of a multibody system through optimization. More...
 
class  ZMPPlanner
 Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parameterized as a piecewise polynomial, an optimal center of mass (CoM) trajectory is planned using a linear inverted pendulum model (LIPM). More...
 
class  DirectCollocation
 DirectCollocation implements the approach to trajectory optimization as described in C. More...
 
class  DirectTranscription
 DirectTranscription is perhaps the simplest implementation of a multiple shooting method, where we have decision variables representing the control and input at every sample time in the trajectory, and one-step of numerical integration provides the dynamic constraints between those decision variables. More...
 
class  MultipleShooting
 MultipleShooting is an abstract class for trajectory optimization that creates decision variables for inputs, states, and (optionally) sample times along the trajectory, then provides a number of methods for working with those decision variables. More...
 

Functions

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

Function Documentation

◆ DoDifferentialInverseKinematics() [1/3]

DifferentialInverseKinematicsResult drake::manipulation::planner::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.
JJacobian with respect to generalized velocities v. 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/3]

DifferentialInverseKinematicsResult drake::manipulation::planner::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/3]

DifferentialInverseKinematicsResult drake::manipulation::planner::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.