pydrake.multibody.inverse_kinematics
InverseKinematics module
- pydrake.multibody.inverse_kinematics.AddUnitQuaternionConstraintOnPlant(*args, **kwargs)
Overloaded function.
AddUnitQuaternionConstraintOnPlant(plant: pydrake.multibody.plant.MultibodyPlant, q_vars: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram) -> None
Add unit length constraints to all the variables representing quaternion in
q_vars
. Namely the quaternions for floating base joints inplant
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.
AddUnitQuaternionConstraintOnPlant(plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], q_vars: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram) -> None
Add unit length constraints to all the variables representing quaternion in
q_vars
. Namely the quaternions for floating base joints inplant
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 vectorb
is between [θ_lower, θ_upper].a
is fixed to a frame A, whileb
is fixed to a frame B. Mathematically, if we denote a_unit_A asa
expressed in frame A after normalization (a_unit_A has unit length), and b_unit_B asb
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.
__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
andb
. It is denoted as θ_lower in the class documentation.- Parameter
angle_upper
: The upper bound on the angle between
a
andb
. 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
andframeB
must belong toplant
.
- 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. –
__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
andb
. c is a constant scalar.- __init__(*args, **kwargs)
Overloaded function.
__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
andframeB
must belong toplant
.
- 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. –
__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.
__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.
__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 inplant
.
- 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.
__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. –
__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 toTrue
, then differential IK is computed using therobot_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.
__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 totime_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 toFalse
if you want IsDifferenceEquationSystem() to returnTrue
.
Note: All references must remain valid for the lifetime of this system.
- Precondition:
frame_E != frame_A.
__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 totime_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 toFalse
if you want IsDifferenceEquationSystem() to returnTrue
.
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).
- Parameter
- 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. –
- Parameter
- 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. –
- Parameter
- 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. –
- Parameter
- 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.
__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.
__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.
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 asV
.- 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.
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 inparameters
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.
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 inparameters
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
inframe
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.
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 fromcontext
, and dt is taken fromparameters
.- 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.
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 fromcontext
, and dt is taken fromparameters
.- 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
inframe_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.
__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
andframeB
must belong toplant
.
- 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. –
__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.
- Parameter
- 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 originBo
in the world frameW
. - p_WBo_desired(i): position of body i’th originBo
in the world frameW
, computed from the desired postureq_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)
andbody_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 theinput argument in the constructor of the class.
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
isthe input argument in the constructor of the class.
body_position_cost(i) is non-negative.
$Raises:
RuntimeError if the precondition is not satisfied.
- Parameter
- 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 matricesR1
andR2
asθ
, i.e. θ is the angle of the angle-axis representation of the rotation matrixR1ᵀ * R2
, we then knowtrace(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 constraint2 * 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.
- Parameter
- 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 bodyB
(whose index isbody_index
), is within a box in a specified frameF
. The constraint is that the pointQ`'s position should lie within a bounding box in the frame `F
. Namelybox_lb_F <= p_FQ <= box_ub_F
where p_FQ is the position of the point Q measured and expressed in the
F
, computed asp_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 isp_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 frameF
. 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.
- Parameter
- 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 bodyB
relative to a pointP
on bodyA
, is within a box in a specified frameF
. 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.
- Parameter
- 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. –
- Parameter
- 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. –
- Parameter
- 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.
- Returns
- 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 useJoint::Lock
on the joints in that Context that should be fixed during IK.- __init__(*args, **kwargs)
Overloaded function.
__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);
__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. –
- Parameter
- 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
andnb
. 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θ).
- Parameter
- 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.
- Parameter
- 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. –
- Parameter
- 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
< ∞
- Parameter
- 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
< ∞
- Parameter
- 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.
- Parameter
- 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, andc
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.
- Parameter
- 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.
- Parameter
- 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.
- Parameter
- 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.
- Parameter
- AddPositionConstraint(*args, **kwargs)
Overloaded function.
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.
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.
- Parameter
- 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.
__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 isClick 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.
__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.
__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.
__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.
__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.
__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.
__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 orientationR_BbarB
measured in frame B. We constrain the angle between frame A and B to be less thantheta_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. –
__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, andc
is a cost scaling.- __init__(*args, **kwargs)
Overloaded function.
__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. –
__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.
__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.
__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.
__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.
__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.
__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.
__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.
__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
andframeB
must belong toplant
.- 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. –
__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
andframeB
must belong toplant
.- 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. –
__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.
__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.
- Parameter
- 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.
__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. –
__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