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 coordinatesinertia 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’sMass
structureRobotLink.setMass(mass)
: sets the link’sMass
structureRigidObjectModel.getMass()
: returns the object’sMass
structureRigidObjectModel.setMass(mass)
: sets the object’sMass
structure
Mass
structure
mass.get/setMass(m)
: get/sets total mass to a floatmass.get/setCom(com)
: get/sets center of mass to a float 3-tuple in link-local coordinatesmass.getInertia(inertia)
: gets inertia as a 9-vector, indicating rows of the inertia matrix in link-local coordinatesmass.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\)
Where:
\(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 floatsrobot.getMassMatrix()
: returns nxn array (n nested length-n lists) describing the mass matrix B at the robot’s currentConfig
robot.getMassMatrixInv()
: returns nxn array (n nested length-n lists) describing the inverse mass matrix B-1 at the robot’s currentConfig
robot.getCoriolisForceMatrix()
: returns nxn array (n nested length-n lists) describing the Coriolis force matrix such that at the robot’s currentConfig
/Velocity
robot.getCoriolisForces()
: returns the length-n list giving the Coriolis torques at the robot’s currentConfig
/Velocity
robot.getGravityForces(gravity)
: returns the length-n list giving the generalized gravity G at the robot’s currentConfig
, 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 currentConfig
/Velocity
robot.accelFromTorques(torques)
: solves for the forward dynamics, returning the length-n list of joint accelerations produced by the torques torques at the currentConfig
/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 callingcomEqulibrium
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 ofHold
s.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. Hold
s may be saved and loaded
from disk.
A Stance
defines all contact constraints
of a robot. It is defined simply as a list of Hold
s.