Dynamics and contact mechanics

Robot Dynamic Parameters

Each body contains inertial properties, which are collectively set referred to as a Mass object.

  • mass (float)
  • center of mass (Vector3) given in local coordinates
  • inertia matrix (Matrix3) \(H_L\) given in local coordinates

Robot (.rob) and Rigid Object (.obj) files can automatically infer inertial properties from link masses using the automass line.

API summary

  • RobotLink.getMass(): returns the link’s Mass structure
  • RobotLink.setMass(mass): sets the link’s Mass structure
  • RigidObjectModel.getMass(): returns the object’s Mass structure
  • RigidObjectModel.setMass(mass): sets the object’s Mass structure

Mass structure

  • mass.get/setMass(m): get/sets total mass to a float
  • mass.get/setCom(com): get/sets center of mass to a float 3-tuple in link-local coordinates
  • mass.getInertia(inertia): gets inertia as a 9-vector, indicating rows of the inertia matrix in link-local coordinates
  • mass.setInertia(inertia): sets inertia, either a float, float 3-tuple, or float 9-tuple. If float, sets inertia matrix to \(H_L=I_{3x3} \cdot\) inertia. If 3-tuple, sets matrix to \(H_L=diag\) (inertia)

Robot Dynamics

The fundamental Langrangian mechanics equation is

\(B(q)\ddot{q}+C(q,\dot{q})+G(q) = \tau + \sum_i J_i(q)^T f_i\)


  • \(q\) is configuration,
  • \(\dot{q}\) is velocity,
  • \(\ddot{q}\) is acceleration,
  • \(B(q)\) is the positive semidefinite mass matrix,
  • \(C(q,\dot{q})\) is the Coriolis force,
  • \(G(q)\) is the generalized gravity,
  • \(\tau\) is the link torque,
  • \(f_i\) are external forces, and
  • \(J_i(q)\) are the Jacobians of the points at which the points are applied.

A robot’s motion under given torques and external forces can be computed by determining the acceleration \(\ddot{q}\) by multiplying both sides by \(B^{-1}\) and integrating the equation forward in time. This is known as forward dynamics.

The reverse operation, determining a required right-hand-side (torques + forces) to effect a given acceleration, is known as inverse dynamics

API summary

The RobotModel class can compute each of these items using the Newton-Euler method:

  • robot.getCom(): returns robot’s center of mass as a 3-tuple of floats
  • robot.getMassMatrix(): returns nxn array (n nested length-n lists) describing the mass matrix B at the robot’s current Config
  • robot.getMassMatrixInv(): returns nxn array (n nested length-n lists) describing the inverse mass matrix B-1 at the robot’s current Config
  • robot.getCoriolisForceMatrix(): returns nxn array (n nested length-n lists) describing the Coriolis force matrix such that at the robot’s current Config / Velocity
  • robot.getCoriolisForces(): returns the length-n list giving the Coriolis torques at the robot’s current Config / Velocity
  • robot.getGravityForces(gravity): returns the length-n list giving the generalized gravity G at the robot’s current Config, given the 3-tuple gravity vector (usually (0,0,-9.8))
  • robot.torquesFromAccel(ddq): solves for the inverse dynamics, returning the length-n list of joint torques that would produce the acceleration ddq at the current Config/Velocity
  • robot.accelFromTorques(torques): solves for the forward dynamics, returning the length-n list of joint accelerations produced by the torques torques at the current Config/Velocity

Using the Featherstone algorithm, forward and inverse dynamics take O(n) time. Similarly, retrieving the COM, generalized gravity vector, and Coriolis force vector are O(n). The other methods are \(O(n^2)\), which is optimal.

Contact mechanics

Klamp’t supports several operations for working with contacts. Currently these support legged locomotion more conveniently than object manipulation, because the manipulated object must be defined as part of the robot, and robot-object contact is considered self-contact.

API summary

These routines can be found in klampt.model.contact which are thin wrappers around the underlying C++ functions.

  • ContactPoint: either a frictionless or frictional point contact. Consist of a position, normal, and coefficient of friction. May also refer to which objects are in contact.
  • forceClosure tests whether a given set of contacts is in force closure.
  • comEquilibrium tests whether the center of mass of a rigid body can be stably supported against gravity by valid contact forces at the given contact list.
  • supportPolygon computes a support polygon for a given contact list. Testing the resulting boundaries of the support polygon is much faster than calling comEqulibrium multiple times.
  • equilibriumTorques solves for equilibrium of an articulated robot under gravity and torque constraints. It can handle both statically balanced and dynamically moving robots.

Holds, Stances, and Grasps

The contact state of a single link, or a related set of links, is modeled with three higher-level concepts.

  • Hold: a set of contacts of a link against the environment and are used for locomotion planning.
  • Stance: a set of Holds.
  • Grasp: not available in the Python API at the moment.

A Hold is defined as a set of contacts (the contacts member) and the associated IK constraint (the ikConstraint member) that keeps a link on the robot placed at those contacts. These contacts are considered fixed in the world frame. Holds may be saved and loaded from disk.

A Stance defines all contact constraints of a robot. It is defined simply as a list of Holds.