Inverse kinematics

Detailed Description

These algorithms help define configurations of dynamical systems based on inverse kinematics (IK).

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

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, const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &N=std::nullopt, const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &Nplus=std::nullopt)
	Computes a generalized velocity v_next, via the following MathematicalProgram: More...
DifferentialInverseKinematicsResult	DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_WE_desired, const 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 MultibodyPlant< double > &robot, const systems::Context< double > &context, const Vector6< double > &V_AE_desired, const Frame< double > &frame_A, const 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 in frame A. More...
DifferentialInverseKinematicsResult	DoDifferentialInverseKinematics (const MultibodyPlant< double > &robot, const systems::Context< double > &context, const math::RigidTransform< double > &X_WE_desired, const 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 MultibodyPlant< double > &robot, const systems::Context< double > &context, const math::RigidTransform< double > &X_AE_desired, const Frame< double > &frame_A, const Frame< double > &frame_E, const DifferentialInverseKinematicsParameters &parameters)
	A wrapper over DoDifferentialInverseKinematics(robot, context, V_AE_desired, frame_A, frame_E, params) that tracks frame E's pose in frame A. More...

Function Documentation

DoDifferentialInverseKinematics() [1/5]

DifferentialInverseKinematicsResult drake::multibody::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,
		const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &	N = std::nullopt,
		const std::optional< Eigen::Ref< const Eigen::SparseMatrix< double >>> &	Nplus = std::nullopt
	)

Computes a generalized velocity v_next, via the following MathematicalProgram:

min_{v_next,alpha}
  -100 * alpha + |P⋅(v_next - N⁺(q)⋅K⋅(q_nominal - q_current))|²

s.t.
  J⋅v_next = alpha⋅V, // J⋅v_next has the same direction as V
  0 <= alpha <= 1,        // Never go faster than V
  joint_lim_min <= q_current + N⋅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,
  and additional linear velocity constraints,

where:

• The rows of P form an orthonormal basis for the nullspace of J,
• J.rows() == V.size(),
• J.cols() == v_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 spatial velocity is typical, but not required),
• K is the joint_centering_gain,
• the "additional linear velocity constraints" are added via DifferentialInverseKinematicsParameters::AddLinearVelocityConstraint().

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. In the nullspace of this objective, we have a secondary objective to minimize |v_next - N⁺(q)⋅K⋅(q_nominal - q_current)|².

For more details, see https://manipulation.csail.mit.edu/pick.html#diff_ik_w_constraints .

If q_current is a feasible point, then v_next = 0 should always be a feasible solution. If the problem data is bad (q_current is infeasible, and no feasible velocities can restore feasibility in one step), then it is possible that the solver cannot find a solution, in which case, status will be set to kNoSolution in the returned DifferentialInverseKinematicsResult. If the velocity scaling, alpha, is very small, then the status will be set to kStuck.

Parameters

q_current	The current generalized position.
v_current	The current generalized position.
V	Desired spatial velocity. It must have the same number of rows as J.
J	Jacobian with respect to generalized velocities v. It must have the same number of rows as V. J * v needs to represent the same spatial velocity as V.
parameters	Collection of various problem specific constraints and constants.
N	(optional) matrix which maps q̇ = N(q)⋅v. See MultibodyPlant::MakeVelocityToQDotMap(). By default, it is taken to be the identity matrix. If dim(q) != dim(v) and any joint position limits are set in parameters, then you must provide N.
Nplus	(optional) matrix which maps q̇ = N⁺(q)⋅v. See MultibodyPlant::MakeQDotToVelocityMap(). By default, it is taken to be the identity matrix. If dim(q) != dim(v) and J is not full column rank, then you must provide Nplus.

Returns

If the solver successfully finds a solution, joint_velocities will be set to v, otherwise it will be nullopt.

DoDifferentialInverseKinematics() [2/5]

DifferentialInverseKinematicsResult drake::multibody::DoDifferentialInverseKinematics	(	const MultibodyPlant< double > &	robot,
		const systems::Context< double > &	context,
		const Vector6< double > &	V_WE_desired,
		const 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 and J are expressed in E, and only the elements with non-zero gains in parameters get_end_effector_velocity_gains() are used in the program.

Parameters

robot	A MultibodyPlant model.
context	Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.
V_WE_desired	Desired world frame spatial velocity of frame_E.
frame_E	End effector frame.
parameters	Collection 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 drake::multibody::DoDifferentialInverseKinematics	(	const MultibodyPlant< double > &	robot,
		const systems::Context< double > &	context,
		const Vector6< double > &	V_AE_desired,
		const Frame< double > &	frame_A,
		const 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 in frame A.

q_current and v_current are taken from context. V and J are expressed in E, and only the elements with non-zero gains in parameters get_end_effector_velocity_gains() are used in the program.

Parameters

robot	A MultibodyPlant model.
context	Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.
V_AE_desired	Desired spatial velocity of frame_E in frame A.
frame_A	Reference frame (inertial or non-inertial).
frame_E	End effector frame.
parameters	Collection 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 drake::multibody::DoDifferentialInverseKinematics	(	const MultibodyPlant< double > &	robot,
		const systems::Context< double > &	context,
		const math::RigidTransform< double > &	X_WE_desired,
		const 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 context. V_WE_desired is computed by ComputePoseDiffInCommonFrame(X_WE, X_WE_desired) / dt, where X_WE is computed from context, and dt is taken from parameters.

Parameters

robot	A MultibodyPlant model.
context	Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.
X_WE_desired	Desired pose of frame_E in the world frame.
frame_E	End effector frame.
parameters	Collection 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() [5/5]

DifferentialInverseKinematicsResult drake::multibody::DoDifferentialInverseKinematics	(	const MultibodyPlant< double > &	robot,
		const systems::Context< double > &	context,
		const math::RigidTransform< double > &	X_AE_desired,
		const Frame< double > &	frame_A,
		const Frame< double > &	frame_E,
		const DifferentialInverseKinematicsParameters &	parameters
	)

A wrapper over DoDifferentialInverseKinematics(robot, context, V_AE_desired, frame_A, frame_E, params) that tracks frame E's pose in frame A.

q_current and v_current are taken from context. V_AE_desired is computed by ComputePoseDiffInCommonFrame(X_AE, X_AE_desired) / dt, where X_WE is computed from context, and dt is taken from parameters.

Parameters

robot	A MultibodyPlant model.
context	Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.
X_AE_desired	Desired pose of frame_E in frame_A.
frame_A	Reference frame (inertial or non-inertial).
frame_E	End effector frame.
parameters	Collection 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.