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.

To perform IK on a subset of the plant, use the constructor overload that takes a plant_context and use Joint::Lock on the joints in that Context that should be fixed during IK.

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

Public Member Functions
	~InverseKinematics ()

	InverseKinematics (const MultibodyPlant< double > &plant, bool with_joint_limits=true)
	Constructs an inverse kinematics problem for a MultibodyPlant. More...

	InverseKinematics (const MultibodyPlant< double > &plant, systems::Context< double > *plant_context, bool with_joint_limits=true)
	Constructs an inverse kinematics problem for a MultibodyPlant. More...

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

solvers::Binding< solvers::Constraint >	AddPositionConstraint (const Frame< double > &frameB, const Eigen::Ref< const Eigen::Vector3d > &p_BQ, const Frame< double > &frameAbar, const std::optional< math::RigidTransformd > &X_AbarA, 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::Cost >	AddPositionCost (const Frame< double > &frameA, const Eigen::Ref< const Eigen::Vector3d > &p_AP, const Frame< double > &frameB, const Eigen::Ref< const Eigen::Vector3d > &p_BQ, const Eigen::Ref< const Eigen::Matrix3d > &C)
	Adds a cost of the form (p_AP - p_AQ)ᵀ C (p_AP - p_AQ), where point P is specified relative to frame A and point Q is specified relative to frame B, and the cost is evaluated in frame A. More...

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

solvers::Binding< solvers::Cost >	AddOrientationCost (const Frame< double > &frameAbar, const math::RotationMatrix< double > &R_AbarA, const Frame< double > &frameBbar, const math::RotationMatrix< double > &R_BbarB, double c)
	Adds a cost of the form `c * (1 - cos(θ))`, where θ is the angle between the orientation of frame A and the orientation of frame B, and `c` is a cost scaling. More...

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

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

solvers::Binding< solvers::Cost >	AddAngleBetweenVectorsCost (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 c)
	Add a cost c * (1-cosθ) where θ is the angle between the vector `na` and `nb`. More...

solvers::Binding< solvers::Constraint >	AddMinimumDistanceLowerBoundConstraint (double bound, double influence_distance_offset=0.01)
	Adds the constraint that the pairwise distance between objects should be no smaller than `bound`. More...

solvers::Binding< solvers::Constraint >	AddMinimumDistanceUpperBoundConstraint (double bound, double influence_distance_offset)
	Adds the constraint that at least one pair of geometries has distance no larger than `bound`. More...

solvers::Binding< solvers::Constraint >	AddDistanceConstraint (const SortedPair< geometry::GeometryId > &geometry_pair, double distance_lower, double distance_upper)
	Adds the constraint that the distance between a pair of geometries is within some bounds. More...

solvers::Binding< solvers::Constraint >	AddPointToPointDistanceConstraint (const Frame< double > &frame1, const Eigen::Ref< const Eigen::Vector3d > &p_B1P1, const Frame< double > &frame2, const Eigen::Ref< const Eigen::Vector3d > &p_B2P2, double distance_lower, double distance_upper)
	Add a constraint that the distance between point P1 attached to frame 1 and point P2 attached to frame 2 is within the range [distance_lower, distance_upper]. More...

solvers::Binding< solvers::Constraint >	AddPointToLineDistanceConstraint (const Frame< double > &frame_point, const Eigen::Ref< const Eigen::Vector3d > &p_B1P, const Frame< double > &frame_line, const Eigen::Ref< const Eigen::Vector3d > &p_B2Q, const Eigen::Ref< const Eigen::Vector3d > &n_B2, double distance_lower, double distance_upper)
	Add a constraint that the distance between point P attached to frame_point (denoted as B1) and a line attached to frame_line (denoted as B2) is within the range [distance_lower, distance_upper]. More...

solvers::Binding< solvers::Constraint >	AddPolyhedronConstraint (const Frame< double > &frameF, const Frame< double > &frameG, const Eigen::Ref< const Eigen::Matrix3Xd > &p_GP, const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b)
	Adds the constraint that the position of P1, ..., Pn satisfy A * [p_FP1; p_FP2; ...; p_FPn] <= b. More...

const solvers::VectorXDecisionVariable &	q () const
	Getter for q. More...

const solvers::MathematicalProgram &	prog () const
	Getter for the optimization program constructed by InverseKinematics. More...

solvers::MathematicalProgram *	get_mutable_prog ()
	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

InverseKinematics &	operator= (const InverseKinematics &)=delete

	InverseKinematics (InverseKinematics &&)=delete

InverseKinematics &	operator= (InverseKinematics &&)=delete

Constructor & Destructor Documentation

◆ InverseKinematics() [1/4]

InverseKinematics ( const InverseKinematics & )

delete

◆ InverseKinematics() [2/4]

InverseKinematics ( InverseKinematics && )

delete

◆ ~InverseKinematics()

~InverseKinematics ( )

◆ InverseKinematics() [3/4]

InverseKinematics	(	const MultibodyPlant< double > &	plant,
		bool	with_joint_limits = `true`
	)

explicit

Constructs an inverse kinematics problem for a MultibodyPlant.

This constructor will create and own a context for

Parameters

plant.
plant	The robot on which the inverse kinematics problem will be solved.
with_joint_limits	If set to true, then the constructor imposes the joint limit (obtained from plant.GetPositionLowerLimits() and plant.GetPositionUpperLimits(). If set to false, then the constructor does not impose the joint limit constraints in the constructor.

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,
		bool	with_joint_limits = `true`
	)

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

plant	The robot on which the inverse kinematics problem will be solved. This plant should have been connected to a SceneGraph within a Diagram
plant_context	The context for the plant. This context should be a part of the Diagram context. Any locked joints in the `plant_context` will remain fixed at their locked value. (This provides a convenient way to perform IK on a subset of the plant.) 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, 0.0); // 2. Add collision geometries to the plant Parser(&(items.plant)).AddModels("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())); This context will be modified during calling ik.prog.Solve(...). When Solve() returns `result`, context will store the optimized posture, namely plant.GetPositions(*context) will be the same as in result.GetSolution(ik.q()). The user could then use this context to perform kinematic computation (like computing the position of the end-effector etc.).
with_joint_limits	If set to true, then the constructor imposes the joint limit (obtained from plant.GetPositionLowerLimits() and plant.GetPositionUpperLimits(). If set to false, then the constructor does not impose the joint limit constraints in the constructor.

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

frameA	The frame to which na is fixed.
na_A	The vector na fixed to frame A, expressed in frame A.

Precondition: na_A should be a non-zero vector.

Exceptions

std::exception if na_A is close to zero.

Parameters

frameB	The frame to which nb is fixed.
nb_B	The vector nb fixed to frame B, expressed in frame B.

Precondition: nb_B should be a non-zero vector.

Exceptions

std::exception if nb_B is close to zero.

Parameters

angle_lower The 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::exception if angle_lower is negative.

Parameters

angle_upper The 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::exception if angle_upper is outside the bounds.

◆ AddAngleBetweenVectorsCost()

solvers::Binding<solvers::Cost> AddAngleBetweenVectorsCost	(	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	c
	)

Add a cost c * (1-cosθ) where θ is the angle between the vector na and nb.

na is fixed to a frame A, while nb is fixed to a frame B.

Parameters

frameA	The frame to which na is fixed.
na_A	The vector na fixed to frame A, expressed in frame A.

Precondition: na_A should be a non-zero vector.

Exceptions

std::exception if na_A is close to zero.

Parameters

frameB	The frame to which nb is fixed.
nb_B	The vector nb fixed to frame B, expressed in frame B.

Precondition: nb_B should be a non-zero vector.

Exceptions

std::exception if nb_B is close to zero.

Parameters

c	The cost is c * (1-cosθ).

◆ AddDistanceConstraint()

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

Adds the constraint that the distance between a pair of geometries is within some bounds.

Parameters

geometry_pair	The pair of geometries between which the distance is constrained. Notice that we only consider the distance between a static geometry and a dynamic geometry, or a pair of dynamic geometries. We don't allow constraining the distance between two static geometries.
distance_lower	The lower bound on the distance.
distance_upper	The upper bound on the distance.

◆ 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

frameA	The frame where the gaze cone is fixed to.
p_AS	The position of the cone source point S, measured and expressed in frame A.
n_A	The directional vector representing the center ray of the cone, expressed in frame A.

Precondition: n_A cannot be a zero vector.

Exceptions

std::exception is n_A is close to a zero vector.

Parameters

frameB	The frame where the target point T is fixed to.
p_BT	The position of the target point T, measured and expressed in frame B.
cone_half_angle	The 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::exception if cone_half_angle is outside of the bound.

◆ AddMinimumDistanceLowerBoundConstraint()

solvers::Binding<solvers::Constraint> AddMinimumDistanceLowerBoundConstraint	(	double	bound,
		double	influence_distance_offset = `0.01`
	)

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

We consider the distance between pairs of

Anchored (static) object and a dynamic object.

A dynamic object and another dynamic object, if one is not the parent link of the other.

Parameters

bound	The minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries.
influence_distance_offset	See MinimumDistanceLowerBoundConstraint for explanation.

Precondition: The MultibodyPlant passed to the constructor of this has registered its geometry with a SceneGraph.; 0 < influence_distance_offset < ∞

◆ AddMinimumDistanceUpperBoundConstraint()

solvers::Binding<solvers::Constraint> AddMinimumDistanceUpperBoundConstraint	(	double	bound,
		double	influence_distance_offset
	)

Adds the constraint that at least one pair of geometries has distance no larger than bound.

We consider the distance between pairs of

Anchored (static) object and a dynamic object.

A dynamic object and another dynamic object, if one is not the parent link of the other.

Parameters

bound	The upper bound of the minimum signed distance between any candidate pair of geometries. Notice this is NOT the upper bound of every distance, but the upper bound of the smallest distance.
influence_distance_offset	See MinimumDistanceUpperBoundConstraint for more details on influence_distance_offset.

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θ â + (1-cosθ)â² (1) where R_AB is the orientation of frame B expressed in frame A. 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θ â + (1-cosθ)â² where trace(R_AB) = 2cos(θ) + 1 ≥ 2cos(θ_bound) + 1

Parameters

frameAbar	frame A_bar, the frame A is fixed to frame A_bar.
R_AbarA	The orientation of frame A measured in frame A_bar.
frameBbar	frame B_bar, the frame B is fixed to frame B_bar.
R_BbarB	The orientation of frame B measured in frame B_bar.
theta_bound	The 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.

◆ AddOrientationCost()

solvers::Binding<solvers::Cost> AddOrientationCost	(	const Frame< double > &	frameAbar,
		const math::RotationMatrix< double > &	R_AbarA,
		const Frame< double > &	frameBbar,
		const math::RotationMatrix< double > &	R_BbarB,
		double	c
	)

Adds a cost of the form c * (1 - cos(θ)), where θ is the angle between the orientation of frame A and the orientation of frame B, and c is a cost scaling.

Parameters

frameAbar	A frame on the MultibodyPlant.
R_AbarA	The rotation matrix describing the orientation of frame A relative to Abar.
frameBbar	A frame on the MultibodyPlant.
R_BbarB	The rotation matrix describing the orientation of frame B relative to Bbar.
c	A scalar cost weight.

◆ AddPointToLineDistanceConstraint()

solvers::Binding<solvers::Constraint> AddPointToLineDistanceConstraint	(	const Frame< double > &	frame_point,
		const Eigen::Ref< const Eigen::Vector3d > &	p_B1P,
		const Frame< double > &	frame_line,
		const Eigen::Ref< const Eigen::Vector3d > &	p_B2Q,
		const Eigen::Ref< const Eigen::Vector3d > &	n_B2,
		double	distance_lower,
		double	distance_upper
	)

Add a constraint that the distance between point P attached to frame_point (denoted as B1) and a line attached to frame_line (denoted as B2) is within the range [distance_lower, distance_upper].

The line passes through a point Q with a directional vector n.

Parameters

frame_point	The frame to which P is attached.
p_B1P	The position of P measured and expressed in frame_point.
frame_line	The frame to which the line is attached.
p_B2Q	The position of Q measured and expressed in frame_line, the line passes through Q.
n_B2	The direction vector of the line measured and expressed in frame_line.
distance_lower	The lower bound on the distance.
distance_upper	The upper bound on the distance.

◆ AddPointToPointDistanceConstraint()

solvers::Binding<solvers::Constraint> AddPointToPointDistanceConstraint	(	const Frame< double > &	frame1,
		const Eigen::Ref< const Eigen::Vector3d > &	p_B1P1,
		const Frame< double > &	frame2,
		const Eigen::Ref< const Eigen::Vector3d > &	p_B2P2,
		double	distance_lower,
		double	distance_upper
	)

Add a constraint that the distance between point P1 attached to frame 1 and point P2 attached to frame 2 is within the range [distance_lower, distance_upper].

Parameters

frame1	The frame to which P1 is attached.
p_B1P1	The position of P1 measured and expressed in frame 1.
frame2	The frame to which P2 is attached.
p_B2P2	The position of P2 measured and expressed in frame 2.
distance_lower	The lower bound on the distance.
distance_upper	The upper bound on the distance.

◆ AddPolyhedronConstraint()

solvers::Binding<solvers::Constraint> AddPolyhedronConstraint	(	const Frame< double > &	frameF,
		const Frame< double > &	frameG,
		const Eigen::Ref< const Eigen::Matrix3Xd > &	p_GP,
		const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::VectorXd > &	b
	)

Adds the constraint that the position of P1, ..., Pn satisfy A * [p_FP1; p_FP2; ...; p_FPn] <= b.

Parameters

frameF	The frame in which the position P is measured and expressed
frameG	The frame in which the point P is rigidly attached.
p_GP	p_GP.col(i) is the position of the i'th point Pi measured and expressed in frame G.
A	We impose the constraint A * [p_FP1; p_FP2; ...; p_FPn] <= b.

Precondition: A.cols() = 3 * p_GP.cols().

Parameters

b	We impose the constraint A * [p_FP1; p_FP2; ...; p_FPn] <= b.

◆ AddPositionConstraint() [1/2]

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

frameB	The frame in which point Q is fixed.
p_BQ	The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.
frameA	The frame in which the bounding box p_AQ_lower <= p_AQ <= p_AQ_upper is expressed.
p_AQ_lower	The lower bound on the position of point Q, measured and expressed in frame A.
p_AQ_upper	The upper bound on the position of point Q, measured and expressed in frame A.

◆ AddPositionConstraint() [2/2]

solvers::Binding<solvers::Constraint> AddPositionConstraint	(	const Frame< double > &	frameB,
		const Eigen::Ref< const Eigen::Vector3d > &	p_BQ,
		const Frame< double > &	frameAbar,
		const std::optional< math::RigidTransformd > &	X_AbarA,
		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

frameB	The frame in which point Q is fixed.
p_BQ	The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.
frameAbar	We will compute frame A from frame Abar. The bounding box p_AQ_lower <= p_AQ <= p_AQ_upper is expressed in frame A.
X_AbarA	The relative transform between frame Abar and A. If empty, then we use the identity transform.
p_AQ_lower	The lower bound on the position of point Q, measured and expressed in frame A.
p_AQ_upper	The upper bound on the position of point Q, measured and expressed in frame A.

◆ AddPositionCost()

solvers::Binding<solvers::Cost> AddPositionCost	(	const Frame< double > &	frameA,
		const Eigen::Ref< const Eigen::Vector3d > &	p_AP,
		const Frame< double > &	frameB,
		const Eigen::Ref< const Eigen::Vector3d > &	p_BQ,
		const Eigen::Ref< const Eigen::Matrix3d > &	C
	)

Adds a cost of the form (p_AP - p_AQ)ᵀ C (p_AP - p_AQ), where point P is specified relative to frame A and point Q is specified relative to frame B, and the cost is evaluated in frame A.

Parameters

frameA	The frame in which point P's position is measured.
p_AP	The point P.
frameB	The frame in which point Q's position is measured.
p_BQ	The point Q.
C	A 3x3 matrix representing the cost in quadratic form.

◆ 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 ( )

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()

const solvers::VectorXDecisionVariable& q ( ) const

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 file:

drake/multibody/inverse_kinematics/inverse_kinematics.h

Detailed Description

Public Member Functions

Constructor & Destructor Documentation

◆ InverseKinematics() [1/4]

◆ InverseKinematics() [2/4]

◆ ~InverseKinematics()

◆ InverseKinematics() [3/4]

◆ InverseKinematics() [4/4]

Member Function Documentation

◆ AddAngleBetweenVectorsConstraint()

◆ AddAngleBetweenVectorsCost()

◆ AddDistanceConstraint()

◆ AddGazeTargetConstraint()

◆ AddMinimumDistanceLowerBoundConstraint()

◆ AddMinimumDistanceUpperBoundConstraint()

◆ AddOrientationConstraint()

◆ AddOrientationCost()

◆ AddPointToLineDistanceConstraint()

◆ AddPointToPointDistanceConstraint()

◆ AddPolyhedronConstraint()

◆ AddPositionConstraint() [1/2]

◆ AddPositionConstraint() [2/2]

◆ AddPositionCost()

◆ context()

◆ get_mutable_context()

◆ get_mutable_prog()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ prog()

◆ q()