pydrake.multibody.inverse_kinematics

InverseKinematics module

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.

AddAngleBetweenVectorsConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame_[float], na_A: numpy.ndarray[float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[float], nb_B: numpy.ndarray[float64[3, 1]], angle_lower: float, angle_upper: float) → pydrake.solvers.mathematicalprogram.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:ValueError 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:ValueError 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:ValueError 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:ValueError if angle_upper is outside the bounds.
AddGazeTargetConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameA: pydrake.multibody.tree.Frame_[float], p_AS: numpy.ndarray[float64[3, 1]], n_A: numpy.ndarray[float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[float], p_BT: numpy.ndarray[float64[3, 1]], cone_half_angle: float) → pydrake.solvers.mathematicalprogram.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:ValueError 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:ValueError if cone_half_angle is outside of the bound.
AddMinimumDistanceConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, minimum_distance: float, threshold_distance: float = 1.0) → pydrake.solvers.mathematicalprogram.Binding_Constraint

Adds the constraint that the pairwise distance between objects should be no smaller than minimum_distance. 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 minimum_distance:
The minimum allowed value, dₘᵢₙ, of the signed distance between any candidate pair of geometries.
Parameter influence_distance_offset:
The difference (in meters) between the influence distance, d_influence, and the minimum distance, dₘᵢₙ. 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:* 1 meter

See also

MinimumDistanceConstraint for more details on the constraint formulation.

Precondition:
The MultibodyPlant passed to the constructor of this has registered its geometry with a SceneGraph.
Precondition:
0 < influence_distance_offset < ∞
AddOrientationConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameAbar: pydrake.multibody.tree.Frame_[float], R_AbarA: pydrake.math.RotationMatrix_[float], frameBbar: pydrake.multibody.tree.Frame_[float], R_BbarB: pydrake.math.RotationMatrix_[float], theta_bound: float) → pydrake.solvers.mathematicalprogram.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.
AddPositionConstraint(self: pydrake.multibody.inverse_kinematics.InverseKinematics, frameB: pydrake.multibody.tree.Frame_[float], p_BQ: numpy.ndarray[float64[3, 1]], frameA: pydrake.multibody.tree.Frame_[float], p_AQ_lower: numpy.ndarray[float64[3, 1]], p_AQ_upper: numpy.ndarray[float64[3, 1]]) → pydrake.solvers.mathematicalprogram.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.
get_mutable_prog(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.solvers.mathematicalprogram.MathematicalProgram

Getter for the optimization program constructed by InverseKinematics.

prog(self: pydrake.multibody.inverse_kinematics.InverseKinematics) → pydrake.solvers.mathematicalprogram.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.