pydrake.multibody.inverse_kinematics

InverseKinematics module

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

For unit quaternion and (holonomic) constraints registered with plant adds a corresponding solver::Constraint to prog, using decision variables q to represent the generalized positions of the plant.

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.

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.

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.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 differential inverse kinematics.

__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) → Optional[tuple[numpy.ndarray[numpy.float64[3, 1]], numpy.ndarray[numpy.float64[3, 1]]]]

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) → Optional[tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]]

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) → Optional[tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]]

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

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

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.

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.

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.

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.

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.

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

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

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

Parameter 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 limit (obtained from plant.GetPositionLowerLimits() and plant.GetPositionUpperLimits(). If set to false, then the constructor does not impose the joint limit constraints in the constructor.

Note

The inverse kinematics problem constructed in this way doesn’t permit collision related constraint (such as calling AddMinimumDistanceConstraint). To enable collision related constraint, call InverseKinematics(const MultibodyPlant<double>& plant, systems::Context<double>* plant_context);

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(&(items.plant)).AddModels("model.sdf");
// 3. Construct the diagram
auto diagram = builder.Build();
// 4. Create diagram context.
auto diagram_context= diagram->CreateDefaultContext();
// 5. Get the context for the plant.
auto plant_context = &(diagram->GetMutableSubsystemContext(items.plant,
diagram_context.get()));

This context will be modified during calling ik.prog.Solve(…). When Solve() returns result, context will store the optimized posture, namely plant.GetPositions(*context) will be the same as in result.GetSolution(ik.q()). The user could then use this context to perform kinematic computation (like computing the position of the end-effector etc.).

Parameter with_joint_limits:

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

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