Inverse kinematics

Inverse kinematics (IK) is the problem of computing motions (velocities, accelerations) that make the robot achieve a given set of tasks, such as putting a foot on a surface, moving the center of mass (COM) to a target location, etc. If you are not familiar with these concepts, check out this introduction to inverse kinematics.

Tasks

Tasks are the way to specify objectives to the robot model in a human-readable way.

class pymanoid.tasks.AxisAngleContactTask(robot, link, target, weight=None, gain=None, exclude_dofs=None)

Contact task using angle-axis rather than quaternion computations. The benefit of this task is that some degrees of constraint (DOC) of the contact constraint can be (fully or partially) disabled via the doc_mask attribute.

Parameters:
  • robot (Robot) – Target robot.
  • link (Link or string) – Robot Link, or name of the link field in the robot argument.
  • target (list or array (shape=(7,)) or pymanoid.Body) – Pose coordinates of the link’s target.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
doc_mask

array – Array of dimension six whose values lie between 0 (degree of constraint fully disabled) and 1 (fully enabled). The first three values correspond to translation (Tx, Ty, Tz) while the last three values correspond to rotation (Rx, Ry, Rz).

update_target(target)

Update the task residual with a new target.

Parameters:target (Point or array or list) – New link position target.
class pymanoid.tasks.COMAccelTask(robot, weight=None, gain=None, exclude_dofs=None)

COM acceleration task.

Parameters:
  • robot (Humanoid) – Target robot.
  • target (list or array or Point) – Coordinates or the target COM position.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.

Notes

Expanding \(\ddot{x}_G = u\) in terms of COM Jacobian and Hessian, the equation of the task is:

\[J_\mathrm{COM} \dot{q}_\mathrm{next} = \frac12 u + J_\mathrm{COM} \dot{q} - \frac12 \delta t \dot{q}^T H_\mathrm{COM} \dot{q}\]

See the documentation of the PendulumModeTask for a detailed derivation.

class pymanoid.tasks.COMTask(robot, target, weight=None, gain=None, exclude_dofs=None)

COM tracking task.

Parameters:
  • robot (Humanoid) – Target robot.
  • target (list or array or Point) – Coordinates or the target COM position.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
update_target(target)

Update the task residual with a new target.

Parameters:target (Point or array or list) – New COM position target.
class pymanoid.tasks.ContactTask(robot, link, target, weight=None, gain=None, exclude_dofs=None)

In essence, a contact task is a pymanoid.tasks.PoseTask with a much (much) larger weight. For plausible motions, no task should have a weight higher than (or even comparable to) that of contact tasks.

class pymanoid.tasks.DOFTask(robot, index, target, weight=None, gain=None, exclude_dofs=None)

Track a reference DOF value.

Parameters:
  • robot (Robot) – Target robot.
  • index (string or integer) – DOF index or string of DOF identifier.
  • target (scalar) – Target DOF value.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
class pymanoid.tasks.MinAccelTask(robot, weight=None, gain=None, exclude_dofs=None)

Task to minimize joint accelerations.

Parameters:
  • robot (Robot) – Target robot.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.

Note

As the differential IK returns velocities, we approximate the minimization over \(\ddot{q}\) by that over \((\dot{q}_\mathrm{next} - \dot{q})\). See the documentation of the PendulumModeTask for details on the discrete approximation of \(\ddot{q}\).

class pymanoid.tasks.MinCAMTask(robot, weight=None, gain=None, exclude_dofs=None)

Minimize the centroidal angular momentum (not its derivative).

Parameters:
  • robot (Humanoid) – Target robot.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
class pymanoid.tasks.MinVelTask(robot, weight=None, gain=None, exclude_dofs=None)

Task to minimize joint velocities

Parameters:
  • robot (Robot) – Target robot.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
class pymanoid.tasks.PendulumModeTask(robot, weight=None, gain=None, exclude_dofs=None)

Task to minimize the rate of change of the centroidal angular momentum.

Parameters:
  • robot (Humanoid) – Target robot.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.

Notes

The way this task is implemented may be surprising. Basically, keeping a constant CAM \(L_G\) means that \(\dot{L}_G = 0\), that is,

\[\frac{\mathrm{d} (J_\mathrm{CAM} \dot{q})}{\mathrm{d} t} = 0 \ \Leftrightarrow\ J_\mathrm{CAM} \ddot{q} + \dot{q}^T H_\mathrm{CAM} \dot{q} = 0\]

Because the IK works at the velocity level, we approximate \(\ddot{q}\) by finite difference from the previous robot velocity. Assuming that the velocity \(\dot{q}_\mathrm{next}\) output by the IK is applied immediately, joint angles become:

\[q' = q + \dot{q}_\mathrm{next} \delta t\]

Meanwhile, the Taylor expansion of q is

\[q' = q + \dot{q} \delta t + \frac12 \ddot{q} \delta t^2,\]

so that applying \(\dot{q}_\mathrm{next}\) is equivalent to having the following constant acceleration over \(\delta t\):

\[\ddot{q} = 2 \frac{\dot{q}_\mathrm{next} - \dot{q}}{\delta t}.\]

Replacing in the Jacobian/Hessian expansion yields:

\[2 J_\mathrm{CAM} \frac{\dot{q}_\mathrm{next} - \dot{q}}{\delta t} + + \dot{q}^T H_\mathrm{CAM} \dot{q} = 0.\]

Finally, the task in \(\dot{q}_\mathrm{next}\) is:

\[J_\mathrm{CAM} \dot{q}_\mathrm{next} = J_\mathrm{CAM} \dot{q} - \frac12 \delta t \dot{q}^T H_\mathrm{CAM} \dot{q}\]

Note how there are two occurrences of \(J_\mathrm{CAM}\): one in the task residual, and the second in the task Jacobian.

class pymanoid.tasks.PosTask(robot, link, target, weight=None, gain=None, exclude_dofs=None)

Position task for a given link.

Parameters:
  • robot (Robot) – Target robot.
  • link (Link) – One of the Link objects in the kinematic chain of the robot.
  • target (list or array (shape=(3,)) or pymanoid.Body) – Coordinates of the link’s target.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
update_target(target)

Update the task residual with a new target.

Parameters:target (Point or array or list) – New link position target.
class pymanoid.tasks.PoseTask(robot, link, target, weight=None, gain=None, exclude_dofs=None)

Pose task for a given link.

Parameters:
  • robot (Robot) – Target robot.
  • link (Link or string) – Robot Link, or name of the link field in the robot argument.
  • target (list or array (shape=(7,)) or pymanoid.Body) – Pose coordinates of the link’s target.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
update_target(target)

Update the task residual with a new target.

Parameters:target (Point or array or list) – New link position target.
class pymanoid.tasks.PostureTask(robot, q_ref, weight=None, gain=None, exclude_dofs=None)

Track a set of reference joint angles, a common choice to regularize the weighted IK problem.

Parameters:
  • robot (Robot) – Target robot.
  • q_ref (array) – Vector of reference joint angles.
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.
class pymanoid.tasks.Task(weight=None, gain=None, exclude_dofs=None)

Generic IK task.

Parameters:
  • weight (scalar, optional) – Task weight used in IK cost function. If None, needs to be set later.
  • gain (scalar, optional) – Proportional gain of the task.
  • exclude_dofs (list of integers, optional) – DOF indices not used by task.

Note

See <https://scaron.info/teaching/inverse-kinematics.html> for an introduction to the concepts used here.

cost(dt)

Compute the weighted norm of the task residual.

Parameters:dt (scalar) – Time step in [s].
Returns:cost – Current cost value.
Return type:scalar
exclude_dofs(dofs)

Exclude additional DOFs from being used by the task.

jacobian()

Compute the Jacobian matrix of the task.

Returns:J – Jacobian matrix of the task.
Return type:array
residual(dt)

Compute the residual of the task.

Returns:r – Residual vector of the task.
Return type:array

Notes

Residuals returned by the residual function must have the unit of a velocity. For instance, \(\dot{q}\) and \((q_1 - q_2) / dt\) are valid residuals, but \(\frac12 q\) is not.

stiffness

Compatibility field for cohabitation with Tasks.

Solver

The IK solver is the numerical optimization program that converts task targets and the current robot state to joint motions. In pymanoid, joint motions are computed as velocities that are integrated forward during each simulation cycle (other IK solvers may compute acceleration or jerk values, which are then integrated twice or thrice respectively).

class pymanoid.ik.IKSolver(robot, active_dofs=None, doflim_gain=0.5)

Compute velocities bringing the system closer to fulfilling a set of tasks.

Parameters:
  • robot (Robot) – Robot to be updated.
  • active_dofs (list of integers, optional) – List of DOFs updated by the IK solver.
  • doflim_gain (scalar, optional) – DOF-limit gain as described in [Kanoun12]. In this implementation, it should be between zero and one.
doflim_gain

scalar, optional – DOF-limit gain as described in [Kanoun12]. In this implementation, it should be between zero and one.

lm_damping

scalar – Add Levenberg-Marquardt damping as described in [Sugihara11]. This damping significantly improves numerical stability, but convergence gets slower when its value is too high.

slack_dof_limits

bool – Add slack variables to maximize DOF range? This method is used in [Nozawa16] to keep joint angles as far away from their limits as possible. It slows down computations as there are twice as many optimization variables, but is more numerically stable and won’t produce inconsistent constraints. Defaults to False.

slack_maximize

scalar – Linear cost weight applied when slack_dof_limits is True.

slack_regularize

scalar – Regularization weight applied when slack_dof_limits is True.

qd

array – Velocity returned by last solver call.

robot

pymanoid.Robot – Robot model.

tasks

dict – Dictionary of active IK tasks, indexed by task name.

Notes

One unsatisfactory aspect of the DOF-limit gain is that it slows down the robot when approaching DOF limits. For instance, it may slow down a foot motion when approaching the knee singularity, despite the robot being able to move faster with a fully extended knee.

add(task)

Add a new task to the IK solver.

Parameters:task (Task) – New task to add to the list.
build_qp_matrices(dt)

Build matrices of the quatratic program.

Parameters:dt (scalar) – Time step in [s].
Returns:
  • P ((n, n) array) – Positive semi-definite cost matrix.
  • q (array) – Cost vector.
  • qd_max (array) – Maximum joint velocity vector.
  • qd_min (array) – Minimum joint velocity vector.

Notes

When the robot model has joint acceleration limits, special care should be taken when computing the corresponding velocity bounds for the IK. In short, the robot now needs to avoid the velocity range where it (1) is not going to collide with a DOF limit in one iteration but (2) cannot brake fast enough to avoid a collision in the future due to acceleration limits. This function implements the solution to this problem described in Equation (14) of [Flacco15].

clear()

Clear all tasks in the IK solver.

compute_cost(dt)

Compute the IK cost of the present system state for a time step of dt.

Parameters:dt (scalar) – Time step in [s].
compute_velocity(dt)

Compute a new velocity satisfying all tasks at best.

Parameters:dt (scalar) – Time step in [s].
Returns:qd – Vector of active joint velocities.
Return type:array

Note

This QP formulation is the default for pymanoid.ik.IKSolver.solve() (posture generation) as it converges faster.

Notes

The method implemented in this function is reasonably fast but may become unstable when some tasks are widely infeasible. In such situations, you can either increase the Levenberg-Marquardt bias self.lm_damping or set slack_dof_limits=True which will call pymanoid.ik.IKSolver.compute_velocity_with_slack().

The returned velocity minimizes squared residuals as in the weighted cost function, which corresponds to the Gauss-Newton algorithm. Indeed, expanding the square expression in cost(task, qd) yields

\[\mathrm{minimize} \ \dot{q} J^T J \dot{q} - 2 r^T J \dot{q}\]

Differentiating with respect to \(\dot{q}\) shows that the minimum is attained for \(J^T J \dot{q} = r\), where we recognize the Gauss-Newton update rule.

compute_velocity_with_slack(dt)

Compute a new velocity satisfying all tasks at best, while trying to stay away from kinematic constraints.

Parameters:dt (scalar) – Time step in [s].
Returns:qd – Vector of active joint velocities.
Return type:array

Note

This QP formulation is the default for pymanoid.ik.IKSolver.step() as it has a more numerically-stable behavior.

Notes

Check out the discussion of this method around Equation (10) of [Nozawa16]. DOF limits are better taken care of by slack variables, but the variable count doubles and the QP takes roughly 50% more time to solve.

on_tick(sim)

Step the IK at each simulation tick.

Parameters:sim (Simulation) – Simulation instance.
print_costs(qd, dt)

Print task costs for the current IK step.

Parameters:
  • qd (array) – Robot DOF velocities.
  • dt (scalar) – Timestep for the IK.
remove(ident)

Remove a task.

Parameters:ident (string or object) – Name or object with a name field identifying the task.
set_active_dofs(active_dofs)

Set DOF indices modified by the IK.

Parameters:active_dofs (list of integers) – List of DOF indices.
set_gains(gains)

Set task gains from a dictionary.

Parameters:gains (string -> double dictionary) – Dictionary mapping task labels to default gain values.
set_weights(weights)

Set task weights from a dictionary.

Parameters:weights (string -> double dictionary) – Dictionary mapping task labels to default weight values.
solve(max_it=1000, cost_stop=1e-10, impr_stop=1e-05, dt=0.01, warm_start=False, debug=False)

Compute joint-angles that satisfy all kinematic constraints at best.

Parameters:
  • max_it (integer) – Maximum number of solver iterations.
  • cost_stop (scalar) – Stop when cost value is below this threshold.
  • impr_stop (scalar, optional) – Stop when cost improvement (relative variation from one iteration to the next) is less than this threshold.
  • dt (scalar, optional) – Time step in [s].
  • warm_start (bool, optional) – Set to True if the current robot posture is a good guess for IK. Otherwise, the solver will start by an exploration phase with DOF velocity limits relaxed and no Levenberg-Marquardt damping.
  • debug (bool, optional) – Set to True for additional debug messages.
Returns:

  • nb_it (int) – Number of solver iterations.
  • cost (scalar) – Final value of the cost function.

Notes

Good values of dt depend on the weights of the IK tasks. Small values make convergence slower, while big values make the optimization unstable (in which case there may be no convergence at all).

step(dt)

Apply velocities computed by inverse kinematics.

Parameters:dt (scalar) – Time step in [s].

Stance

The most common IK tasks for humanoid locomotion are the COM task and end-effector pose task. The Stance class avoids the hassle of creating and adding these tasks one by one. To use it, first create your targets:

com_target = robot.get_com_point_mass()
lf_target = robot.left_foot.get_contact(pos=[0, 0.3, 0])
rf_target = robot.right_foot.get_contact(pos=[0, -0.3, 0])

Then create the stance and bind it to the robot:

stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target)
stance.bind(robot)

Calling the bind function will automatically add the corresponding tasks to the robot IK solver. See the inverse_kinematics.py example for more details.

class pymanoid.stance.Stance(com, left_foot=None, right_foot=None, left_hand=None, right_hand=None)

Set of IK tasks for the humanoid’s center of mass (COM) and end effectors.

Parameters:
  • com (PointMass) – Center of mass target.
  • left_foot (Contact, optional) – Left-foot contact target.
  • right_foot (Contact, optional) – Right-foot contact target.
  • left_hand (Contact, optional) – Left-hand contact target.
  • right_hand (Contact, optional) – Right-hand contact target.
bind(robot, reg='posture')

Bind stance as robot IK targets.

Parameters:
  • robot (pymanoid.Robot) – Target robot.
  • reg (string, optional) – Regularization task, either “posture” or “min_vel”.
bodies

List of end-effector and COM bodies.

compute_pendular_accel_cone(com_vertices=None, zdd_max=None, reduced=False)

Compute the pendular COM acceleration cone of the stance.

The pendular cone is the reduction of the Contact Wrench Cone when the angular momentum at the COM is zero.

Parameters:
  • com_vertices (list of (3,) arrays, optional) – Vertices of a COM bounding polytope.
  • zdd_max (scalar, optional) – Maximum vertical acceleration in the output cone.
  • reduced (bool, optional) – If True, returns the reduced 2D form rather than a 3D cone.
Returns:

vertices – List of 3D vertices of the (truncated) COM acceleration cone, or of the 2D vertices of the reduced form if reduced is True.

Return type:

list of (3,) arrays

Notes

The method is based on a rewriting of the CWC formula, followed by a 2D convex hull on dual vertices. The algorithm is described in [Caron16].

When com is a list of vertices, the returned cone corresponds to COM accelerations that are feasible from all COM located inside the polytope. See [Caron16] for details on this conservative criterion.

compute_static_equilibrium_polygon(method='hull')

Compute the halfspace and vertex representations of the static-equilibrium polygon (SEP) of the stance.

Parameters:method (string, optional) – Which method to use to perform the projection. Choices are ‘bretl’, ‘cdd’ and ‘hull’ (default).
compute_zmp_support_area(height, method='bretl')

Compute an extension of the (pendular) multi-contact ZMP support area with optional pressure limits on each contact.

Parameters:
  • height (array, shape=(3,)) – Height at which the ZMP support area is computed.
  • method (string, default='bretl') – Polytope projection algorithm, can be "bretl" or "cdd".
Returns:

vertices – Vertices of the ZMP support area.

Return type:

list of arrays

Notes

There are two polytope projection algorithms: ‘bretl’ is adapted from in [Bretl08] while ‘cdd’ corresponds to the double-description formulation from [Caron17z]. See the Appendix from [Caron16] for a performance comparison.

contacts

List of active contacts.

dist_to_sep_edge(com)

Algebraic distance of a COM position to the edge of the static-equilibrium polygon.

Parameters:com (array, shape=(3,)) – COM position to evaluate the distance from.
Returns:dist – Algebraic distance to the edge of the polygon. Inner points get a positive value, outer points a negative one.
Return type:scalar
find_static_supporting_wrenches()

Find supporting contact wrenches in static-equilibrium.

Returns:support – Mapping between each contact i in the contact set and a supporting contact wrench \(w^i_{C_i}\).
Return type:list of (Contact, array) couples
free_contact(effector_name)

Free a contact from the stance and get the corresponding end-effector target. Its task stays in the IK, but the effector is not considered in contact any more (e.g. for contact wrench computations).

Parameters:effector_name (string) – Name of end-effector target, for example “left_foot”.
Returns:effector – IK target contact.
Return type:pymanoid.Contact
static from_json(path)

Create a new stance from a JSON file.

Parameters:path (string) – Path to the JSON file.
hide()

Hide end-effector and COM body handles.

load(path)

Load stance from JSON file.

Parameters:path (string) – Path to the JSON file.
nb_contacts

Number of active contacts.

save(path)

Save stance into JSON file.

Parameters:path (string) – Path to JSON file.
set_contact(effector)

Set contact on a effector.

Parameters:effector (pymanoid.Contact) – IK target contact.
show()

Show end-effector and COM body handles.

Example

In this example, we will see how to put the humanoid robot model in a desired configuration. Let us initialize a simulation with a 30 ms timestep and load the JVRC-1 humanoid:

sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()  # open GUI window

We define targets for foot contacts;

lf_target = Contact(robot.sole_shape, pos=[0, 0.3, 0], visible=True)
rf_target = Contact(robot.sole_shape, pos=[0, -0.3, 0], visible=True)

Next, let us set the altitude of the robot’s free-flyer (attached to the waist link) 80 cm above contacts. This is useful to give the IK solver a good initial guess and avoid coming out with a valid but weird solution in the end.

robot.set_dof_values([0.8], dof_indices=[robot.TRANS_Z])

This being done, we initialize a point-mass that will serve as center-of-mass (COM) target for the IK. Its initial position is set to robot.com, which will be roughly 80 cm above contacts as it is close to the waist link:

com_target = PointMass(pos=robot.com, mass=robot.mass)

All our targets being defined, we initialize IK tasks for the feet and COM position, as well as a posture task for the (necessary) regularization of the underlying QP problem:

lf_task = ContactTask(robot, robot.left_foot, lf_target, weight=1.)
rf_task = ContactTask(robot, robot.right_foot, rf_target, weight=1.)
com_task = COMTask(robot, com_target, weight=1e-2)
reg_task = PostureTask(robot, robot.q, weight=1e-6)

We initialize the robot’s IK using all DOFs, and insert our tasks:

robot.init_ik(active_dofs=robot.whole_body)
robot.ik.add_task(lf_task)
robot.ik.add_task(rf_task)
robot.ik.add_task(com_task)
robot.ik.add_task(reg_task)

We can also throw in some extra DOF tasks for a nicer posture:

robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5))
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5))

Finally, call the IK solver to update the robot posture:

robot.ik.solve(max_it=100, impr_stop=1e-4)

The resulting posture looks like this:

_images/inverse_kinematics.png

Robot posture found by inverse kinematics.

Alternatively, rather than creating and adding all tasks one by one, we could have used the Stance interface:

stance = Stance(com=com_target, left_foot=lf_target, right_foot=rf_target)
stance.bind(robot)
robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, gain=0.5, weight=1e-5))
robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, +0.5, gain=0.5, weight=1e-5))
robot.ik.solve(max_it=100, impr_stop=1e-4)

This code is more concise and yields the same result.