pydrake.multibody.inverse_kinematics

InverseKinematics module

pydrake.multibody.inverse_kinematics.AddMultibodyPlantConstraints(plant: object, q: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram, plant_context: pydrake.systems.framework.Context = None) → list[pydrake.solvers.Binding[Constraint]]

For all kinematic constraints associated with plant adds a corresponding solver::Constraint to prog, using decision variables q to represent the generalized positions of the plant.

Adds joint limits constraints, unit quaternion constraints, and constraints for any locked joints (via Joint::Lock()). Note that you must pass a valid plant_context to use joint locking.

Adds constraints for coupler, distance, ball, and weld constraints. The distance constraint is implemented here as a hard kinematic constraint (i.e., d(q) == d₀), instead of a soft “spring” force.

See also

AddUnitQuaternionConstraintOnPlant() for the unit quaternion constraints.

Precondition:

plant.is_finalized() == true.

Raises:

• RuntimeError if plant has constraints registered that are not –

• yet supported by this method. –

• RuntimeError if prog is nullptr. –

• RuntimeError if plant_context is nullptr and one of the –

• MultibodyPlant constraints requires it. (unit quaternion –

• constraints and coupler constraints do not). –

pydrake.multibody.inverse_kinematics.AddUnitQuaternionConstraintOnPlant(*args, **kwargs)

Overloaded function.

1. AddUnitQuaternionConstraintOnPlant(plant: pydrake.multibody.plant.MultibodyPlant, q_vars: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram) -> list[pydrake.solvers.Binding[Constraint]]

Add unit length constraints to all the variables representing quaternion in q_vars. Namely the quaternions for floating base joints in plant will be enforced to have a unit length, and all quaternion variables will be bounded to be within [-1, 1].

Additionally, if the initial guess for the quaternion variables has not been set (it is nan), then this method calls MathematicalProgram::SetInitialGuess() with [1, 0, 0, 0], to help the solver avoid singularities.

Parameter plant:

The plant on which we impose the unit quaternion constraints.

Parameter q_vars:

The decision variables for the generalized position of the plant.

Parameter prog:

The unit quaternion constraints are added to this prog.

2. AddUnitQuaternionConstraintOnPlant(plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], q_vars: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram) -> list[pydrake.solvers.Binding[Constraint]]

Add unit length constraints to all the variables representing quaternion in q_vars. Namely the quaternions for floating base joints in plant will be enforced to have a unit length, and all quaternion variables will be bounded to be within [-1, 1].

Additionally, if the initial guess for the quaternion variables has not been set (it is nan), then this method calls MathematicalProgram::SetInitialGuess() with [1, 0, 0, 0], to help the solver avoid singularities.

Parameter plant:

The plant on which we impose the unit quaternion constraints.

Parameter q_vars:

The decision variables for the generalized position of the plant.

Parameter prog:

The unit quaternion constraints are added to this prog.

class pydrake.multibody.inverse_kinematics.AngleBetweenVectorsConstraint

Bases: pydrake.solvers.Constraint

Constrains that the angle between a vector a and another vector b is between [θ_lower, θ_upper]. a is fixed to a frame A, while b is fixed to a frame B. Mathematically, if we denote a_unit_A as a expressed in frame A after normalization (a_unit_A has unit length), and b_unit_B as b expressed in frame B after normalization, the constraint is cos(θ_upper) ≤ a_unit_Aᵀ * R_AB * b_unit_B ≤ cos(θ_lower)

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.AngleBetweenVectorsConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameA: pydrake.multibody.tree.Frame, a_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, b_B: numpy.ndarray[numpy.float64[3, 1]], angle_lower: float, angle_upper: float, plant_context: pydrake.systems.framework.Context) -> None

Constructs an AngleBetweenVectorsConstraint.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameA:

The Frame object for frame A.

Parameter a_A:

The vector a fixed to frame A, expressed in frame A.

Parameter frameB:

The Frame object for frame B.

Parameter b_B:

The vector b fixed to frame B, expressed in frameB.

Parameter angle_lower:

The lower bound on the angle between a and b. It is denoted as θ_lower in the class documentation.

Parameter angle_upper:

The upper bound on the angle between a and b. it is denoted as θ_upper in the class documentation.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Precondition:

frameA and frameB must belong to plant.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if a_A is close to zero. –

• RuntimeError if b_B is close to zero. –

• RuntimeError if angle_lower is negative. –

• RuntimeError if angle_upper ∉ [angle_lower, π]. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.AngleBetweenVectorsConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], a_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], b_B: numpy.ndarray[numpy.float64[3, 1]], angle_lower: float, angle_upper: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Use MultibodyPlant<AutoDiffXd> instead of MultibodyPlant<double>.

class pydrake.multibody.inverse_kinematics.AngleBetweenVectorsCost

Bases: pydrake.solvers.Cost

Implements a cost of the form c*(1-cosθ), where θ is the angle between two vectors a and b. c is a constant scalar.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.AngleBetweenVectorsCost, plant: pydrake.multibody.plant.MultibodyPlant, frameA: pydrake.multibody.tree.Frame, a_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, b_B: numpy.ndarray[numpy.float64[3, 1]], c: float, plant_context: pydrake.systems.framework.Context) -> None

Constructs an AngleBetweenVectorsCost.

Parameter plant:

The MultibodyPlant on which the cost is imposed. plant should be alive during the lifetime of this cost.

Parameter frameA:

The Frame object for frame A.

Parameter a_A:

The vector a fixed to frame A, expressed in frame A.

Parameter frameB:

The Frame object for frame B.

Parameter b_B:

The vector b fixed to frame B, expressed in frameB.

Parameter c:

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

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this cost.

Precondition:

frameA and frameB must belong to plant.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if a_A is close to zero. –

• RuntimeError if b_B is close to zero. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.AngleBetweenVectorsCost, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], a_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], b_B: numpy.ndarray[numpy.float64[3, 1]], c: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Use MultibodyPlant<AutoDiffXd> instead of MultibodyPlant<double>.

class pydrake.multibody.inverse_kinematics.ComInPolyhedronConstraint

Bases: pydrake.solvers.Constraint

Constrains the center of mass to lie within a polyhedron lb <= A * p_EC <= ub where p_EC is the position of the center-of-mass (C) expressed in a frame E.

For example, if you set A as identity matrix, then this constraint enforces a box-region on the CoM position p_EC. If you set the expressed frame E as the robot foot frame, and choose A to describe the foot support polygon, this constraint could enforce the projection of CoM to be within the foot support polygon, which is commonly used to ensure static equilibrium.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.ComInPolyhedronConstraint, plant: pydrake.multibody.plant.MultibodyPlant, model_instances: Optional[list[pydrake.multibody.tree.ModelInstanceIndex]], expressed_frame: pydrake.multibody.tree.Frame, A: numpy.ndarray[numpy.float64[m, 3], flags.f_contiguous], lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], plant_context: pydrake.systems.framework.Context) -> None

Constructs a ComInPolyhedronConstraint object.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant must be alive during the lifetime of this constraint.

Parameter model_instances:

The CoM of these model instances are computed. If model_instances = std::nullopt, then we compute the CoM of all model instances (except the world).

Parameter expressed_frame:

The frame in which the CoM is expressed.

Parameter A:

The CoM position p_EC satisfies lb <= A * p_EC <= ub

Parameter lb:

The CoM position p_EC satisfies lb <= A * p_EC <= ub

Parameter ub:

The CoM position p_EC satisfies lb <= A * p_EC <= ub

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context must be alive during the lifetime of this constraint.

2. __init__(self: pydrake.multibody.inverse_kinematics.ComInPolyhedronConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], model_instances: Optional[list[pydrake.multibody.tree.ModelInstanceIndex]], expressed_frame: pydrake.multibody.tree.Frame_[AutoDiffXd], A: numpy.ndarray[numpy.float64[m, 3], flags.f_contiguous], lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>. Except the gradient of the constraint is computed from autodiff.

Precondition:

if model_instances is not std::nullopt, then all indices in model_instances refer to valid model instances in plant.

class pydrake.multibody.inverse_kinematics.ComPositionConstraint

Bases: pydrake.solvers.Constraint

Impose the constraint p_EScm(q) - p_EC = 0, where p_EScm(q) is a function that computes the center-of-mass (COM) position from robot generalized position q, expressed in a frame E. p_EC ∈ ℝ³ is the variable representing robot CoM (C) position expressed in frame E. The evaluated variables are [q;r], where q is the generalized position vector of the entire plant.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.ComPositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant, model_instances: Optional[list[pydrake.multibody.tree.ModelInstanceIndex]], expressed_frame: pydrake.multibody.tree.Frame, plant_context: pydrake.systems.framework.Context) -> None

Constructor, constrain f(q) = p_EC, where f(q) evaluates the CoM position expressed in frame E using the generalized position q.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter model_instances:

We compute the model with these model instances in plant. If model_instances=std::nullopt, then we compute the CoM position of all model instances except the world.

Parameter expressed_frame:

The frame in which the CoM position is expressed.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Raises:

RuntimeError if plant or plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.ComPositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], model_instances: Optional[list[pydrake.multibody.tree.ModelInstanceIndex]], expressed_frame: pydrake.multibody.tree.Frame_[AutoDiffXd], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor with MultibodyPlant<AutoDiffXd> and Context<AutoDiffXd>. It is preferable to use the constructor with MBP<double> and Context<double>. But if you only have MBP<AutoDiffXd> and Context<AutoDiffXd>, then use this constructor.

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsController

Bases: pydrake.systems.framework.Diagram

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsController, differential_inverse_kinematics: object, planar_rotation_dof_indices: list[int]) → None

Constructs a DifferentialInverseKinematicsController.

Parameter differential_inverse_kinematics:

a DifferentialInverseKinematicsSystem leaf system.

Parameter planar_rotation_dof_indices:

the indices of commanded position output that will be wrapped to ±π.

Warning

a DifferentialInverseKinematicsSystem leaf system may only be added to at most one controller. Multiple controller instances cannot share the same leaf system.

differential_inverse_kinematics(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsController) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem

get_mutable_differential_inverse_kinematics(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsController) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem

set_initial_position(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsController, context: pydrake.systems.framework.Context, value: numpy.ndarray[numpy.float64[m, 1]]) → None

Sets the integral part of the DiscreteTimeIntegrator to value. value has the dimension of the full plant.num_positions(); non-active dofs will be ignored. This in effect sets the initial position that is fed to the DifferentialInverseKinematicsSystem leaf system.

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator

Bases: pydrake.systems.framework.LeafSystem

A LeafSystem that integrates successive calls to DoDifferentialInverseKinematics (which produces joint velocity commands) to produce joint position commands.

In each evaluation, DoDifferentialInverseKinematics uses a linearization of the robot kinematics around a nominal position. The nominal position is obtained by either: 1. If the optional boolean (abstract-)valued input port use_robot_state is connected and set to True, then differential IK is computed using the robot_state input port (which must also be connected). Note: Using measured joint positions in a feedback loop can lead to undamped oscillations in the redundant joints; we hope to resolve this and are tracking it in #9773. 2. Otherwise, differential IK is computed using this System’s internal state, representing the current joint position command. This is equivalent to integrating (open loop) the velocity commands obtained from the differential IK solutions.

It is also important to set the initial state of the integrator: 1. If the robot_state port is connected, then the initial state of the integrator is set to match the positions from this port (the port accepts the state vector with positions and velocities for easy of use with MultibodyPlant, but only the positions are used). 2. Otherwise, it is highly recommended that the user call SetPositions() to initialize the integrator state.

X_AE_desired→ robot_state (optional)→ use_robot_state (optional)→

DifferentialInverseKinematicsIntegrator

→ joint_positions

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator, robot: pydrake.multibody.plant.MultibodyPlant, frame_A: pydrake.multibody.tree.Frame, frame_E: pydrake.multibody.tree.Frame, time_step: float, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, robot_context: pydrake.systems.framework.Context = None, log_only_when_result_state_changes: bool = True) -> None

Constructs the system.

Parameter robot:

A MultibodyPlant describing the robot.

Parameter frame_A:

Reference frame (inertial or non-inertial).

Parameter frame_E:

End-effector frame.

Parameter time_step:

the discrete time step of the (Euler) integration.

Parameter parameters:

Collection of various problem specific constraints and constants. The time_step parameter will be set to time_step.

Parameter robot_context:

Optional Context of the MultibodyPlant. The position values of this context will be overwritten during integration; you only need to pass this in if the robot has any non-default parameters. $*Default:* robot.CreateDefaultContext().

Parameter log_only_when_result_state_changes:

is a boolean that determines whether the system will log on every differential IK failure, or only when the failure state changes. When the value is True, it will cause the system to have an additional discrete state variable to store the most recent DifferentialInverseKinematicsStatus. Set this to False if you want IsDifferenceEquationSystem() to return True.

Note: All references must remain valid for the lifetime of this system.

Precondition:

frame_E != frame_A.

2. __init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator, robot: pydrake.multibody.plant.MultibodyPlant, frame_E: pydrake.multibody.tree.Frame, time_step: float, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, robot_context: pydrake.systems.framework.Context = None, log_only_when_result_state_changes: bool = True) -> None

Constructs the system.

Parameter robot:

A MultibodyPlant describing the robot.

Parameter frame_E:

End-effector frame.

Parameter time_step:

the discrete time step of the (Euler) integration.

Parameter parameters:

Collection of various problem specific constraints and constants. The time_step parameter will be set to time_step.

Parameter robot_context:

Optional Context of the MultibodyPlant. The position values of this context will be overwritten during integration; you only need to pass this in if the robot has any non-default parameters. $*Default:* robot.CreateDefaultContext().

Parameter log_only_when_result_state_changes:

is a boolean that determines whether the system will log on every differential IK failure, or only when the failure state changes. When the value is True, it will cause the system to have an additional discrete state variable to store the most recent DifferentialInverseKinematicsStatus. Set this to False if you want IsDifferenceEquationSystem() to return True.

In this overload, the reference frame, A, is taken to be the world frame.

Note: All references must remain valid for the lifetime of this system.

Precondition:

frame_E != robot.world_frame().

ForwardKinematics(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator, context: pydrake.systems.framework.Context) → pydrake.math.RigidTransform

Provides X_AE as a function of the joint position set in context.

get_mutable_parameters(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters

Returns a mutable reference to the differential IK parameters owned by this system.

get_parameters(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters

Returns a const reference to the differential IK parameters owned by this system.

SetPositions(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsIntegrator, context: pydrake.systems.framework.Context, positions: numpy.ndarray[numpy.float64[m, 1]]) → None

Sets the joint positions, which are stored as state in the context. It is recommended that the user calls this method to initialize the position commands to match the initial positions of the robot.

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters

Contains parameters for the family of differential inverse kinematics function overloads below, each named DoDifferentialInverseKinematics().

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, num_positions: int, num_velocities: int = None) → None

Constructor. Initializes the nominal joint position to zeros of size num_positions. The time step is initialized to 1. The end effector flags are initialized to True. The joint centering gains are initialized to zero. All constraints are initialized to nullopt.

Parameter num_positions:

Number of generalized positions.

Parameter num_velocities:

Number of generalized velocities (by default it will be set to num_positions).

get_end_effector_angular_speed_limit(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → float

get_end_effector_translational_velocity_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → tuple[numpy.ndarray[numpy.float64[3, 1]], numpy.ndarray[numpy.float64[3, 1]]] | None

get_end_effector_velocity_flag(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → numpy.ndarray[bool[6, 1]]

get_joint_acceleration_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]] | None

get_joint_centering_gain(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → numpy.ndarray[numpy.float64[m, n]]

get_joint_position_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]] | None

get_joint_velocity_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]] | None

get_maximum_scaling_to_report_stuck(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → float

get_mutable_solver_options(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → pydrake.solvers.SolverOptions

Provides mutable access to change the solver options, e.g., to tune for speed vs accuracy.

get_nominal_joint_position(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → numpy.ndarray[numpy.float64[m, 1]]

get_num_positions(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → int

get_num_velocities(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → int

get_time_step(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) → float

@name Getters.

set_end_effector_angular_speed_limit(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, speed: float) → None

When calling DoDifferentialInverseKinematics with a desired end-effector pose, this limits the magnitude of the angular velocity vector.

set_end_effector_translational_velocity_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, lower: numpy.ndarray[numpy.float64[3, 1]], upper: numpy.ndarray[numpy.float64[3, 1]]) → None

When calling DoDifferentialInverseKinematics with a desired end-effector pose, this sets limits on the translational velocity.

set_end_effector_velocity_flag(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, flag_E: numpy.ndarray[bool[6, 1]]) → None

Sets the end effector flags in the body frame. If a spatial velocity flag is set to false, it will not be included in the differential IK formulation.

set_joint_acceleration_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, arg0: tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]) → None

Sets the joint acceleration limits.

Parameter vd_bounds:

The first element is the lower bound, and the second is the upper bound.

Raises:

• RuntimeError if the first or second element of q_bounds has –

• the wrong dimension or any element of the second element is –

• smaller than its corresponding part in the first element. –

set_joint_centering_gain(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, K: numpy.ndarray[numpy.float64[m, n]]) → None

Sets the joint centering gain, K, so that the joint centering command is attempting to achieve v_next = N⁺(q) * K * (q_nominal - q_current).

Precondition:

K must be num_positions x num_positions.

set_joint_position_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, arg0: tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]) → None

Sets the joint position limits.

Parameter q_bounds:

The first element is the lower bound, and the second is the upper bound.

Raises:

• RuntimeError if the first or second element of q_bounds has –

• the wrong dimension or any element of the second element is –

• smaller than its corresponding part in the first element. –

set_joint_velocity_limits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, arg0: tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]) → None

Sets the joint velocity limits.

Parameter v_bounds:

The first element is the lower bound, and the second is the upper bound.

Raises:

• RuntimeError if the first or second element of q_bounds has –

• the wrong dimension or any element of the second element is –

• smaller than its corresponding part in the first element. –

set_maximum_scaling_to_report_stuck(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, scaling: float) → None

Sets the threshold for α below which the status returned is DifferentialInverseKinematicsStatus::kStuck. α is the scaling of the commanded spatial velocity, so when α is small, it means that the actual spatial velocity magnitude will be small proportional to the commanded.

Default: 0.01.

set_nominal_joint_position(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, arg0: numpy.ndarray[numpy.float64[m, 1]]) → None

Sets the nominal joint position.

Raises:

RuntimeError if nominal_joint_position's dimension differs. –

set_time_step(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, dt: float) → None

@name Setters. Sets time step to dt.

Raises:

RuntimeError if dt <= 0. –

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult, **kwargs) → None

property joint_velocities

property status

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsStatus

Members:

kSolutionFound : Found the optimal solution.

kNoSolutionFound : Solver unable to find a solution.

kStuck : Unable to follow the desired velocity direction

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsStatus, value: int) → None

kNoSolutionFound = <DifferentialInverseKinematicsStatus.kNoSolutionFound: 1>

kSolutionFound = <DifferentialInverseKinematicsStatus.kSolutionFound: 0>

kStuck = <DifferentialInverseKinematicsStatus.kStuck: 2>

property name

property value

class pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem

Bases: pydrake.systems.framework.LeafSystem

The DifferentialInverseKinematicsSystem takes as input desired cartesian poses (or cartesian velocities) for an arbitrary number of “goal” frames on the robot, and produces a generalized velocity command as output to move the goal frames toward the desired state. This system is stateless, but is intended to be clocked at a known, fixed time step Δt by evaluating its output port at integer multiples of Δt.

The velocity command is computed by solving a MathematicalProgram optimization problem, consisting of: - one primary objective (LeastSquaresCost), - typically a secondary objective (JointCenteringCost) to resolve the nullspace within the primary objective, and - various optional constraints: - CartesianPositionLimitConstraint - CartesianVelocityLimitConstraint - CollisionConstraint - JointVelocityLimitConstraint

In brief, we solve for v_next such that Jv_TGs * v_next is close to Vd_TGs subject to the constraints, where: - v_next is the generalized velocity command on the output port, which has the dimension of the number of active degrees of freedom, - Vd_TGs is the desired spatial velocities of the goal frames (when desired positions are input, the desired velocity is inferred using the difference from the current position vs the time step), - Jv_TGs is the jacobian relating spatial velocities to generalized velocities, i.e., V_TGs (rows) with respect to v_active (cols) – v_active is the subset of generalized velocities for the active degrees of freedom.

For an introduction to differential inverse kinematics via optimization, see section 10.6 of: https://manipulation.csail.mit.edu/pick.html#diff_ik_w_constraints

position→ nominal_posture→ desired_cartesian_velocities (optional)→ desired_cartesian_poses (optional)→

DifferentialInverseKinematicsSystem

→ commanded_velocity

Port position accepts the current generalized position (for the full plant, not just the active dofs).

Port desired_cartesian_velocities accepts desired cartesian velocities, typed as systems::BusValue where key is the name of the frame to track and the value is the SpatialVelocity<double> w.r.t the task frame. Frame names should be provided as fully-scoped names (model_instance::frame).

Port desired_cartesian_poses accepts desired cartesian poses, typed as systems::BusValue where key is the name of the frame to track and the value is the math::RigidTransformd spatial pose w.r.t the task frame. Frame names should be provided as fully-scoped names (model_instance::frame).

Port nominal_posture accepts a generalized position to be used to handle nullspace resolution; this has the dimension of the full degrees of freedom (not just active dofs). Typical choices for setting this input would be to use a constant “home” reference posture, or to use a dynamically changing recent posture.

Port commanded_velocity emits generalized velocity that realizes the desired cartesian velocities or poses within the constraints. This has the dimension of the number of active degrees of freedom.

Either desired_cartesian_velocities or desired_cartesian_poses must be connected. Connecting both ports will result in an exception.

Note

There is no consistency check to ensure that the frames being tracked are the same across multiple time steps.

Warning

This class only works correctly when the plant has v = q̇.

= Notation =

The implementation uses “monogram notation” abbreviations throughout. See https://drake.mit.edu/doxygen_cxx/group__multibody__quantities.html for details. The relevant frame names are: - B: base frame - Gi: the i’th goal frame (per the desired_cartesian_… input port) - T: task frame - W: world frame

To denote desired spatial velocities, we use a “d” suffix (i.e., “Vd”). Quantities like Vd_TGi (and therefore also Vd_TGlist and Vd_TGs) refer to the desired velocity, not the current velocity. Other quantities without the “d” subscript (e.g., X_TGi, Jv_TGi, etc.) refer to the current kinematics.

When ‘s’ is used as a suffix (e.g., ‘Vd_TGs’), it refers to the stack of all goals one after another, e.g., Vd_TGs refers to the concatenation of all Vd_TGi in Vd_TGlist. You can think of the ‘s’ either as an abbreviation for “stack” or as a plural.

We also use the letter ‘S’ to refer a “multibody system”, in our case the robot portion of the controller plant (not including the environment), as defined by the collision_checker. For example, use the notation ‘Scm’ to denote the robot’s center of mass.

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem, recipe: object, task_frame: str, collision_checker: object, active_dof: pydrake.planning.DofMask, time_step: float, K_VX: float, Vd_TG_limit: pydrake.multibody.math.SpatialVelocity) → None

(Advanced) Constructs the DifferentialInverseKinematicsSystem with a user-provided recipe for the mathematical program formulation.

Parameter recipe:

Specifies the mathematical formation and its parameters.

Parameter task_frame:

Specifies the task frame name (i.e., “T”) that cartesian goal poses are expressed in. Frame names should be provided as fully-scoped names (model_instance::frame). This must be a frame in collision_checker.plant().

Parameter collision_checker:

Specifies the plant and collision model to use.

Parameter active_dof:

Specifies which generalized velocities of the plant are commanded by this system and appear on the “commanded_velocity” output port.

Parameter time_step:

Specifies Δt, the time horizon assumed when relating position values to velocity values. The output commands from this system should be sent to the robot at this period.

Parameter K_VX:

Specifies a scale factor used to convert desired_cartesian_poses input to velocities. The i’th desired cartesian velocity is computed as: Vd_TGi = K_VX * (X_TGi_requested - X_TGi_current) / Δt A typical value is 1.0 so that the desired velocity exactly matches what’s necessary to move to the request pose in the next step, but may be tuned smaller (e.g., 0.5) to mitigate overshoot. This value is not used when the desired_cartesian_velocities input port is being used for commands.

Parameter Vd_TG_limit:

A clamping limit applied to desired cartesian velocities. All desired cartesian velocities (whether specified explicitly on the desired_cartesian_velocities input port, or inferred from the desired_cartesian_poses input port) are “clamped”; the absolute value of each measure in the clamped spatial velocity will be no greater than the corresponding measure in Vd_TG_limit. This clamped velocity becomes the input to the optimization problem. (NOTE: Clamping for desired_cartesian_velocities is not yet implemented, but will be soon.) If the CartesianVelocityLimitConstraint is in use, typically this value should be the same as that ingredient’s V_next_TG_limit.

active_dof(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.planning.DofMask

Gets the mask of active DOFs in plant() that are being controlled.

class CartesianPositionLimitConstraint

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Constrains the goal frames to a cartesian bounding box: ∀i p_TG_next_lower ≤ p_TGi + Jv_TGi[3:6] * v_next * Δt ≤ p_TG_next_upper where: - p_TGi is the translation component of the i’th goal point w.r.t the task frame.

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint.Config) → None

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint.Config, **kwargs) → None

property p_TG_next_lower

Lower bound on p_TGi for all i.

property p_TG_next_upper

Upper bound on p_TGi for all i.

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint.Config

Returns the current config.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianPositionLimitConstraint.Config) → None

Replaces the config set in the constructor.

class CartesianVelocityLimitConstraint

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Constrains the spatial velocities of the goal frames: ∀i, ∀j ∈ [0, 5]: abs(Jv_TGi * v_next)[j] ≤ V_next_TG_limit[j].

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint.Config) → None

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint.Config, **kwargs) → None

property V_next_TG_limit

This limits the absolute value of the measures of the spatial velocities V_next[i] for all goal frames, so must be entry-wise non-negative. If the “desired_cartesian_poses” input port to DifferentialInverseKinematicsSystem is in use, then typically this should be set to the same value as the Vd_TG_limit passed to that system’s constructor. The element order is [ωx, ωy, ωz, vx, vy, vz], which matches the SpatialVelocity order.

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint.Config

Returns the current config.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CartesianVelocityLimitConstraint.Config) → None

Replaces the config set in the constructor.

collision_checker(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.planning.CollisionChecker

Gets the collision checker used by the controller.

class CollisionConstraint

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Constrains the collision clearance around the robot to remain above the safety distance: ∀j ϕₛ ≤ ϕⱼ + ∂ϕⱼ/∂q_active * v_next * Δt where: - ϕₛ is the safety_distance; - ϕⱼ is the current signed distance between the robot and some j’th obstacle; - ∂ϕⱼ/∂q_active is the gradient of ϕⱼ with respect to the active dof positions. Obstacles beyond the influence distance are ignored. The optional select_data_for_collision_constraint may be used to preprocess the clearance data prior to adding the constraint (e.g., to ignore some parts).

TODO(jeremy-nimmer) This constraint should account for passive dof velocities as well (i.e., v_passive), using the full ∂q (not just ∂q_active).

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint.Config) → None

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint.Config, **kwargs) → None

property influence_distance

Obstacles beyond the influence distance (in meters) are ignored. Must be non-negative.

property safety_distance

“ϕₛ” in the class overview (in meters). Must be finite.

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint.Config

Returns the current config.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint.Config) → None

Replaces the config set in the constructor.

SetSelectDataForCollisionConstraintFunction(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.CollisionConstraint, select_data_for_collision_constraint: Callable[[pydrake.planning.DofMask, pydrake.planning.RobotClearance, numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n]]], None]) → None

(Advanced) Provides a mechanism for ignoring certain clearance rows.

get_input_port_desired_cartesian_poses(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.systems.framework.InputPort

Returns the input port for the desired cartesian poses (of type systems::BusValue containing math::RigidTransformd).

get_input_port_desired_cartesian_velocities(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.systems.framework.InputPort

Returns the input port for the desired cartesian velocities (of type systems::BusValue containing SpatialVelocity).

get_input_port_nominal_posture(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.systems.framework.InputPort

Returns the input port for the nominal joint positions to be used to handle nullspace resolution. This has the dimension of the full plant.num_positions(); non-active dofs will be ignored.

get_input_port_position(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.systems.framework.InputPort

Returns the input port for the joint positions.

get_output_port_commanded_velocity(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.systems.framework.OutputPort

Returns the output port for the generalized velocity command that realizes the desired poses within the constraints. The size is equal to get_active_dof().count().

class Ingredient

(Internal use only) A user-provided set of constraint(s) and/or cost(s) for a DifferentialInverseKinematicsSystem recipe, to allow for user customization of the mathematical program formulation.

TODO(jeremy-nimmer) In the future, we should remove “internal use only” so that users can officially bake their own recipes for DifferentialInverseKinematicsSystem.

__init__(*args, **kwargs)

class JointCenteringCost

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Provides a secondary minimization objective. There will almost inevitably be times when the jacobian Jv_TGs is not full rank. Sometimes because of current position values and sometimes because Jv_TGs is rectangular by construction. At those times, there’s no longer a single solution and relying on the mathematical program to pick a reasonable solution from the large space is unreasonable.

This cost is intended to work in conjunction with the primary cost (e.g., LeastSquaresCost). When Jv_TGs is not full rank, this provides guidance for selecting a unique velocity from the space of possible solutions. If the primary cost produces a space of optimal velocities, this secondary cost will select the velocity from that space that brings the arm closer to the “nominal posture”:

|P⋅(v_next - K⋅(nominal_posture_active - q_active))|²

where, - K is the proportional gain of a joint-space controller that pulls toward the nominal posture. - P is the linear map from generalized velocities to the null space of masked Jacobian S ⋅ Jv_TGs. Mapping to the null space, allows refinement of the velocity without changing the primary cost. - S is a block-diagonal matrix of axis masks, enabling per-axis tracking.

Notes: - When combined with a primary cost, the primary cost should be weighed far more heavily to keep this secondary cost from interfering. A factor of 100X or so is advisable. (See LeastSqauresCost::cartesian_qp_weight for tuning the gain on the primary objective.) - For this cost to behave as expected, it is critical that the null space of S ⋅ Jv_TGs be a subspace of the null space used by the primary cost. The simplest way is to make sure both costs receive the same axis masks and jacobian. Failure to do so would lead this cost to fight the primary cost instead of complementing it.

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

TODO(sean.curtis) As with the other costs, this should also have a scaling weight (e.g. what G is for LeastSquaresCost).

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost.Config) → None

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost.Config, **kwargs) → None

property cartesian_axis_masks

Map from fully-scoped frame names to their 6D spatial velocity axis mask. Each Vector6d is a binary mask [ωx, ωy, ωz, vx, vy, vz] indicating which Cartesian velocity components (angular and translational) are being tracked. All elements must be set to either 0 or 1, and at least one element must be 1.

property posture_gain

The proportional gain matrix K is a diagonal matrix with the diagonal set to this value (i.e., all joints use the same posture_gain). Must be non-negative.

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost.Config

Returns the current config.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointCenteringCost.Config) → None

Replaces the config set in the constructor.

class JointVelocityLimitConstraint

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Constrains the generalized velocity to prevent a commanded velocity that would push the generalized position outside its limits.

The configured joint velocity limits are not used directly. When the joint position is near a limit, the joint velocity limit may be insufficient to constrain v_next from pushing the position beyond its limits.

Instead, we use a scaled velocity limit. In simple terms, the closer q is to its position limit, the more velocity towards that limit is constrained. When q lies at the limit boundary, the velocity in that direction would be zero. However, the configured velocity limit should be applied when q is “sufficiently” far away from the position limit boundary. So, we compute and apply a scale factor to attenuate the “near” velocity limit.

The constraint is parameterized to define the domain of the attenuation; how far away from the near limit boundary should the scaled limit be zero and how far should it be restored to its configured value? We parameterize those two distances as:

• min_margin: the distance at which the velocity limit is scaled to be zero.

Must be non-negative and is typically positive. - influence_margin: the distance at which the velocity limit is restored to its configured value. This value must be strictly greater than min_margin.

The velocity limit is only ever attenuated on one boundary.

If we define distᵢ as the distance to the near position limit boundary, and near_limit as the value of that limit, we can define the attenuation as:

Click to expand C++ code...

distᵢ = min(qᵢ_current - qᵢ_min, qᵢ_max - qᵢ_current)
scale = clamp((distᵢ - min_margin) / (influence_margin - min_margin), 0, 1)
scaled_near_limit = near_limit * scale.

Implications: - Because we’re attenuating the configured velocity limits, an infinite limit value won’t be attenuated. It will be zero when distᵢ ≤ min_margin. - Position limits define an interval for q, which we’ll call P. If P / 2 - min_margin < influence_margin - min_margin then one limit will always be attenuated to be less than its configured value.

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint.Config, joint_limits: pydrake.planning.JointLimits) → None

(Advanced) Constructs the DifferentialInverseKinematicsSystem with a user-provided recipe for the mathematical program formulation.

Parameter recipe:

Specifies the mathematical formation and its parameters.

Parameter task_frame:

Specifies the task frame name (i.e., “T”) that cartesian goal poses are expressed in. Frame names should be provided as fully-scoped names (model_instance::frame). This must be a frame in collision_checker.plant().

Parameter collision_checker:

Specifies the plant and collision model to use.

Parameter active_dof:

Specifies which generalized velocities of the plant are commanded by this system and appear on the “commanded_velocity” output port.

Parameter time_step:

Specifies Δt, the time horizon assumed when relating position values to velocity values. The output commands from this system should be sent to the robot at this period.

Parameter K_VX:

Specifies a scale factor used to convert desired_cartesian_poses input to velocities. The i’th desired cartesian velocity is computed as: Vd_TGi = K_VX * (X_TGi_requested - X_TGi_current) / Δt A typical value is 1.0 so that the desired velocity exactly matches what’s necessary to move to the request pose in the next step, but may be tuned smaller (e.g., 0.5) to mitigate overshoot. This value is not used when the desired_cartesian_velocities input port is being used for commands.

Parameter Vd_TG_limit:

A clamping limit applied to desired cartesian velocities. All desired cartesian velocities (whether specified explicitly on the desired_cartesian_velocities input port, or inferred from the desired_cartesian_poses input port) are “clamped”; the absolute value of each measure in the clamped spatial velocity will be no greater than the corresponding measure in Vd_TG_limit. This clamped velocity becomes the input to the optimization problem. (NOTE: Clamping for desired_cartesian_velocities is not yet implemented, but will be soon.) If the CartesianVelocityLimitConstraint is in use, typically this value should be the same as that ingredient’s V_next_TG_limit.

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint.Config, **kwargs) → None

property influence_margin

The distance (in each joint’s configuration space) at which the velocity limit is restored to its configured value. The units will depend on what kind of joint is being limited. This value must be strictly greater than min_margin.

property min_margin

The distance (in each joint’s configuration space) at which the velocity limit is scaled to be zero. Must be non-negative and is typically positive. The units will depend on what kind of joint is being limited.

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint.Config

Returns the current config.

GetJointLimits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint) → pydrake.planning.JointLimits

Returns the current joint limits.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint.Config) → None

Replaces the config set in the constructor.

SetJointLimits(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.JointVelocityLimitConstraint, joint_limits: pydrake.planning.JointLimits) → None

Replaces the limits set in the constructor.

K_VX(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → float

Gets the gain factor used to convert desired cartesian poses to velocities.

class LeastSquaresCost

Bases: pydrake.multibody.inverse_kinematics.Ingredient

Provides a primary DifferentialInverseKinematicsSystem objective to minimize G*| S * (Vd_TGs - Jv_TGs * v_next)|², also known as the “least squares” formulation.

Where: - G is cartesian_qp_weight; this coefficient can be used to balance the relative weight of multiple costs in the mathematical program. - S is a block-diagonal selector matrix applying per-frame axis masks to filter out components that are not tracked. Mask behavior: - Each 6×6 block of S corresponds to a goal frame and is constructed from the frame’s entry in cartesian_axis_masks. If a frame is not explicitly listed, all axes are enabled. - Each axis mask is a binary Vector6d of the form [ωx, ωy, ωz, vx, vy, vz] ∈ {0, 1}⁶.

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost.Config) → None

class Config

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost.Config, **kwargs) → None

property cartesian_axis_masks

Map from fully-scoped frame names to their 6D spatial velocity axis mask. Each Vector6d is a binary mask [ωx, ωy, ωz, vx, vy, vz] indicating which Cartesian velocity components (angular and translational) are being tracked. All elements must be set to either 0 or 1, and at least one element must be 1.

property cartesian_qp_weight

‘G’ in the class overview. Must be non-negative and finite.

property use_legacy_implementation

This is a temporary parameter intended for backwards compatibility. Do not change this value (unless you really, really know what you’re doing)!

GetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost.Config

Returns the current config.

SetConfig(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost, config: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.LeastSquaresCost.Config) → None

Replaces the config set in the constructor.

plant(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.multibody.plant.MultibodyPlant

Gets the plant used by the controller.

class Recipe

A recipe collects a list of ingredients for DifferentialInverseKinematicsSystem, allowing the user to customize the program being solved.

__init__(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Recipe) → None

AddIngredient(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Recipe, ingredient: object) → None

ingredient(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Recipe, i: int) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Ingredient

num_ingredients(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Recipe) → int

recipe(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem.Recipe

Gets the mathematical formulation recipe.

task_frame(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.multibody.tree.Frame

Gets the frame assumed on the desired_cartesian_poses input port.

time_step(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → float

Gets the time step used by the controller.

Vd_TG_limit(self: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsSystem) → pydrake.multibody.math.SpatialVelocity

Gets the clamping limit applied to inferred desired cartesian velocities.

class pydrake.multibody.inverse_kinematics.DistanceConstraint

Bases: pydrake.solvers.Constraint

Constrains the distance between a pair of geometries to be within a range [distance_lower, distance_upper].

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.DistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant, geometry_pair: Tuple[pydrake.geometry.GeometryId], plant_context: pydrake.systems.framework.Context, distance_lower: float, distance_upper: float) -> None

Parameter plant:

The plant to which the pair of geometries belong. plant should outlive this DistanceConstraint object.

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

Parameter plant_context:

The context for the plant. plant_context should outlive this DistanceConstraint object.

Parameter distance_lower:

The lower bound on the distance.

Parameter distance_upper:

The upper bound on the distance.

2. __init__(self: pydrake.multibody.inverse_kinematics.DistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], geometry_pair: Tuple[pydrake.geometry.GeometryId], plant_context: pydrake.systems.framework.Context_[AutoDiffXd], distance_lower: float, distance_upper: float) -> None

Overloaded constructor. Constructs the constraint with MultibodyPlant<AutoDiffXd>.

pydrake.multibody.inverse_kinematics.DoDifferentialInverseKinematics(*args, **kwargs)

Overloaded function.

1. DoDifferentialInverseKinematics(q_current: numpy.ndarray[numpy.float64[m, 1]], v_current: numpy.ndarray[numpy.float64[m, 1]], V: numpy.ndarray[numpy.float64[m, 1]], J: numpy.ndarray[numpy.float64[m, n]], parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters, N: Optional[numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]] = None, Nplus: Optional[numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]] = None) -> pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

Computes a generalized velocity v_next, via the following MathematicalProgram:

Click to expand C++ code...

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.

Parameter q_current:

The current generalized position.

Parameter v_current:

The current generalized position.

Parameter V:

Desired spatial velocity. It must have the same number of rows as J.

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

Parameter parameters:

Collection of various problem specific constraints and constants.

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

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

Note

There is a newer framework-based formulation for differential inverse kinematics: DifferentialInverseKinematicsSystem. This implementation has been shown to be more effective for real-world robots. Furthermore, its architecture is more flexible, allowing for more customization of the cost and constraint functions.

2. DoDifferentialInverseKinematics(robot: pydrake.multibody.plant.MultibodyPlant, context: pydrake.systems.framework.Context, V_WE_desired: numpy.ndarray[numpy.float64[6, 1]], frame_E: pydrake.multibody.tree.Frame, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) -> pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

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.

Parameter robot:

A MultibodyPlant model.

Parameter context:

Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.

Parameter V_WE_desired:

Desired world frame spatial velocity of frame_E.

Parameter frame_E:

End effector frame.

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

Note

There is a newer framework-based formulation for differential inverse kinematics: DifferentialInverseKinematicsSystem. This implementation has been shown to be more effective for real-world robots. Furthermore, its architecture is more flexible, allowing for more customization of the cost and constraint functions.

3. DoDifferentialInverseKinematics(robot: pydrake.multibody.plant.MultibodyPlant, context: pydrake.systems.framework.Context, V_AE_desired: numpy.ndarray[numpy.float64[6, 1]], frame_A: pydrake.multibody.tree.Frame, frame_E: pydrake.multibody.tree.Frame, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) -> pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

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.

Parameter robot:

A MultibodyPlant model.

Parameter context:

Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.

Parameter V_AE_desired:

Desired spatial velocity of frame_E in frame A.

Parameter frame_A:

Reference frame (inertial or non-inertial).

Parameter frame_E:

End effector frame.

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

Note

There is a newer framework-based formulation for differential inverse kinematics: DifferentialInverseKinematicsSystem. This implementation has been shown to be more effective for real-world robots. Furthermore, its architecture is more flexible, allowing for more customization of the cost and constraint functions.

4. DoDifferentialInverseKinematics(robot: pydrake.multibody.plant.MultibodyPlant, context: pydrake.systems.framework.Context, X_WE_desired: pydrake.math.RigidTransform, frame_E: pydrake.multibody.tree.Frame, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) -> pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

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.

Parameter robot:

A MultibodyPlant model.

Parameter context:

Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.

Parameter X_WE_desired:

Desired pose of frame_E in the world frame.

Parameter frame_E:

End effector frame.

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

Note

There is a newer framework-based formulation for differential inverse kinematics: DifferentialInverseKinematicsSystem. This implementation has been shown to be more effective for real-world robots. Furthermore, its architecture is more flexible, allowing for more customization of the cost and constraint functions.

5. DoDifferentialInverseKinematics(robot: pydrake.multibody.plant.MultibodyPlant, context: pydrake.systems.framework.Context, X_AE_desired: pydrake.math.RigidTransform, frame_A: pydrake.multibody.tree.Frame, frame_E: pydrake.multibody.tree.Frame, parameters: pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsParameters) -> pydrake.multibody.inverse_kinematics.DifferentialInverseKinematicsResult

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.

Parameter robot:

A MultibodyPlant model.

Parameter context:

Must be the Context of the MultibodyPlant. Contains the current generalized position and velocity.

Parameter X_AE_desired:

Desired pose of frame_E in frame_A.

Parameter frame_A:

Reference frame (inertial or non-inertial).

Parameter frame_E:

End effector frame.

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

Note

There is a newer framework-based formulation for differential inverse kinematics: DifferentialInverseKinematicsSystem. This implementation has been shown to be more effective for real-world robots. Furthermore, its architecture is more flexible, allowing for more customization of the cost and constraint functions.

class pydrake.multibody.inverse_kinematics.GazeTargetConstraint

Bases: pydrake.solvers.Constraint

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.

Mathematically the constraint is p_ST_Aᵀ * n_unit_A ≥ 0 (p_ST_Aᵀ * n_unit_A)² ≥ (cosθ)²p_ST_Aᵀ * p_ST_A where p_ST_A is the vector from S to T, expressed in frame A. n_unit_A is the unit length directional vector representing the center ray of the cone.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.GazeTargetConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameA: pydrake.multibody.tree.Frame, p_AS: numpy.ndarray[numpy.float64[3, 1]], n_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BT: numpy.ndarray[numpy.float64[3, 1]], cone_half_angle: float, plant_context: pydrake.systems.framework.Context) -> None

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameA:

The frame to which the gaze cone is fixed.

Parameter p_AS:

The position of the cone source point S, measured and expressed in frame A.

Parameter n_A:

The directional vector representing the center ray of the cone, expressed in frame A.

Parameter frameB:

The frame to which the target point T is fixed.

Parameter p_BT:

The position of the target point T, measured and expressed in frame B.

Parameter cone_half_angle:

The half angle of the cone. We denote it as θ in the class documentation. cone_half_angle is in radians.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Precondition:

frameA and frameB must belong to plant.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if n_A is close to zero. –

• RuntimeError if cone_half_angle ∉ [0, π/2]. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.GazeTargetConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], p_AS: numpy.ndarray[numpy.float64[3, 1]], n_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], p_BT: numpy.ndarray[numpy.float64[3, 1]], cone_half_angle: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Construct from MultibodyPlant<AutoDiffXd> instead of MultibodyPlant<double>.

class pydrake.multibody.inverse_kinematics.GlobalInverseKinematics

Solves the inverse kinematics problem as a mixed integer convex optimization problem. We use a mixed-integer convex relaxation of the rotation matrix. So if this global inverse kinematics problem says the solution is infeasible, then it is guaranteed that the kinematics constraints are not satisfiable. If the global inverse kinematics returns a solution, the posture should approximately satisfy the kinematics constraints, with some error. The approach is described in Global Inverse Kinematics via Mixed-integer Convex Optimization by Hongkai Dai, Gregory Izatt and Russ Tedrake, International Journal of Robotics Research, 2019.

__init__(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, plant: pydrake.multibody.plant.MultibodyPlant, options: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics.Options = GlobalInverseKinematics.Options(num_intervals_per_half_axis=2, approach=Approach.kBilinearMcCormick, interval_binning=IntervalBinning.kLogarithmic, linear_constraint_only=False)) → None

Parses the robot kinematics tree. The decision variables include the pose for each body (position/orientation). This constructor loops through each body inside the robot kinematics tree, adds the constraint on each body pose, so that the adjacent bodies are connected correctly by the joint in between the bodies.

Parameter plant:

The robot on which the inverse kinematics problem is solved. plant must be alive for as long as this object is around.

Parameter options:

The options to relax SO(3) constraint as mixed-integer convex constraints. Refer to MixedIntegerRotationConstraintGenerator for more details on the parameters in options.

AddJointLimitConstraint(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex, joint_lower_bound: float, joint_upper_bound: float, linear_constraint_approximation: bool = False) → None

Adds joint limits on a specified joint.

Note

This method is called from the constructor.

Parameter body_index:

The joint connecting the parent link to this body will be constrained.

Parameter joint_lower_bound:

The lower bound for the joint.

Parameter joint_upper_bound:

The upper bound for the joint.

Parameter linear_constraint_approximation:

If true, joint limits are approximated as linear constraints on parent and child link orientations, otherwise they are imposed as Lorentz cone constraints. With the Lorentz cone formulation, the joint limit constraint would be tight if our mixed-integer constraint on SO(3) were tight. By enforcing the joint limits as linear constraint, the original inverse kinematics problem is further relaxed, on top of SO(3) relaxation, but potentially with faster computation. $*Default:* is false.

AddPostureCost(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, q_desired: numpy.ndarray[numpy.float64[m, 1]], body_position_cost: numpy.ndarray[numpy.float64[m, 1]], body_orientation_cost: numpy.ndarray[numpy.float64[m, 1]]) → None

Penalizes the deviation to the desired posture.

For each body (except the world) in the kinematic tree, we add the cost

∑ᵢ body_position_cost(i) * body_position_error(i) + body_orientation_cost(i) * body_orientation_error(i) where body_position_error(i) is computed as the Euclidean distance error |p_WBo(i) - p_WBo_desired(i)| where - p_WBo(i) : position of body i’th origin Bo in the world frame W. - p_WBo_desired(i): position of body i’th origin Bo in the world frame W, computed from the desired posture q_desired.

body_orientation_error(i) is computed as (1 - cos(θ)), where θ is the angle between the orientation of body i’th frame and body i’th frame using the desired posture. Notice that 1 - cos(θ) = θ²/2 + O(θ⁴), so this cost is on the square of θ, when θ is small. Notice that since body 0 is the world, the cost on that body is always 0, no matter what value body_position_cost(0) and body_orientation_cost(0) take.

Parameter q_desired:

The desired posture.

Parameter body_position_cost:

The cost for each body’s position error. Unit is [1/m] (one over meters).

Precondition: 1. body_position_cost.rows() == plant.num_bodies(), where plant is the

input argument in the constructor of the class.

2. body_position_cost(i) is non-negative.

   $Raises:

RuntimeError if the precondition is not satisfied.

Parameter body_orientation_cost:

The cost for each body’s orientation error.

Precondition: 1. body_orientation_cost.rows() == plant.num_bodies() , where plant is

the input argument in the constructor of the class.

2. body_position_cost(i) is non-negative.

   $Raises:

RuntimeError if the precondition is not satisfied.

AddWorldOrientationConstraint(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex, desired_orientation: pydrake.common.eigen_geometry.Quaternion, angle_tol: float) → pydrake.solvers.Binding[LinearConstraint]

Adds a constraint that the angle between the body orientation and the desired orientation should not be larger than angle_tol. If we denote the angle between two rotation matrices R1 and R2 as θ, i.e. θ is the angle of the angle-axis representation of the rotation matrix R1ᵀ * R2, we then know

trace(R1ᵀ * R2) = 2 * cos(θ) + 1 as in http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/ To constraint θ < angle_tol, we can impose the following constraint

2 * cos(angle_tol) + 1 <= trace(R1ᵀ * R2) <= 3

Parameter body_index:

The index of the body whose orientation will be constrained.

Parameter desired_orientation:

The desired orientation of the body.

Parameter angle_tol:

The tolerance on the angle between the body orientation and the desired orientation. Unit is radians.

Returns binding:

The newly added constraint, together with the bound variables.

AddWorldPositionConstraint(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex, p_BQ: numpy.ndarray[numpy.float64[3, 1]], box_lb_F: numpy.ndarray[numpy.float64[3, 1]], box_ub_F: numpy.ndarray[numpy.float64[3, 1]], X_WF: pydrake.math.RigidTransform = RigidTransform(R=RotationMatrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]), p=[0.0, 0.0, 0.0])) → pydrake.solvers.Binding[LinearConstraint]

Adds the constraint that the position of a point Q on a body B (whose index is body_index), is within a box in a specified frame F. The constraint is that the point Q`'s position should lie within a bounding box in the frame `F. Namely

box_lb_F <= p_FQ <= box_ub_F

where p_FQ is the position of the point Q measured and expressed in the F, computed as

p_FQ = X_FW * (p_WBo + R_WB * p_BQ)

hence this is a linear constraint on the decision variables p_WBo and R_WB. The inequality is imposed elementwise.

Note

since the rotation matrix R_WB does not lie exactly on the SO(3), due to the McCormick envelope relaxation, this constraint is subject to the accumulated error from the root of the kinematics tree.

Parameter body_index:

The index of the body on which the position of a point is constrained.

Parameter p_BQ:

The position of the point Q measured and expressed in the body frame B.

Parameter box_lb_F:

The lower bound of the box in frame F.

Parameter box_ub_F:

The upper bound of the box in frame F.

Parameter X_WF:

The frame in which the box is specified. This frame is represented by a RigidTransform X_WF, the transform from the constraint frame F to the world frame W. Namely if the position of the point Q in the world frame is p_WQ, then the constraint is

box_lb_F <= R_FW * (p_WQ-p_WFo) <= box_ub_F where - R_FW is the rotation matrix of frame W expressed and measured in frame F. R_FW = X_WF.linear().transpose()`. - p_WFo is the position of frame F’s origin, expressed and measured in frame W`. p_WFo = X_WF.translation().

Default: is the identity transform.

$Returns binding:

The newly added constraint, together with the bound variables.

AddWorldRelativePositionConstraint(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index_B: pydrake.multibody.tree.BodyIndex, p_BQ: numpy.ndarray[numpy.float64[3, 1]], body_index_A: pydrake.multibody.tree.BodyIndex, p_AP: numpy.ndarray[numpy.float64[3, 1]], box_lb_F: numpy.ndarray[numpy.float64[3, 1]], box_ub_F: numpy.ndarray[numpy.float64[3, 1]], X_WF: pydrake.math.RigidTransform = RigidTransform(R=RotationMatrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]), p=[0.0, 0.0, 0.0])) → pydrake.solvers.Binding[LinearConstraint]

Adds the constraint that the position of a point Q on a body B relative to a point P on body A, is within a box in a specified frame F. Using monogram notation we have:

box_lb_F <= p_FQ - p_FP <= box_ub_F

where p_FQ and p_FP are the position of the points measured and expressed in F. The inequality is imposed elementwise. See AddWorldPositionConstraint for more details.

Parameter body_index_B:

The index of the body B.

Parameter p_BQ:

The position of the point Q measured and expressed in the body frame B.

Parameter body_index_A:

The index of the body A.

Parameter p_AP:

The position of the point P measured and expressed in the body frame A.

Parameter box_lb_F:

The lower bound of the box in frame F.

Parameter box_ub_F:

The upper bound of the box in frame F.

Parameter X_WF:

Defines the frame in which the box is specified. $*Default:* is the identity transform.

Returns binding:

The newly added constraint, together with the bound variables.

body_position(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex) → numpy.ndarray[object[3, 1]]

Getter for the decision variables on the position p_WBo of the body B’s origin measured and expressed in the world frame.

Parameter body_index:

The index of the queried body. Notice that body 0 is the world, and thus not a decision variable.

Raises:

• RuntimeError if the index is smaller than 1, or greater than or –

• equal to the total number of bodies in the robot. –

body_rotation_matrix(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex) → numpy.ndarray[object[3, 3]]

Getter for the decision variables on the rotation matrix R_WB for a body with the specified index. This is the orientation of body i’s frame measured and expressed in the world frame.

Parameter body_index:

The index of the queried body. Notice that body 0 is the world, and thus not a decision variable.

Raises:

• RuntimeError if the index is smaller than 1, or greater than or –

• equal to the total number of bodies in the robot. –

BodyPointInOneOfRegions(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex, p_BQ: numpy.ndarray[numpy.float64[3, 1]], region_vertices: list[numpy.ndarray[numpy.float64[3, n]]]) → numpy.ndarray[object[m, 1]]

Constrain the point Q lying within one of the convex polytopes. Each convex polytope Pᵢ is represented by its vertices as Pᵢ = ConvexHull(v_i1, v_i2, … v_in). Mathematically we want to impose the constraint that the p_WQ, i.e., the position of point Q in world frame W, satisfies

p_WQ ∈ Pᵢ for one i. To impose this constraint, we consider to introduce binary variable zᵢ, and continuous variables w_i1, w_i2, …, w_in for each vertex of Pᵢ, with the following constraints

p_WQ = sum_i (w_i1 * v_i1 + w_i2 * v_i2 + … + w_in * v_in) w_ij >= 0, ∀i,j w_i1 + w_i2 + … + w_in = zᵢ sum_i zᵢ = 1 zᵢ ∈ {0, 1} Notice that if zᵢ = 0, then w_i1 * v_i1 + w_i2 * v_i2 + … + w_in * v_in is just 0. This function can be used for collision avoidance, where each region Pᵢ is a free space region. It can also be used for grasping, where each region Pᵢ is a surface patch on the grasped object. Note this approach also works if the region Pᵢ overlaps with each other.

Parameter body_index:

The index of the body to on which point Q is attached.

Parameter p_BQ:

The position of point Q in the body frame B.

Parameter region_vertices:

region_vertices[i] is the vertices for the i’th region.

Returns z:

The newly added binary variables. If point Q is in the i’th region, z(i) = 1.

Precondition:

region_vertices[i] has at least 3 columns. Throw a RuntimeError if the precondition is not satisfied.

BodySphereInOneOfPolytopes(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, body_index: pydrake.multibody.tree.BodyIndex, p_BQ: numpy.ndarray[numpy.float64[3, 1]], radius: float, polytopes: list[pydrake.multibody.inverse_kinematics.GlobalInverseKinematics.Polytope3D]) → numpy.ndarray[object[m, 1]]

Adds the constraint that a sphere rigidly attached to a body has to be within at least one of the given bounded polytopes. If the polytopes don’t intersect, then the sphere is in one and only one polytope. Otherwise the sphere is in at least one of the polytopes (could be in the intersection of multiple polytopes.) If the i’th polytope is described as

Aᵢ * x ≤ bᵢ where Aᵢ ∈ ℝⁿ ˣ ³, bᵢ ∈ ℝⁿ. Then a sphere with center position p_WQ and radius r is within the i’th polytope, if

Aᵢ * p_WQ ≤ bᵢ - aᵢr where aᵢ(j) = Aᵢ.row(j).norm() To constrain that the sphere is in one of the n polytopes, we introduce the binary variable z ∈{0, 1}ⁿ, together with continuous variables yᵢ ∈ ℝ³, i = 1, …, n, such that p_WQ = y₁ + … + yₙ Aᵢ * yᵢ ≤ (bᵢ - aᵢr)zᵢ z₁ + … +zₙ = 1 Notice that when zᵢ = 0, Aᵢ * yᵢ ≤ 0 implies that yᵢ = 0. This is due to the boundedness of the polytope. If Aᵢ * yᵢ ≤ 0 has a non-zero solution y̅, that y̅ ≠ 0 and Aᵢ * y̅ ≤ 0. Then for any point x̂ in the polytope satisfying Aᵢ * x̂ ≤ bᵢ, we know the ray x̂ + ty̅, ∀ t ≥ 0 also satisfies Aᵢ * (x̂ + ty̅) ≤ bᵢ, thus the ray is within the polytope, violating the boundedness assumption.

Parameter body_index:

The index of the body to which the sphere is attached.

Parameter p_BQ:

The position of the sphere center in the body frame B.

Parameter radius:

The radius of the sphere.

Parameter polytopes:

polytopes[i] = (Aᵢ, bᵢ). We assume that Aᵢx≤ bᵢ is a bounded polytope. It is the user’s responsibility to guarantee the boundedness.

Returns z:

The newly added binary variables. If z(i) = 1, then the sphere is in the i’th polytope. If two or more polytopes are intersecting, and the sphere is in the intersection region, then it is up to the solver to choose one of z(i) to be 1.

get_mutable_prog(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics) → pydrake.solvers.MathematicalProgram

class Options

__init__(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics.Options) → None

property approach

property interval_binning

property linear_constraint_only

If true, add only mixed-integer linear constraints in the constructor of GlobalInverseKinematics. The mixed-integer relaxation is tighter with nonlinear constraints (such as Lorentz cone constraint) than with linear constraints, but the optimization takes more time with nonlinear constraints.

property num_intervals_per_half_axis

class Polytope3D

Describes a polytope in 3D as 𝐀 * 𝐱 ≤ 𝐛 (a set of half-spaces), where 𝐀 ∈ ℝⁿˣ³, 𝐱 ∈ ℝ³, 𝐛 ∈ ℝⁿ.

__init__(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics.Polytope3D, A: numpy.ndarray[numpy.float64[m, 3], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]]) → None

property A

property b

prog(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics) → pydrake.solvers.MathematicalProgram

ReconstructGeneralizedPositionSolution(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, result: pydrake.solvers.MathematicalProgramResult) → numpy.ndarray[numpy.float64[m, 1]]

After solving the inverse kinematics problem and finding out the pose of each body, reconstruct the robot generalized position (joint angles, etc) that matches with the body poses. Notice that since the rotation matrix is approximated, that the solution of body_rotmat() might not be on SO(3) exactly, the reconstructed body posture might not match with the body poses exactly, and the kinematics constraint might not be satisfied exactly with this reconstructed posture.

Warning

Do not call this method if the problem is not solved successfully! The returned value can be NaN or meaningless number if the problem is not solved.

Returns q:

The reconstructed posture of the robot of the generalized coordinates, corresponding to the RigidBodyTree on which the inverse kinematics problem is solved.

SetInitialGuess(self: pydrake.multibody.inverse_kinematics.GlobalInverseKinematics, q: numpy.ndarray[numpy.float64[m, 1]]) → None

Sets an initial guess for all variables (including the binary variables) by evaluating the kinematics of the plant at q. Currently, this is accomplished by solving the global IK problem subject to constraints that the positions and rotation matrices match the kinematics, which is dramatically faster than solving the original problem.

Raises:

RuntimeError if solving results in an infeasible program. –

class pydrake.multibody.inverse_kinematics.InverseKinematics

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.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.InverseKinematics, plant: pydrake.multibody.plant.MultibodyPlant, with_joint_limits: bool = True) -> None

Constructs an inverse kinematics problem for a MultibodyPlant. This constructor will create and own a context for plant.

Parameter plant:

The robot on which the inverse kinematics problem will be solved.

Parameter with_joint_limits:

If set to true, then the constructor imposes the joint limits (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);

2. __init__(self: pydrake.multibody.inverse_kinematics.InverseKinematics, plant: pydrake.multibody.plant.MultibodyPlant, plant_context: pydrake.systems.framework.Context, with_joint_limits: bool = True) -> None

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.

Parameter plant:

The robot on which the inverse kinematics problem will be solved. This plant should have been connected to a SceneGraph within a Diagram

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

Click to expand C++ code...

// 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(&builder).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.).

Parameter with_joint_limits:

If set to true, then the constructor imposes the joint limits (obtained from plant.GetPositionLowerLimits() and plant.GetPositionUpperLimits(), and from any body/joint locks set in plant_context). If set to false, then the constructor does not impose the joint limit constraints in the constructor.

AddAngleBetweenVectorsConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame, na_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, nb_B: numpy.ndarray[numpy.float64[3, 1]], angle_lower: float, angle_upper: float) → pydrake.solvers.Binding[Constraint]

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.

Parameter frameA:

The frame to which na is fixed.

Parameter na_A:

The vector na fixed to frame A, expressed in frame A.

Precondition:

na_A should be a non-zero vector.

Raises:

RuntimeError if na_A is close to zero. –

Parameter frameB:

The frame to which nb is fixed.

Parameter nb_B:

The vector nb fixed to frame B, expressed in frame B.

Precondition:

nb_B should be a non-zero vector.

Raises:

RuntimeError if nb_B is close to zero. –

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

Raises:

RuntimeError if angle_lower is negative. –

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

Raises:

RuntimeError if angle_upper is outside the bounds. –

AddAngleBetweenVectorsCost(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame, na_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, nb_B: numpy.ndarray[numpy.float64[3, 1]], c: float) → pydrake.solvers.Binding[Cost]

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.

Parameter frameA:

The frame to which na is fixed.

Parameter na_A:

The vector na fixed to frame A, expressed in frame A.

Precondition:

na_A should be a non-zero vector.

Raises:

RuntimeError if na_A is close to zero. –

Parameter frameB:

The frame to which nb is fixed.

Parameter nb_B:

The vector nb fixed to frame B, expressed in frame B.

Precondition:

nb_B should be a non-zero vector.

Raises:

RuntimeError if nb_B is close to zero. –

Parameter c:

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

AddDistanceConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, geometry_pair: Tuple[pydrake.geometry.GeometryId], distance_lower: float, distance_upper: float) → pydrake.solvers.Binding[Constraint]

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

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

Parameter distance_lower:

The lower bound on the distance.

Parameter distance_upper:

The upper bound on the distance.

AddGazeTargetConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame, p_AS: numpy.ndarray[numpy.float64[3, 1]], n_A: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BT: numpy.ndarray[numpy.float64[3, 1]], cone_half_angle: float) → pydrake.solvers.Binding[Constraint]

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.

Parameter frameA:

The frame where the gaze cone is fixed to.

Parameter p_AS:

The position of the cone source point S, measured and expressed in frame A.

Parameter n_A:

The directional vector representing the center ray of the cone, expressed in frame A.

Precondition:

n_A cannot be a zero vector.

Raises:

RuntimeError is n_A is close to a zero vector. –

Parameter frameB:

The frame where the target point T is fixed to.

Parameter p_BT:

The position of the target point T, measured and expressed in frame B.

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

Raises:

RuntimeError if cone_half_angle is outside of the bound. –

AddMinimumDistanceLowerBoundConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, bound: float, influence_distance_offset: float = 0.01) → pydrake.solvers.Binding[Constraint]

Adds the constraint that the pairwise distance between objects should be no smaller than bound. 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.

Parameter bound:

The minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries.

Parameter influence_distance_offset:

See MinimumDistanceLowerBoundConstraint for explanation.

Precondition:

The MultibodyPlant passed to the constructor of this has registered its geometry with a SceneGraph.

Precondition:

0 < influence_distance_offset < ∞

AddMinimumDistanceUpperBoundConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, bound: float, influence_distance_offset: float) → pydrake.solvers.Binding[Constraint]

Adds the constraint that at least one pair of geometries has distance no larger than bound. 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.

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

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

Precondition:

0 < influence_distance_offset < ∞

AddOrientationConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameAbar: pydrake.multibody.tree.Frame, R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame, R_BbarB: pydrake.math.RotationMatrix, theta_bound: float) → pydrake.solvers.Binding[Constraint]

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

Parameter frameAbar:

frame A_bar, the frame A is fixed to frame A_bar.

Parameter R_AbarA:

The orientation of frame A measured in frame A_bar.

Parameter frameBbar:

frame B_bar, the frame B is fixed to frame B_bar.

Parameter R_BbarB:

The orientation of frame B measured in frame B_bar.

Parameter 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(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameAbar: pydrake.multibody.tree.Frame, R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame, R_BbarB: pydrake.math.RotationMatrix, c: float) → pydrake.solvers.Binding[Cost]

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.

Parameter frameAbar:

A frame on the MultibodyPlant.

Parameter R_AbarA:

The rotation matrix describing the orientation of frame A relative to Abar.

Parameter frameBbar:

A frame on the MultibodyPlant.

Parameter R_BbarB:

The rotation matrix describing the orientation of frame B relative to Bbar.

Parameter c:

A scalar cost weight.

AddPointToLineDistanceConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frame_point: pydrake.multibody.tree.Frame, p_B1P: numpy.ndarray[numpy.float64[3, 1]], frame_line: pydrake.multibody.tree.Frame, p_B2Q: numpy.ndarray[numpy.float64[3, 1]], n_B2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float) → pydrake.solvers.Binding[Constraint]

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.

Parameter frame_point:

The frame to which P is attached.

Parameter p_B1P:

The position of P measured and expressed in frame_point.

Parameter frame_line:

The frame to which the line is attached.

Parameter p_B2Q:

The position of Q measured and expressed in frame_line, the line passes through Q.

Parameter n_B2:

The direction vector of the line measured and expressed in frame_line.

Parameter distance_lower:

The lower bound on the distance.

Parameter distance_upper:

The upper bound on the distance.

AddPointToPointDistanceConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frame1: pydrake.multibody.tree.Frame, p_B1P1: numpy.ndarray[numpy.float64[3, 1]], frame2: pydrake.multibody.tree.Frame, p_B2P2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float) → pydrake.solvers.Binding[Constraint]

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

Parameter frame1:

The frame to which P1 is attached.

Parameter p_B1P1:

The position of P1 measured and expressed in frame 1.

Parameter frame2:

The frame to which P2 is attached.

Parameter p_B2P2:

The position of P2 measured and expressed in frame 2.

Parameter distance_lower:

The lower bound on the distance.

Parameter distance_upper:

The upper bound on the distance.

AddPolyhedronConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameF: pydrake.multibody.tree.Frame, frameG: pydrake.multibody.tree.Frame, p_GP: numpy.ndarray[numpy.float64[3, n], flags.f_contiguous], A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]]) → pydrake.solvers.Binding[Constraint]

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

Parameter frameF:

The frame in which the position P is measured and expressed

Parameter frameG:

The frame in which the point P is rigidly attached.

Parameter p_GP:

p_GP.col(i) is the position of the i’th point Pi measured and expressed in frame G.

Parameter A:

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

Precondition:

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

Parameter b:

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

AddPositionConstraint(*args, **kwargs)

Overloaded function.

1. AddPositionConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], frameA: pydrake.multibody.tree.Frame, p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]]) -> pydrake.solvers.Binding[Constraint]

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.

Parameter frameB:

The frame in which point Q is fixed.

Parameter p_BQ:

The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.

Parameter frameA:

The frame in which the bounding box p_AQ_lower <= p_AQ <= p_AQ_upper is expressed.

Parameter p_AQ_lower:

The lower bound on the position of point Q, measured and expressed in frame A.

Parameter p_AQ_upper:

The upper bound on the position of point Q, measured and expressed in frame A.

2. AddPositionConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], frameAbar: pydrake.multibody.tree.Frame, X_AbarA: Optional[pydrake.math.RigidTransform], p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]]) -> pydrake.solvers.Binding[Constraint]

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.

Parameter frameB:

The frame in which point Q is fixed.

Parameter p_BQ:

The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.

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

Parameter X_AbarA:

The relative transform between frame Abar and A. If empty, then we use the identity transform.

Parameter p_AQ_lower:

The lower bound on the position of point Q, measured and expressed in frame A.

Parameter p_AQ_upper:

The upper bound on the position of point Q, measured and expressed in frame A.

AddPositionCost(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame, p_AP: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], C: numpy.ndarray[numpy.float64[3, 3], flags.f_contiguous]) → pydrake.solvers.Binding[Cost]

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.

Parameter frameA:

The frame in which point P’s position is measured.

Parameter p_AP:

The point P.

Parameter frameB:

The frame in which point Q’s position is measured.

Parameter p_BQ:

The point Q.

Parameter C:

A 3x3 matrix representing the cost in quadratic form.

context(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.systems.framework.Context

Getter for the plant context.

get_mutable_context(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.systems.framework.Context

Getter for the mutable plant context.

get_mutable_prog(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.solvers.MathematicalProgram

Getter for the optimization program constructed by InverseKinematics.

prog(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.solvers.MathematicalProgram

Getter for the optimization program constructed by InverseKinematics.

q(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → numpy.ndarray[object[m, 1]]

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

class pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint

Bases: pydrake.solvers.Constraint

Constrain min(d) >= lb, namely the signed distance between all candidate pairs of geometries (according to the logic of SceneGraphInspector::GetCollisionCandidates()) to be no smaller than a specified minimum distance lb. This constraint should be bound to decision variables corresponding to the configuration vector, q, of the associated MultibodyPlant.

The formulation of the constraint is

SmoothOverMax( φ((dᵢ(q) - d_influence)/(d_influence - lb)) / φ(-1) ) ≤ 1

where dᵢ(q) is the signed distance of the i-th pair, lb is the minimum allowable distance, d_influence is the “influence distance” (the distance below which a pair of geometries influences the constraint), φ is a solvers::MinimumValuePenaltyFunction. SmoothOverMax(d) is smooth over approximation of max(d). We require that lb < d_influence. The input scaling (dᵢ(q) - d_influence)/(d_influence - lb) ensures that at the boundary of the feasible set (when dᵢ(q) == lb), we evaluate the penalty function at -1, where it is required to have a non-zero gradient.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint, plant: pydrake.multibody.plant.MultibodyPlant, bound: float, plant_context: pydrake.systems.framework.Context, penalty_function: Callable[[float, bool], tuple] = None, influence_distance_offset: float = 0.01) -> None

Constructs a MinimumDistanceLowerBoundConstraint.

Parameter plant:

The multibody system on which the constraint will be evaluated. plant cannot be a nullptr. plant must outlive this constraint.

Parameter bound:

The minimum allowed value, lb, of the signed distance between any candidate pair of geometries.

Parameter penalty_function:

The penalty function formulation.

Default: QuadraticallySmoothedHinge

$Parameter plant_context:

The context of plant. The context should be obtained as a subsystem context from the diagram context, where the diagram (that contains both the MultibodyPlant and SceneGraph) creates the diagram context. plant_context cannot be a nullptr. plant_context must outlive this constraint. An example code of getting the plant context is

Click to expand C++ code...

{cc}
auto diagram_context = diagram.CreateDefaultContext();
auto plant_context = plant.GetMyMutableContextFromRoot(diagram_context.get());

Parameter influence_distance_offset:

The difference (in meters) between the influence distance, d_influence, and the minimum distance, lb (see class documentation), namely influence_distance = bound + influence_distance_offset. 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:* 0.01 meter. The chosen influence_distance_offset can significantly affect the runtime and optimization performance of using this constraint. Larger values result in more expensive collision checks (since more potential collision candidates must be considered) and may result in worse optimization performance (the optimizer may not be able to find a configuration that satisfies the constraint). In work at TRI, we have used much lower values (e.g. 1e-6) for influence_distance_offset with good results.

Raises:

• RuntimeError if plant has not registered its geometry with a –

• SceneGraph object. –

• RuntimeError if influence_distance_offset = ∞. –

• RuntimeError if influence_distance_offset ≤ 0.The penalty function `penalty_function(x –

  float, compute_grad: bool) -> tuple[float, Optional[float]]` returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

2. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], bound: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd], penalty_function: Callable[[float, bool], tuple] = None, influence_distance_offset: float = 0.01) -> None

Overloaded constructor. Constructs the constraint using MultibodyPlant<AutoDiffXd>.The penalty function penalty_function(x: float, compute_grad: bool) -> tuple[float, Optional[float]] returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

3. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint, collision_checker: pydrake.planning.CollisionChecker, bound: float, collision_checker_context: pydrake.planning.CollisionCheckerContext, penalty_function: Callable[[float, bool], tuple] = None, influence_distance_offset: float = 0.01) -> None

Overloaded constructor. Constructs the constraint with CollisionChecker instead of MultibodyPlant.

Parameter collision_checker:

collision_checker must outlive this constraint.

Parameter collision_checker_context:

The context for the collision checker. See CollisionChecker class for more details.The penalty function penalty_function(x: float, compute_grad: bool) -> tuple[float, Optional[float]] returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

distance_bound(self: pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint) → float

Getter for the lower bound of the minimum distance.

influence_distance(self: pydrake.multibody.inverse_kinematics.MinimumDistanceLowerBoundConstraint) → float

Getter for the influence distance.

class pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint

Bases: pydrake.solvers.Constraint

Constrain min(d) <= ub, namely at least one signed distance between a candidate pairs of geometries (according to the logic of SceneGraphInspector::GetCollisionCandidates()) to be no larger than a specified ub. This constraint should be bound to decision variables corresponding to the configuration vector, q, of the associated MultibodyPlant.

The formulation of the constraint is

SmoothUnderMax( φ((dᵢ(q) - d_influence)/(d_influence - ub)) / φ(-1) ) ≥ 1

where dᵢ(q) is the signed distance of the i-th pair, ub is the upper bound of the minimum distance, d_influence is the “influence distance” (the distance below which a pair of geometries influences the constraint), φ is a solvers::MinimumValuePenaltyFunction. SmoothUnderMax(d) is smooth under approximation of max(d). We require that ub < d_influence. The input scaling (dᵢ(q) - d_influence)/(d_influence - ub) ensures that at the boundary of the feasible set (when dᵢ(q) == ub), we evaluate the penalty function at -1, where it is required to have a non-zero gradient.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint, plant: pydrake.multibody.plant.MultibodyPlant, bound: float, plant_context: pydrake.systems.framework.Context, influence_distance_offset: float, penalty_function: Callable[[float, bool], tuple] = None) -> None

Constructs a MinimumDistanceUpperBoundConstraint.

Parameter plant:

The multibody system on which the constraint will be evaluated.

Parameter bound:

ub in the class documentation. The upper bound minimum signed distance between any candidate pair of geometries.

Parameter plant_context:

The context of plant. The context should be obtained as a subsystem context from the diagram context, where the diagram (that contains both the MultibodyPlant and SceneGraph) creates the diagram context. plant_context cannot be a nullptr. plant_context must outlive this constraint. An example code of getting the plant context is

Click to expand C++ code...

{cc}
auto diagram_context = diagram.CreateDefaultContext();
auto plant_context = plant.GetMyMutableContextFromRoot(diagram_context.get());

Parameter penalty_function:

The penalty function formulation.

Default: QuadraticallySmoothedHinge

$Parameter influence_distance_offset:

The difference (in meters) between the influence distance, d_influence, and the minimum distance_upper, ub (see class documentation), namely influence_distance = bound + influence_distance_offset. This value must be finite and strictly positive, as it is used to scale the signed distances between pairs of geometries. Larger value might increase the possibility of finding a solution through gradient based nonlinear optimization. This is because a geometry pair with distance larger than influence_distance is ignored, so is its gradient; hence the gradient-based optimizer doesn’t know to actively reduce the distance between that pair. We strongly suggest to use a different (and larger) influence_distance_offset as the one used in MinimumValueLowerBoundConstraint.

Raises:

• RuntimeError if plant has not registered its geometry with a –

• SceneGraph object. –

• RuntimeError if influence_distance_offset = ∞. –

• RuntimeError if influence_distance_offset ≤ 0.The penalty function `penalty_function(x –

  float, compute_grad: bool) -> tuple[float, Optional[float]]` returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

2. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], bound: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd], influence_distance_offset: float, penalty_function: Callable[[float, bool], tuple] = None) -> None

Overloaded constructor. Constructs the constraint using MultibodyPlant<AutoDiffXd>.The penalty function penalty_function(x: float, compute_grad: bool) -> tuple[float, Optional[float]] returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

3. __init__(self: pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint, collision_checker: pydrake.planning.CollisionChecker, bound: float, collision_checker_context: pydrake.planning.CollisionCheckerContext, influence_distance_offset: float, penalty_function: Callable[[float, bool], tuple] = None) -> None

Overloaded constructor. Constructs the constraint with CollisionChecker instead of MultibodyPlant.

Parameter collision_checker:

collision_checker must outlive this constraint.

Parameter collision_checker_context:

The context for the collision checker. See CollisionChecker class for more details.The penalty function penalty_function(x: float, compute_grad: bool) -> tuple[float, Optional[float]] returns [penalty_val, penalty_gradient] when compute_grad=True, or [penalty_value, None] when compute_grad=False. See minimum_value_constraint.h on the requirement on MinimumValuePenaltyFunction. Set penalty_function=None and then the constraint will use the default penalty function.

distance_bound(self: pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint) → float

Getter for the upper bound of the minimum distance.

influence_distance(self: pydrake.multibody.inverse_kinematics.MinimumDistanceUpperBoundConstraint) → float

Getter for the influence distance.

class pydrake.multibody.inverse_kinematics.OrientationConstraint

Bases: pydrake.solvers.Constraint

Constrains that the angle difference θ between the orientation of frame A and the orientation of frame B to satisfy θ ≤ θ_bound. 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 froma 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

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.OrientationConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameAbar: pydrake.multibody.tree.Frame, R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame, R_BbarB: pydrake.math.RotationMatrix, theta_bound: float, plant_context: pydrake.systems.framework.Context) -> None

Constructs an OrientationConstraint object. The frame A is fixed to a frame A̅, with orientatation R_AbarA measured in frame A̅. The frame B is fixed to a frame B̅, with orientation R_BbarB measured in frame B. We constrain the angle between frame A and B to be less than theta_bound.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameAbar:

The frame A̅ in the model to which frame A is fixed.

Parameter R_AbarA:

The orientation of frame A measured in frame A̅.

Parameter frameBbar:

The frame B̅ in the model to which frame B is fixed.

Parameter R_BbarB:

The orientation of frame B measured in frame B̅.

Parameter 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 class documentation. theta_bound is in radians.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if frameAbar or frameBbar does not belong to –

• plant` –

• RuntimeError if angle_bound < 0. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.OrientationConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameAbar: pydrake.multibody.tree.Frame_[AutoDiffXd], R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame_[AutoDiffXd], R_BbarB: pydrake.math.RotationMatrix, theta_bound: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Constructs the constraint using MultibodyPlant<AutoDiffXd>

class pydrake.multibody.inverse_kinematics.OrientationCost

Bases: pydrake.solvers.Cost

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

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.OrientationCost, plant: pydrake.multibody.plant.MultibodyPlant, frameAbar: pydrake.multibody.tree.Frame, R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame, R_BbarB: pydrake.math.RotationMatrix, c: float, plant_context: pydrake.systems.framework.Context) -> None

Constructs OrientationCost object.

Parameter plant:

The MultibodyPlant on which the cost is implemented. plant should be alive during the lifetime of this cost.

Parameter frameAbar:

A frame on the MultibodyPlant.

Parameter R_AbarA:

The rotation matrix describing the orientation of frame A relative to Abar.

Parameter frameBbar:

A frame on the MultibodyPlant.

Parameter R_BbarB:

The rotation matrix describing the orientation of frame B relative to Bbar.

Parameter c:

A scalar cost weight.

Parameter plant_context:

A context for the plant.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.OrientationCost, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameAbar: pydrake.multibody.tree.Frame_[AutoDiffXd], R_AbarA: pydrake.math.RotationMatrix, frameBbar: pydrake.multibody.tree.Frame_[AutoDiffXd], R_BbarB: pydrake.math.RotationMatrix, c: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>). Except the gradient of the cost is computed from autodiff.

class pydrake.multibody.inverse_kinematics.PointToLineDistanceConstraint

Bases: pydrake.solvers.Constraint

Constrain that the distance between a point P on frame B1 and another line L on frame B2 is within a range [distance_lower, distance_upper].

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.PointToLineDistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frame_point: pydrake.multibody.tree.Frame, p_B1P: numpy.ndarray[numpy.float64[3, 1]], frame_line: pydrake.multibody.tree.Frame, p_B2Q: numpy.ndarray[numpy.float64[3, 1]], n_B2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float, plant_context: pydrake.systems.framework.Context) -> None

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

Mathematically, we impose the constraint distance_lower² <= distance(P, L)² <= distance_upper². We impose the constraint on the distance square instead of distance directly, because the gradient of distance is not well defined at distance=0; on the other hand, the gradient of the distance square is well defined everywhere.

We parameterize the line using a point Q on the line, and a directional vector n along the line.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant must be alive during the lifetime of this constraint.

Parameter frame_point:

The frame B1 to which the point P is attached.

Parameter p_B1P:

The position of point P measured and expressed in B1.

Parameter frame_line:

The frame B2 to which the line is attached.

Parameter p_B2Q:

Q is a point on the line, p_B2Q is the position of this point Q measured and expressed in B2.

Parameter n_B2:

n is the directional vector of the line, n_B2 is this vector measured and expressed in B2.

Parameter distance_lower:

The lower bound on the distance, must be non-negative.

Parameter distance_upper:

The upper bound on the distance, must be non-negative.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context must be alive during the lifetime of this constraint.

2. __init__(self: pydrake.multibody.inverse_kinematics.PointToLineDistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frame_point: pydrake.multibody.tree.Frame_[AutoDiffXd], p_B1P: numpy.ndarray[numpy.float64[3, 1]], frame_line: pydrake.multibody.tree.Frame_[AutoDiffXd], p_B2Q: numpy.ndarray[numpy.float64[3, 1]], n_B2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>), except the gradient of the constraint is computed from autodiff.

class pydrake.multibody.inverse_kinematics.PointToPointDistanceConstraint

Bases: pydrake.solvers.Constraint

Constrain that the distance between a point P1 on frame B1 and another point P2 on frame B2 is within a range [distance_lower, distance_upper].

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.PointToPointDistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frame1: pydrake.multibody.tree.Frame, p_B1P1: numpy.ndarray[numpy.float64[3, 1]], frame2: pydrake.multibody.tree.Frame, p_B2P2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float, plant_context: pydrake.systems.framework.Context) -> None

Constrain that the distance between a point P1 attached to frame B1 and another point P2 attached to frame B2 is within the range [distance_lower, distance_upper]. Mathematically, we impose the constraint distance_lower² <= distance(P1, P2)² <= distance_upper². We impose the constraint on the distance square instead of distance directly, because the gradient of distance is not well defined at distance=0, the gradient of the distance square is well defined everywhere.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frame1:

The frame in which P1 is attached to.

Parameter p_B1P1:

The position of P1 measured and expressed in B1.

Parameter frame2:

The frame in which P2 is attached to.

Parameter p_B2P2:

The position of P2 measured and expressed in B2.

Parameter distance_lower:

The lower bound on the distance, must be non-negative.

Parameter distance_upper:

The upper bound on the distance, must be non-negative.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

2. __init__(self: pydrake.multibody.inverse_kinematics.PointToPointDistanceConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frame1: pydrake.multibody.tree.Frame_[AutoDiffXd], p_B1P1: numpy.ndarray[numpy.float64[3, 1]], frame2: pydrake.multibody.tree.Frame_[AutoDiffXd], p_B2P2: numpy.ndarray[numpy.float64[3, 1]], distance_lower: float, distance_upper: float, plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>), except the gradient of the constraint is computed from autodiff.

class pydrake.multibody.inverse_kinematics.PolyhedronConstraint

Bases: pydrake.solvers.Constraint

Constrain the position of points P1, P2, …, Pn to satisfy the constraint A * [p_FP1; p_FP2; …; p_FPn] <= b, where p_FPi is the position of point Pi measured and expressed in frame F. Notice the constraint is imposed on the stacked column vector [p_FP1; p_FP2; …; p_FPn], not on each individual point.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.PolyhedronConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameF: pydrake.multibody.tree.Frame, frameG: pydrake.multibody.tree.Frame, p_GP: numpy.ndarray[numpy.float64[3, n], flags.f_contiguous], A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]], plant_context: pydrake.systems.framework.Context) -> None

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

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameF:

The frame in which the position P is measured and expressed

Parameter frameG:

The frame in which the point P is rigidly attached.

Parameter p_GP:

p_GP.col(i) is the position of the i’th point Pi measured and expressed in frame G.

Parameter A:

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

Precondition:

A.cols() = 3 * p_GP.cols();

Parameter b:

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

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

2. __init__(self: pydrake.multibody.inverse_kinematics.PolyhedronConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameF: pydrake.multibody.tree.Frame_[AutoDiffXd], frameG: pydrake.multibody.tree.Frame_[AutoDiffXd], p_GP: numpy.ndarray[numpy.float64[3, n], flags.f_contiguous], A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>). Except the gradient of the constraint is computed from autodiff.

class pydrake.multibody.inverse_kinematics.PositionConstraint

Bases: pydrake.solvers.Constraint

Constrains the position of a point Q, rigidly attached to a frame B, to be within a bounding box measured and expressed in frame A. Namely p_AQ_lower <= p_AQ <= p_AQ_upper.

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.PositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameA: pydrake.multibody.tree.Frame, p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], plant_context: pydrake.systems.framework.Context) -> None

Constructs PositionConstraint object.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameA:

The frame in which point Q’s position is measured.

Parameter p_AQ_lower:

The lower bound on the position of point Q, measured and expressed in frame A.

Parameter p_AQ_upper:

The upper bound on the position of point Q, measured and expressed in frame A.

Parameter frameB:

The frame to which point Q is rigidly attached.

Parameter p_BQ:

The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Precondition:

frameA and frameB must belong to plant.

Precondition:

p_AQ_lower(i) <= p_AQ_upper(i) for i = 1, 2, 3.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.PositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant, frameAbar: pydrake.multibody.tree.Frame, X_AbarA: Optional[pydrake.math.RigidTransform], p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], plant_context: pydrake.systems.framework.Context) -> None

Overloaded constructor. Except that the constructor takes in a frame A̅ and a pose X_AAbar between the frame A and A̅. We will constrain the position of point Q expressed in the frame A to lie within a bounding box of A.

Parameter plant:

The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.

Parameter frameAbar:

The frame A̅ in which point Q’s position is measured.

Parameter X_AbarA:

relative transform between the frame A̅ and A. If empty, then we use identity transform.

Parameter p_AQ_lower:

The lower bound on the position of point Q, measured and expressed in frame A.

Parameter p_AQ_upper:

The upper bound on the position of point Q, measured and expressed in frame A.

Parameter frameB:

The frame to which point Q is rigidly attached.

Parameter p_BQ:

The position of the point Q, rigidly attached to frame B, measured and expressed in frame B.

Parameter plant_context:

The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.

Precondition:

frameA and frameB must belong to plant.

Precondition:

p_AQ_lower(i) <= p_AQ_upper(i) for i = 1, 2, 3.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if plant_context is nullptr. –

3. __init__(self: pydrake.multibody.inverse_kinematics.PositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], p_BQ: numpy.ndarray[numpy.float64[3, 1]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>). Except the gradient of the constraint is computed from autodiff.

4. __init__(self: pydrake.multibody.inverse_kinematics.PositionConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameAbar: pydrake.multibody.tree.Frame_[AutoDiffXd], X_AbarA: Optional[pydrake.math.RigidTransform], p_AQ_lower: numpy.ndarray[numpy.float64[3, 1]], p_AQ_upper: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], p_BQ: numpy.ndarray[numpy.float64[3, 1]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>). Except the gradient of the constraint is computed from autodiff.

set_bounds(self: pydrake.multibody.inverse_kinematics.PositionConstraint, new_lb: numpy.ndarray[numpy.float64[m, 1]], new_ub: numpy.ndarray[numpy.float64[m, 1]]) → None

Set the upper and lower bounds of the constraint.

Parameter new_lb:

A num_constraints x 1 vector.

Parameter new_ub:

A num_constraints x 1 vector.

Note

If the users want to expose this method in a sub-class, do using Constraint::set_bounds, as in LinearConstraint.

UpdateLowerBound(self: pydrake.multibody.inverse_kinematics.PositionConstraint, new_lb: numpy.ndarray[numpy.float64[m, 1]]) → None

Updates the lower bound.

Note

if the users want to expose this method in a sub-class, do using Constraint::UpdateLowerBound, as in LinearConstraint.

UpdateUpperBound(self: pydrake.multibody.inverse_kinematics.PositionConstraint, new_ub: numpy.ndarray[numpy.float64[m, 1]]) → None

Updates the upper bound.

Note

if the users want to expose this method in a sub-class, do using Constraint::UpdateUpperBound, as in LinearConstraint.

class pydrake.multibody.inverse_kinematics.PositionCost

Bases: pydrake.solvers.Cost

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

__init__(*args, **kwargs)

Overloaded function.

1. __init__(self: pydrake.multibody.inverse_kinematics.PositionCost, plant: pydrake.multibody.plant.MultibodyPlant, frameA: pydrake.multibody.tree.Frame, p_AP: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame, p_BQ: numpy.ndarray[numpy.float64[3, 1]], C: numpy.ndarray[numpy.float64[3, 3], flags.f_contiguous], plant_context: pydrake.systems.framework.Context) -> None

Constructs PositionCost object.

Parameter plant:

The MultibodyPlant on which the cost is implemented. plant should be alive during the lifetime of this cost.

Parameter frameA:

The frame in which point P’s position is measured.

Parameter p_AP:

The point P.

Parameter frameB:

The frame in which point Q’s position is measured.

Parameter p_BQ:

The point Q.

Parameter C:

A 3x3 matrix representing the cost in quadratic form.

Parameter plant_context:

A context for the plant.

Raises:

• RuntimeError if plant is nullptr. –

• RuntimeError if plant_context is nullptr. –

2. __init__(self: pydrake.multibody.inverse_kinematics.PositionCost, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], p_AP: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], p_BQ: numpy.ndarray[numpy.float64[3, 1]], C: numpy.ndarray[numpy.float64[3, 3], flags.f_contiguous], plant_context: pydrake.systems.framework.Context_[AutoDiffXd]) -> None

Overloaded constructor. Same as the constructor with the double version (using MultibodyPlant<double> and Context<double>). Except the gradient of the cost is computed from autodiff.

class pydrake.multibody.inverse_kinematics.UnitQuaternionConstraint

Bases: pydrake.solvers.Constraint

Constrains the quaternion to have a unit length.

Note

: It is highly recommended that in addition to adding this constraint, you also call MathematicalProgram::SetInitialGuess(), e.g.

Click to expand C++ code...

// Set a non-zero initial guess to help avoid singularities.
prog_->SetInitialGuess(q_.segment<4>(quaternion_start),
Eigen::Vector4d{1, 0, 0, 0});

__init__(self: pydrake.multibody.inverse_kinematics.UnitQuaternionConstraint) → None