Drake
InverseKinematics Class Reference

Detailed Description

Solves an inverse kinematics (IK) problem on a MultibodyPlant, to find the postures of the robot satisfying certain constraints.

The decision variables include the generalized position of the robot.

#include <drake/multibody/inverse_kinematics/inverse_kinematics.h>

Public Member Functions

 ~InverseKinematics ()
 
 InverseKinematics (const MultibodyPlant< double > &plant)
 Constructs an inverse kinematics problem for a MultibodyPlant. More...
 
 InverseKinematics (const MultibodyPlant< double > &plant, systems::Context< double > *plant_context)
 Constructs an inverse kinematics problem for a MultibodyPlant. More...
 
solvers::Binding< solvers::ConstraintAddPositionConstraint (const Frame< double > &frameB, const Eigen::Ref< const Eigen::Vector3d > &p_BQ, const Frame< double > &frameA, const Eigen::Ref< const Eigen::Vector3d > &p_AQ_lower, const Eigen::Ref< const Eigen::Vector3d > &p_AQ_upper)
 Adds the kinematic constraint that a point Q, fixed in frame B, should lie within a bounding box expressed in another frame A as p_AQ_lower <= p_AQ <= p_AQ_upper, where p_AQ is the position of point Q measured and expressed in frame A. More...
 
solvers::Binding< solvers::ConstraintAddOrientationConstraint (const Frame< double > &frameAbar, const math::RotationMatrix< double > &R_AbarA, const Frame< double > &frameBbar, const math::RotationMatrix< double > &R_BbarB, double theta_bound)
 Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound. More...
 
solvers::Binding< solvers::ConstraintAddGazeTargetConstraint (const Frame< double > &frameA, const Eigen::Ref< const Eigen::Vector3d > &p_AS, const Eigen::Ref< const Eigen::Vector3d > &n_A, const Frame< double > &frameB, const Eigen::Ref< const Eigen::Vector3d > &p_BT, double cone_half_angle)
 Constrains a target point T to be within a cone K. More...
 
solvers::Binding< solvers::ConstraintAddAngleBetweenVectorsConstraint (const Frame< double > &frameA, const Eigen::Ref< const Eigen::Vector3d > &na_A, const Frame< double > &frameB, const Eigen::Ref< const Eigen::Vector3d > &nb_B, double angle_lower, double angle_upper)
 Constrains that the angle between a vector na and another vector nb is between [θ_lower, θ_upper]. More...
 
solvers::Binding< solvers::ConstraintAddMinimumDistanceConstraint (double minimum_distance, double influence_distance_offset=1)
 Adds the constraint that the pairwise distance between objects should be no smaller than minimum_distance. More...
 
solvers::Binding< solvers::ConstraintAddDistanceConstraint (const SortedPair< geometry::GeometryId > &geometry_pair, double distance_lower, double distance_upper)
 
const solvers::VectorXDecisionVariableq () const
 Getter for q. More...
 
const solvers::MathematicalProgramprog () const
 Getter for the optimization program constructed by InverseKinematics. More...
 
solvers::MathematicalProgramget_mutable_prog () const
 Getter for the optimization program constructed by InverseKinematics. More...
 
const systems::Context< double > & context () const
 Getter for the plant context. More...
 
systems::Context< double > * get_mutable_context ()
 Getter for the mutable plant context. More...
 
Does not allow copy, move, or assignment
 InverseKinematics (const InverseKinematics &)=delete
 
InverseKinematicsoperator= (const InverseKinematics &)=delete
 
 InverseKinematics (InverseKinematics &&)=delete
 
InverseKinematicsoperator= (InverseKinematics &&)=delete
 

Constructor & Destructor Documentation

◆ InverseKinematics() [1/4]

InverseKinematics ( const InverseKinematics )
delete

◆ InverseKinematics() [2/4]

◆ ~InverseKinematics()

◆ InverseKinematics() [3/4]

InverseKinematics ( const MultibodyPlant< double > &  plant)
explicit

Constructs an inverse kinematics problem for a MultibodyPlant.

This constructor will create and own a context for

Parameters
plant.
plantThe robot on which the inverse kinematics problem will be solved.
Note
The inverse kinematics problem constructed in this way doesn't permit collision related constraint (such as calling AddMinimumDistanceConstraint). To enable collision related constraint, call InverseKinematics(const MultibodyPlant<double>& plant, systems::Context<double>* plant_context);

◆ InverseKinematics() [4/4]

InverseKinematics ( const MultibodyPlant< double > &  plant,
systems::Context< double > *  plant_context 
)

Constructs an inverse kinematics problem for a MultibodyPlant.

If the user wants to solve the problem with collision related constraint (like calling AddMinimumDistanceConstraint), please use this constructor.

Parameters
plantThe robot on which the inverse kinematics problem will be solved. This plant should have been connected to a SceneGraph within a Diagram
contextThe context for the plant. This context should be a part of the Diagram context. To construct a plant connected to a SceneGraph, with the corresponding plant_context, the steps are // 1. Add a diagram containing the MultibodyPlant and SceneGraph systems::DiagramBuilder<double> builder; auto items = AddMultibodyPlantSceneGraph(&builder); // 2. Add collision geometries to the plant Parser(&(items.plant)).AddModelFromFile("model.sdf"); // 3. Construct the diagram auto diagram = builder.Build(); // 4. Create diagram context. auto diagram_context= diagram->CreateDefaultContext(); // 5. Get the context for the plant. auto plant_context = &(diagram->GetMutableSubsystemContext(items.plant, diagram_context.get()));

Member Function Documentation

◆ AddAngleBetweenVectorsConstraint()

solvers::Binding< solvers::Constraint > AddAngleBetweenVectorsConstraint ( const Frame< double > &  frameA,
const Eigen::Ref< const Eigen::Vector3d > &  na_A,
const Frame< double > &  frameB,
const Eigen::Ref< const Eigen::Vector3d > &  nb_B,
double  angle_lower,
double  angle_upper 
)

Constrains that the angle between a vector na and another vector nb is between [θ_lower, θ_upper].

na is fixed to a frame A, while nb is fixed to a frame B. Mathematically, if we denote na_unit_A as na expressed in frame A after normalization (na_unit_A has unit length), and nb_unit_B as nb expressed in frame B after normalization, the constraint is cos(θ_upper) ≤ na_unit_Aᵀ * R_AB * nb_unit_B ≤ cos(θ_lower), where R_AB is the rotation matrix, representing the orientation of frame B expressed in frame A.

Parameters
frameAThe frame to which na is fixed.
na_AThe vector na fixed to frame A, expressed in frame A.
Precondition
na_A should be a non-zero vector.
Exceptions
std::invalid_argumentif na_A is close to zero.
Parameters
frameBThe frame to which nb is fixed.
nb_BThe vector nb fixed to frame B, expressed in frame B.
Precondition
nb_B should be a non-zero vector.
Exceptions
std::invalid_argumentif nb_B is close to zero.
Parameters
angle_lowerThe lower bound on the angle between na and nb. It is denoted as θ_lower in the documentation. angle_lower is in radians.
Precondition
angle_lower >= 0.
Exceptions
std::invalid_argumentif angle_lower is negative.
Parameters
angle_upperThe upper bound on the angle between na and nb. it is denoted as θ_upper in the class documentation. angle_upper is in radians.
Precondition
angle_lower <= angle_upper <= pi.
Exceptions
std::invalid_argumentif angle_upper is outside the bounds.

◆ AddDistanceConstraint()

solvers::Binding< solvers::Constraint > AddDistanceConstraint ( const SortedPair< geometry::GeometryId > &  geometry_pair,
double  distance_lower,
double  distance_upper 
)

◆ AddGazeTargetConstraint()

solvers::Binding< solvers::Constraint > AddGazeTargetConstraint ( const Frame< double > &  frameA,
const Eigen::Ref< const Eigen::Vector3d > &  p_AS,
const Eigen::Ref< const Eigen::Vector3d > &  n_A,
const Frame< double > &  frameB,
const Eigen::Ref< const Eigen::Vector3d > &  p_BT,
double  cone_half_angle 
)

Constrains a target point T to be within a cone K.

The point T ("T" stands for "target") is fixed in a frame B, with position p_BT. The cone originates from a point S ("S" stands for "source"), fixed in frame A with position p_AS, with the axis of the cone being n, also fixed in frame A. The half angle of the cone is θ. A common usage of this constraint is that a camera should gaze at some target; namely the target falls within a gaze cone, originating from the camera eye.

Parameters
frameAThe frame where the gaze cone is fixed to.
p_ASThe position of the cone source point S, measured and expressed in frame A.
n_AThe directional vector representing the center ray of the cone, expressed in frame A.
Precondition
n_A cannot be a zero vector.
Exceptions
std::invalid_argumentis n_A is close to a zero vector.
Parameters
frameBThe frame where the target point T is fixed to.
p_BTThe position of the target point T, measured and expressed in frame B.
cone_half_angleThe half angle of the cone. We denote it as θ in the documentation. cone_half_angle is in radians.
Precondition
0 <= cone_half_angle <= pi.
Exceptions
std::invalid_argumentif cone_half_angle is outside of the bound.

◆ AddMinimumDistanceConstraint()

solvers::Binding< solvers::Constraint > AddMinimumDistanceConstraint ( double  minimum_distance,
double  influence_distance_offset = 1 
)

Adds the constraint that the pairwise distance between objects should be no smaller than minimum_distance.

We consider the distance between pairs of

  1. Anchored (static) object and a dynamic object.
  2. A dynamic object and another dynamic object, if one is not the parent link of the other.
    Parameters
    minimum_distanceThe minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries.
    influence_distance_offsetThe difference (in meters) between the influence distance, d_influence, and the minimum distance, dₘᵢₙ. This value must be finite and strictly positive, as it is used to scale the signed distances between pairs of geometries. Smaller values may improve performance, as fewer pairs of geometries need to be considered in each constraint evaluation.
    Default: 1 meter
    See also
    MinimumDistanceConstraint for more details on the constraint formulation.
    Precondition
    The MultibodyPlant passed to the constructor of this has registered its geometry with a SceneGraph.
    0 < influence_distance_offset < ∞

◆ AddOrientationConstraint()

solvers::Binding< solvers::Constraint > AddOrientationConstraint ( const Frame< double > &  frameAbar,
const math::RotationMatrix< double > &  R_AbarA,
const Frame< double > &  frameBbar,
const math::RotationMatrix< double > &  R_BbarB,
double  theta_bound 
)

Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound.

Frame A is fixed to frame A_bar, with orientation R_AbarA measured in frame A_bar. Frame B is fixed to frame B_bar, with orientation R_BbarB measured in frame B_bar. The angle difference between frame A's orientation R_WA and B's orientation R_WB is θ, (θ ∈ [0, π]), if there exists a rotation axis a, such that rotating frame A by angle θ about axis a aligns it with frame B. Namely R_AB = I + sinθ â + (1-cosθ)â² (1) where R_AB is the orientation of frame B expressed in frame A. â is the skew symmetric matrix of the rotation axis a. Equation (1) is the Rodrigues formula that computes the rotation matrix from a rotation axis a and an angle θ, https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula If the users want frame A and frame B to align perfectly, they can set θ_bound = 0. Mathematically, this constraint is imposed as trace(R_AB) ≥ 2cos(θ_bound) + 1 (1) To derive (1), using Rodrigues formula R_AB = I + sinθ â + (1-cosθ)â² where trace(R_AB) = 2cos(θ) + 1 ≥ 2cos(θ_bound) + 1

Parameters
frameAbarframe A_bar, the frame A is fixed to frame A_bar.
R_AbarAThe orientation of frame A measured in frame A_bar.
frameBbarframe B_bar, the frame B is fixed to frame B_bar.
R_BbarBThe orientation of frame B measured in frame B_bar.
theta_boundThe bound on the angle difference between frame A's orientation and frame B's orientation. It is denoted as θ_bound in the documentation. theta_bound is in radians.

◆ AddPositionConstraint()

solvers::Binding< solvers::Constraint > AddPositionConstraint ( const Frame< double > &  frameB,
const Eigen::Ref< const Eigen::Vector3d > &  p_BQ,
const Frame< double > &  frameA,
const Eigen::Ref< const Eigen::Vector3d > &  p_AQ_lower,
const Eigen::Ref< const Eigen::Vector3d > &  p_AQ_upper 
)

Adds the kinematic constraint that a point Q, fixed in frame B, should lie within a bounding box expressed in another frame A as p_AQ_lower <= p_AQ <= p_AQ_upper, where p_AQ is the position of point Q measured and expressed in frame A.

Parameters
frameBThe frame in which point Q is fixed.
p_BQThe position of the point Q, rigidly attached to frame B, measured and expressed in frame B.
frameAThe frame in which the bounding box p_AQ_lower <= p_AQ <= p_AQ_upper is expressed.
p_AQ_lowerThe lower bound on the position of point Q, measured and expressed in frame A.
p_AQ_upperThe upper bound on the position of point Q, measured and expressed in frame A.

◆ context()

const systems::Context<double>& context ( ) const

Getter for the plant context.

◆ get_mutable_context()

systems::Context<double>* get_mutable_context ( )

Getter for the mutable plant context.

◆ get_mutable_prog()

solvers::MathematicalProgram* get_mutable_prog ( ) const

Getter for the optimization program constructed by InverseKinematics.

◆ operator=() [1/2]

InverseKinematics& operator= ( const InverseKinematics )
delete

◆ operator=() [2/2]

InverseKinematics& operator= ( InverseKinematics &&  )
delete

◆ prog()

const solvers::MathematicalProgram& prog ( ) const

Getter for the optimization program constructed by InverseKinematics.

◆ q()

Getter for q.

q is the decision variable for the generalized positions of the robot.


The documentation for this class was generated from the following files: