Inverse Dynamics with Rigid Contact and Friction

Inverse Dynamics with Rigid Contact and Friction

Samuel Zapolsky Samuel Zapolsky George Washington University
Dept. of Computer Science
samzapo@gwu.edu Evan Drumwright George Washington University
Dept. of Computer Science
drum@gwu.edu
   Evan Drumwright Samuel Zapolsky George Washington University
Dept. of Computer Science
samzapo@gwu.edu Evan Drumwright George Washington University
Dept. of Computer Science
drum@gwu.edu
Received: date / Accepted: date
Abstract

Inverse dynamics is used extensively in robotics and biomechanics applications. In manipulator and legged robots, it can form the basis of an effective nonlinear control strategy by providing a robot with both accurate positional tracking and active compliance. In biomechanics applications, inverse dynamics control can approximately determine the net torques applied at anatomical joints that correspond to an observed motion.

In the context of robot control, using inverse dynamics requires knowledge of all contact forces acting on the robot; accurately perceiving external forces applied to the robot requires filtering and thus significant time delay. An alternative approach has been suggested in recent literature: predicting contact and actuator forces simultaneously under the assumptions of rigid body dynamics, rigid contact, and friction. Existing such inverse dynamics approaches have used approximations to the contact models, which permits use of fast numerical linear algebra algorithms. In contrast, we describe inverse dynamics algorithms that are derived only from first principles and use established phenomenological models like Coulomb friction.

We assess these inverse dynamics algorithms in a control context using two virtual robots: a locomoting quadrupedal robot and a fixed-based manipulator gripping a box while using perfectly accurate sensor data from simulation. The data collected from these experiments gives an upper bound on the performance of such controllers in situ. For points of comparison, we assess performance on the same tasks with both error feedback control and inverse dynamics control with virtual contact force sensing.

Keywords:
inverse dynamics, rigid contact, impact, Coulomb friction
\tcbuselibrary

most \newtcolorboxredboxcolframe=red,enhanced,breakable,hbox \newtcolorboxblueboxcolframe=blue,enhanced,breakable

1 Introduction

Inverse dynamics can be a highly effective method in multiple contexts including control of robots and physically simulated characters and estimating muscle forces in biomechanics. The inverse dynamics problem, which entails computing actuator (or muscular) forces that yield desired accelerations, requires knowledge of all “external” forces acting on the robot, character, or human. Measuring such forces is limited by the ability to instrument surfaces and filter force readings, and such filtering effectively delays force sensing to a degree unacceptable for real-time operation. An alternative is to use contact force predictions, for which reasonable agreement between models and reality have been observed (see, e.g.Aukes & Cutkosky, 2013). Formulating such approaches is technically challenging, however, because the actuator forces are generally coupled to the contact forces, requiring simultaneous solution.

Inverse dynamics approaches that simultaneously compute contact forces exist in literature. Though these approaches were developed without incorporating all of the established modeling aspects (like complementarity) and addressing all of the technical challenges (like inconsistent configurations) of hard contact, these methods have been demonstrated performing effectively on real robots. In contrast, this article focuses on formulating inverse dynamics with these established modeling aspects—which allows forward and inverse dynamics models to match—and addresses the technical challenges, including solvability.

1.1 Invertibility of the rigid contact model

An obstacle to such a formulation has been the claim that the rigid contact model is not invertible (Todorov, 2014), implying that inverse dynamics is unsolveable for multi-rigid bodies subject to rigid contact. If forces on the multi-body other than contact forces at state are designated and contact forces are designated , then the rigid contact model (to be described in detail in Section 2.3.3) yields the relationship . It is then true that there exists no left inverse of that provides the mapping for . However, this article will show that there does exist a right inverse of such that, for , , and in Section 5 we will show that this mapping is computable in expected polynomial time. This article will use this mapping to formulate inverse dynamics approaches for rigid contact with both no-slip constraints and frictional surface properties.

1.2 Indeterminacy in the rigid contact model

The rigid contact model is also known to be susceptible to the problem of contact indeterminacy, the presence of multiple equally valid solutions to the contact force-acceleration mapping. This indeterminacy is the factor that prevents strict invertibility and which, when used for contact force predictions in the context of inverse dynamics, can result in torque chatter that is potentially destructive for physically situated robots. We show that computing a mapping from accelerations to contact forces that evolves without harmful torque chatter is no worse than NP-hard in the number of contacts modeled for Coulomb friction and can be calculated in polynomial time for the case of infinite (no-slip) friction.

This article also describes a computationally tractable approach for mitigating torque chatter that is based upon a rigid contact model without complementarity conditions (see Sections 2.3.3 and 2.3.4). The model appears to produce reasonable predictions: Anitescu (2006); Drumwright & Shell (2010); Todorov (2014) have all used the model within simulation and physical artifacts have yet to become apparent.

We will assess these inverse dynamics algorithms in the context of controlling a virtual locomoting robot and a fixed-base manipulator robot. We will examine performance of error feedback and inverse dynamics controllers with virtual contact force sensors for points of comparison. Performance will consider smoothness of torque commands, trajectory tracking accuracy, locomotion performance, and computation time.

1.3 Contributions

This paper provides the following contributions:

  1. Proof that the coupled problem of computing inverse dynamics-derived torques and contact forces under the rigid body dynamics, non-impacting rigid contact, and Coulomb friction models (with linearized friction cone) is solvable in expected polynomial time.

  2. An algorithm that computes inverse dynamics-derived torques without torque chatter under the rigid body dynamics model and the rigid contact model assuming no slip along the surfaces of contact, in expected polynomial time.

  3. An algorithm that yields inverse dynamics-derived torques without torque chatter under the rigid body dynamics model and the rigid, non-impacting contact model with Coulomb friction in exponential time in the number of points of contact, and hence a proof that this problem is no harder than NP-hard.

  4. An algorithm that computes inverse dynamics-derived torques without torque chatter under the rigid body dynamics model and a rigid contact model with Coulomb friction but does not enforce complementarity conditions (again, see Sections 2.3.3 and 2.3.4), in expected polynomial time.

These algorithms differ in their operating assumptions. For example, the algorithms that enforce normal complementarity (to be described in Section 2.3) assume that all contacts are non-impacting; similarly, the algorithms that do not enforce complementarity assume that bodies are impacting at one or more points of contact. As will be explained in Section 3, control loop period endpoint times do not necessarily coincide with contact event times, so a single algorithm must deal with both impacting and non-impacting contacts. It is an open problem of the effects of enforcing complementarity when it should not be enforced, or vice versa. The algorithms also possess various computational strengths. As results of these open problems and varying computational strengths, we present multiple algorithms to the reader as well as a guide (see Appendix A) that details task use cases for these controllers.

This work validates controllers based upon these approaches in simulation to determine their performance under a variety of measurable phenomena that would not be accessible on present day physically situated robot hardware. Experiments indicating performance differentials on present day (prototype quality) hardware might occur due to control forces exciting unmodeled effects, for example. Results derived from simulation using ideal sensing and perfect torque control indicate the limit of performance for control software described in this work. We also validate one of the more robust presented controllers on a fixed-base manipulator robot grasping task to demonstrate operation under disparate morphological and contact modeling assumptions.

1.4 Contents

Section 2 describes background in rigid body dynamics and rigid contact, as well as related work in inverse dynamics with contact and friction. We then present the implementation of three disparate inverse dynamics formulations in Sections 4, 5, and 6. With each implementation, we seek to: \1 successfully control a robot through its assigned task; \2 mitigate torque chatter from indeterminacy; \3 evenly distribute contact forces between active contacts; \4 speed computation so that the implementation can be run at realtime on standard hardware. Section 4 presents an inverse dynamics formulation with contact force prediction that utilizes the non-impacting rigid contact model (to be described in Section 2.3) with no-slip frictional constraints. Section 5 presents an inverse dynamics formulation with contact force prediction that utilizes the non-impacting rigid contact model with Coulomb friction constraints. We show that the problem of mitigating torque chatter from indeterminate contact configurations is no harder than NP-hard. Section 6 presents an inverse dynamics formulation that uses a rigid impact model and permits the contact force prediction problem to be convex. This convexity will allow us to mitigate torque chatter from indeterminacy.

Section 7 describes experimental setups for assessing the inverse dynamics formulations in the context of simulated robot control along multiple dimensions: accuracy of trajectory tracking; contact force prediction accuracy; general locomotion stability; and computational speed. Tests are performed on terrains with varied friction and compliance. Presented controllers are compared against both PID control and inverse dynamics control with sensed contact and perfectly accurate virtual sensors. Assessment under both rigid and compliant contact models permits both exact and in-the-limit verification that controllers implementing these inverse dynamics approaches for control work as expected. These experiments also examine behavior when modeling assumptions break down. Section 8 analyzes the findings from these experiments.

1.5 The multi-body

This paper centers around a multi-body, which is the system of rigid bodies to which inverse dynamics is applied. The multi-body may come into contact with “fixed” parts of the environment (e.g., solid ground) which are sufficiently modeled as non-moving bodies— this is often the case when simulating locomotion. Alternatively, the multi-body may contact other bodies, in which case effective inverse dynamics will require knowledge of those bodies’ kinematic and dynamic properties — necessary for manipulation tasks.

The articulated body approach can be extended to a multi-body to account for physically interacting with movable rigid bodies by appending the six degree-of-freedom velocities () and external wrenches () of each contacted rigid body to the velocity and external force vectors and by augmenting the generalized inertia matrix () similarly:

(1)
(2)
(3)

Without loss of generality, our presentation will hereafter consider only a single multi-body in contact with a static environment (excepting an example with a manipulator arm grasping a box in Section 7).

2 Background and related work

This section surveys the independent parts that are combined to formulate algorithms for calculating inverse dynamics torques with simultaneous contact force computation. Section 2.1 discusses complementarity problems, a domain outside the purview of typical roboticists. Section 2.2 introduces the rigid body dynamics model for Newtonian mechanics under generalized coordinates. Section 2.3 covers the rigid contact model, and unilaterally constrained contact. Sections 2.3.12.3.3 show how to formulate constraints on the rigid contact model to account for Coulomb friction and no-slip constraints. Section 2.3.4 describe an algebraic impact model that will form the basis of one of the inverse dynamics methods. Section 2.4 describes the phenomenon of “indeterminacy” in the rigid contact model. Lastly, Sections 2.5 and 2.6 discusses other work relevant to inverse dynamics with simultaneous contact force computation.

2.1 Complementarity problems

Complementarity problems are a particular class of mathematical programming problems often used to model hard and rigid contacts. A nonlinear complementarity problem (NCP) is composed of three nonlinear constraints (Cottle et al., 1992), which taken together constitute a complementarity condition:

(4)
(5)
(6)

where and . Henceforth, we will use the following shorthand to denote a complementarity constraint:

(7)

which signifies that and .

A LCP, or linear complementarity problem (), where and , is the linear version of this problem:

for unknowns .

Theory of LCPs has been established to a greater extent than for NCPs. For example, theory has indicated certain classes of LCPs that are solvable, which includes both determining when a solution does not exist and computing a solution, based on properties of the matrix (above). Such classes include positive definite matrices, positive semi-definite matrices, -matrices, and -matrices, to name only a few; (Murty, 1988; Cottle et al., 1992) contain far more information on complementarity problems, including algorithms for solving them. Given that the knowledge of NCPs (including algorithms for solving them) is still relatively thin, this article will relax NCP instances that correspond to contacting bodies to LCPs using a common practice, linearizing the friction cone.

Duality theory in optimization establishes a correspondence between LCPs and quadratic programs (see Cottle et al., 1992, Pages 4 and 5) via the Karush-Kuhn-Tucker conditions; for example, positive semi-definite LCPs are equivalent to convex QPs. Algorithms for quadratic programs (QPs) can be used to solve LCPs, and vice versa.

2.2 Rigid body dynamics

The multi rigid body dynamics (generalized Newton) equation governing the dynamics of a robot undergoing contact can be written in its generalized form as:

(8)

Equation 8 introduces the variables (“external”, non-actuated based, forces on the degree-of-freedom multibody, like gravity and Coriolis forces); (contact forces); unknown actuator torques ( is the number of actuated degrees of freedom); and a binary selection matrix . If all of the degrees-of-freedom of the system are actuated will be an identity matrix. For, e.g., legged robots, some variables in the system will correspond to unactuated, “floating base” degrees-of-freedom (DoF); the corresponding columns of the binary matrix will be zero vectors, while every other column will possess a single “1”.

2.3 Rigid contact model

This section will summarize existing theory of modeling non-impacting rigid contact and draws from Stewart & Trinkle (1996); Trinkle et al. (1997); Anitescu & Potra (1997). Let us define a set of gap functions (for ), where gap function gives the signed distance between a link of the robot and another rigid body (part of the environment, another link of the robot, an object to be manipulated, etc.)

Our notation assumes independent coordinates (and velocities and accelerations ), and that generalized forces and inertias are also given in minimal coordinates.

Figure 1: The contact frame consisting of , , and , vectors corresponding to the normal, first tangential, and second tangential directions (for 3D) to the contact surface.

The gap functions return a positive real value if the bodies are separated, a negative real value if the bodies are geometrically intersecting, and zero if the bodies are in a “kissing” configuration. The rigid contact model specifies that bodies never overlap, i.e.:

(9)

One or more points of contact between bodies is determined for two bodies in a kissing configuration (). For clarity of presentation, we will assume that each gap function corresponds to exactly one point of contact (even for disjoint bodies), so that . In the absence of friction, the constraints on the gap functions are enforced by forces that act along the contact normal. Projecting these forces along the contact normal yields scalars . The forces should be compressive (i.e., forces that can not pull bodies together), which is denoted by restricting the sign of these scalars to be non-negative:

(10)

A complementarity constraint keeps frictional contacts from doing work: when the constraint is inactive () no force is applied and when force is applied, the constraint must be active (). This constraint is expressed mathematically as . All three constraints can be combined into one equation using the notation in Section 2.1:

(11)

These constraints can be differentiated with respect to time to yield velocity-level or acceleration-level constraints suitable for expressing the differential algebraic equations (DAEs), as an index 1 DAE:

(12)
(13)

2.3.1 Modeling Coulomb friction

Dry friction is often simulated using the Coulomb model, a relatively simple, empirically derived model that yields the approximate outcome of sophisticated physical interactions. Coulomb friction covers two regimes: sticking/rolling friction (where the tangent velocity at a point of contact is zero) and sliding friction (nonzero tangent velocity at a point of contact). Rolling friction is distinguished from sticking friction by whether the bodies are moving relative to one another other than at the point of contact.

There are many phenomena Coulomb friction does not model, including “drilling friction” (it is straightforward to augment computational models of Coulomb friction to incorporate this feature, as seen in Leine & Glocker, 2003), the Stribeck effect (Stribeck, 1902), and viscous friction, among others. This article focuses only on Coulomb friction, because it captures important stick/slip transitions and uses only a single parameter; the LuGRe model (Do et al., 2007), for example, is governed by seven parameters, making system identification tedious.

Coulomb friction uses a unitless friction coefficient, commonly denoted . If we define the tangent velocities and accelerations in 3D frames (located at the point of contact) as and , respectively, and the tangent forces as and , then the sticking/rolling constraints which are applicable exactly when , can be expressed via the Fritz-John optimality conditions (Mangasarian & Fromovitz, 1967; Trinkle et al., 1997):

(14)
(15)
(16)

These conditions ensure that the friction force lies within the friction cone (Equation 14) and that the friction forces push against the tangential acceleration (Equations 1516).

In the case of sliding at the contact ( or ), the constraints become:

(17)
(18)
(19)

Note that this case is only applicable if , so there is no need to include such a constraint in Equation 17 (as was necessary in Equation 14).

The rigid contact model with Coulomb friction is subject to inconsistent configurations (Stewart, 2000a), exemplified by Painlevé’s Paradox (Painlevé, 1895), in which impulsive forces may be necessary to satisfy all constraints of the model even when the bodies are not impacting. The acceleration-level dynamics can be approximated using finite differences; a first order approximation is often used (see, e.g.Posa & Tedrake, 2012), which moves the problem to the velocity/impulsive force domain. Such an approach generally uses an algebraic collision law (see Chatterjee & Ruina, 1998) to model all contacts, both impacting, as inelastic impacts; typical “time stepping methods” (Moreau, 1983) for simulating dynamics often treat the generalized coordinates as constant over the small timespan of contact/impact (i.e., a first order approximation); see, e.g.Stewart & Trinkle (2000). Stewart has shown that this approximation converges to the solution of the continuous time formulation as the step size tends to zero (1998).

Upon moving to the velocity/impulsive force domain, Equations 1419 require a slight transformation to the equations:

(20)
(21)
(22)

and there is no longer separate consideration of sticking/rolling and slipping contacts.

2.3.2 No-slip constraints

If the Coulomb friction constraints are replaced by no-slip constraints, which is a popular assumption in legged locomotion research, one must also use the discretization approach; without permitting impulsive forces, slip can occur even with infinite friction (Lynch & Mason, 1995). The no-slip constraints are then simply (replacing Equations 2022).

2.3.3 Model for rigid, non-impacting contact with Coulomb friction

The model of rigid contact with Coulomb friction for two bodies in non-impacting rigid contact at can be summarized by the following equations:

(23)
(24)
(25)
(26)
(27)
(28)

where , , and are the signed magnitudes of the contact force applied along the normal and two tangent directions, respectively; is the relative acceleration of the bodies normal to the contact surface; and and are the relative velocities of the bodies projected along the two tangent directions. The operator indicates that , for vectors and . Detailed interpretation of these equations can be found in Trinkle et al. (1997); we present a summary below. Equation 23 ensures that \1 only compressive forces are applied (); \2 bodies cannot interpenetrate (); and \3 no work is done for frictionless contacts (). Equation 24 models Coulomb friction: either the velocity in the contact tangent plane is zero—which allows frictional forces to lie within the friction cone—or the contact is slipping and the frictional forces must lie strictly on the edge of the friction cone. Equations 25 and 26—applicable to sliding contacts (i.e., those for which or )—constrain friction forces to act against the direction of slip, while Equations 27 and 28 constrain frictional forces for rolling or sticking contacts (i.e., those for which ) to act against the direction of tangential acceleration.

These equations form a nonlinear complementarity problem (Cottle et al., 1992), and this problem may not possess a solution with nonimpulsive forces due to the existence of inconsistent configurations like Painlevé’s Paradox (Stewart, 2000b). This issue led to the movement to the impulsive force/velocity domain for modeling rigid contact, which can provably model the dynamics of such inconsistent configurations.

A separate issue with the rigid contact model is that of indeterminacy, where multiple sets of contact forces and possibly multiple sets of accelerations—or velocities, if an impulse-velocity model is used—can satisfy the contact model equations. Our inverse dynamics approaches, which use rigid contact models, address inconsistency and, given some additional computation, can address indeterminacy (useful for controlled systems).

2.3.4 Contacts without complementarity

Complementarity along the surface normal arises from Equation 11 for contacting rigid bodies that are not impacting. For impacting bodies, complementarity conditions are unrealistic (Chatterjee, 1999). Though the distinction between impacting and non-impacting may be clear in free body diagrams and symbolic mathematics, the distinction between the two modes is arbitrary in floating point arithmetic. This arbitrary distinction has led researchers in dynamic simulation, for example, to use one model—either with complementarity or without—for both impacting and non-impacting contact.

Anitescu (2006) described a contact model without complementarity (Equation 11) used for multi-rigid body simulation. Drumwright & Shell (2010) and Todorov (2014) rediscovered this model (albeit with slight modifications, addition of viscous friction, and guarantees of solution existence and non-negativity energy dissipation in the former); Drumwright & Shell (2010) also motivated acceptability of removing the complementarity condition based on the work by Chatterjee (1999). When treated as a simultaneous impact model, the model is consistent with first principles. Additionally, using arguments in Smith et al. (2012), it can be shown that solutions of this model exhibit symmetry. This impact model, under the assumption of inelastic impact—it is possible to model partially or fully elastic impact as well, but one must then consider numerous models of restitution, see, e.g.Chatterjee & Ruina (1998)—will form the basis of the inverse dynamics approach described in Section 6.

The model is formulated as the convex quadratic program below. For consistency of presentation with the non-impacting rigid model described in the previous section, only a single impact point is considered.

\tcbset

lefttitle=2mm,righttitle=1mm,left=2mm,title=Complementarity-free impact model (single point of contact) {tcolorbox}

(29)
subject to: (30)
(31)
(32)

where , , and are the signed magnitudes of the impulsive forces applied along the normal and two tangent directions, respectively; and are the generalized velocities of the multi-body immediately before and after impact, respectively; is the generalized inertia matrix of the degree of freedom multi-body; and , , and are generalized wrenches applied along the normal and two tangential directions at the point of contact (see Appendix B for further details on these matrices).

The physical interpretation of the impact model is straightforward: it selects impact forces that maximize the rate of kinetic energy dissipation. Finally, we note that rigid impact models do not enjoy the same degree of community consensus as the non-impacting rigid contact models because three types of impact models (algebraic, incremental, and full deformation) currently exist (Chatterjee & Ruina, 1998), because simultaneous impacts and impacts between multi-bodies can be highly sensitive to initial conditions (Ivanov, 1995), and because intuitive physical parameters for capturing all points of the feasible impulse space do not yet exist (Chatterjee & Ruina, 1998), among other issues. These difficulties lead this article to consider only inelastic impacts, a case for which the feasible impulse space is constrained.

2.4 Contact force indeterminacy

In previous work (Zapolsky et al., 2013), we found that indeterminacy in the rigid contact model can be a significant problem for controlling quadrupedal robots (and, presumably, hexapods, etc.) by yielding torques that switch rapidly between various actuators (torque chatter). The problem can occur in bipedal walkers; for example, Collins et al. (2001) observed instability from rigid contact indeterminacy in passive walkers. Even manipulators may also experience the phenomenon of rigid contact indeterminacy, indicated by torque chatter.

Rigid contact configurations can be indeterminate in terms of forces; for the example of a table with all four legs perfectly touching a ground plane, infinite enumerations of force configurations satisfy the contact model (as discussed in Mirtich, 1996), although the accelerations predicted by the model are unique. Other rigid contact configurations can be indeterminate in terms of predicting different accelerations/velocities through multiple sets of valid force configurations. We present two methods of mitigating indeterminacy in this article (see Sections 4.6 and 6.2.4). Defining a manner by which actuator torques evolve over time, or selecting a preferred distribution of contact forces may remedy the issues resulting from indeterminacy.

2.5 Contact models for inverse dynamics in the context of robot control

This section focuses on “hard”, by which we mean perfectly rigid, models of contact incorporated into inverse dynamics and whole body control for robotics. We are unaware of research that has attempted to combine inverse dynamics with compliant contact (one possible reason for absence of such work is that such compliant models can require significant parameter tuning for accuracy and to prevent prediction of large contact forces).

Mistry et al. (2010) developed a fast inverse dynamics control framework for legged robots in contact with rigid environments under the assumptions that feet do not slip. Righetti et al. (2013) extended this work with a framework that permits quickly optimizing a mixed linear/quadratic function of motor torques and contact forces using fast linear algebra techniques. Hutter & Siegwart (2012) also uses this formulation in an operational space control scheme, simplifying the contact mathematics by assuming contacts are sticking. Mistry et al.; Righetti et al.; Hutter & Siegwart demonstrate effective trajectory tracking performance on quadrupedal robots.

The inverse dynamics approach of Ames (2013) assumes sticking impact upon contact with the ground and immediate switching of support to the new contact, while enforcing a unilateral constraint of the normal forces and predicting no-slip frictional forces.

Kuindersma et al. (2014) use a no-slip constraint but allow for bounded violation of that constraint in order to avoid optimizing over an infeasible or inconsistent trajectory.

Stephens & Atkeson (2010) incorporate a contact model into an inverse dynamics formulation for dynamic balance force control. Their approach uses a quadratic program (QP) to estimate contact forces quickly on a simplified model of a bipedal robot’s dynamics. Newer work by Feng et al. (2013) builds on this by approximating the friction cone with a circumscribed friction pyramid.

Ott et al. (2011) also use an optimization approach for balance, modeling contact to distribute forces among a set of pre-defined contacts to enact a generalized wrench on a simplified model of a biped; their controller seeks to minimize the Euclidian norm of the predicted contact forces to mitigate slip. In underconstrained cases (where multiple solutions to the inverse dynamics with contact system exist), Saab et al. (2013) and Zapolsky et al. (2013) use a multi-phase QP formulation for bipeds and quadrupeds, respectively. Zapolsky et al. mitigates the indeterminacy in the rigid contact model by selecting a solution that minimizes total actuator torques, while Saab et al. use the rigid contact model in the context of cascades of QPs to perform several tasks in parallel (i.e., whole body control). The latter work primarily considers contacts without slip, but does describe modifications that would incorporate Coulomb friction (inconsistent and indeterminate rigid contact configurations are not addressed). Todorov (2014) uses the same contact model (to be described below) but without using a two-stage solution; that approach uses regularization to make the optimization problem strictly convex (yielding a single, globally optimal solution). None of Saab et al.; Zapolsky et al.; Todorov utilize the complementarity constraint (i.e., in Equation 11) in their formulation. Zapolsky et al. and Todorov motivate dropping this constraint in favor of maximizing energy dissipation through contact, an assumption that they show performs reasonably in practice (Drumwright & Shell, 2010; Todorov, 2011).

2.6 Contact models for inverse dynamics in the context of biomechanics

Inverse dynamics is commonly applied in biomechanics to determine approximate net torques at anatomical joints for observed motion capture and force plate data. Standard Newton-Euler inverse dynamics algorithms (as described in Featherstone, 2008) are applied; least squares is required because the problem is overconstrained. Numerous such approaches are found in biomechanics literature, including (Kuo, 1998; Hatze, 2002; Blajer et al., 2007; Bisseling & Hof, 2006; Yang et al., 2007; Van Den Bogert & Su, 2008; Sawers & Hahn, 2010). These force plate based approaches necessarily limit the environments in which the inverse dynamics computation can be conducted.

3 Discretized inverse dynamics

We discretize inverse dynamics because the resolution to rigid contact models both without slip and with Coulomb friction can require impulsive forces even when there are no impacts (see Section 2.3.1). This choice will imply that the dynamics are accurate to only first order, but that approximation should limit modeling error considerably for typical control loop rates (Zapolsky & Drumwright, 2015).

As noted above, dynamics are discretized using a first order approximation to acceleration. Thus, the solution to the equation of motion over is approximated by , where . We use the superscript “” to denote that a term is evaluated at and the superscript “” is applied to denote that a term is computed at . For example, the generalized inertia matrix has the superscript “” and the generalized post-contact velocity () has the superscript “”. We will hereafter adopt the convention that application of a superscript once will indicate implicit evaluation of that quantity at that time thereafter (unless another superscript is applied). For example, we will continue to treat as evaluated at in the remainder of this paper.

The remainder of this section describes how contact constraints should be determined for discretized inverse dynamics.

3.1 Incorporating contact into planned motion

First, we note that the inverse dynamics controller attempts to realize a planned motion. That planned motion must account for pose data and geometric models of objects in the robot’s environment. If planned motion is inconsistent with contact constraints, e.g., the robot attempts to push through a wall, undesirable behavior will clearly result. Obtaining accurate geometric data (at least for novel objects) and pose data are presently challenging problems; additional work in inverse dynamics control with predictive contact is necessary to address contact compliance and sensing uncertainty.

3.2 Incorporating contact constraints that do not coincide with control loop period endpoint times

Contact events—making or breaking contact, transitioning from sticking to sliding or vice versa—do not generally coincide with control loop period endpoint times. Introducing a contact constraint “early”, i.e., before the robot comes into contact with an object, will result in a poor estimate of the load on the robot (as the anticipated link-object reaction force will be absent). Introducing a contact constraint “late”, i.e., after the robot has already contacted the object, implies that an impact occurred; it is also likely that actuators attached to the contacted link and on up the kinematic chain are heavily loaded, resulting in possible damage to the robot, the environment, or both. Figure 2 depicts both of these scenarios for a walking bipedal robot.

Figure 2: If the contact constraint is introduced early (left figure, constraint depicted using dotted line) the anticipated load will be wrong. The biped will pitch forward, possibly falling over in this scenario. If the contact constraint is introduced late, an impact may occur while the actuators are loaded. The biped on the right is moving its right lower leg toward a foot placement; the impact as the foot touches down is prone to damaging the loaded powertrain.

We address this problem by borrowing a constraint stabilization (Ascher et al., 1995) approach from Anitescu & Hart (2004), which is itself a form of Baumgarte Stabilization (Baumgarte, 1972). Recalling that two bodies are separated by signed distance , constraints on velocities are determined such that .

To realize these constraints mathematically, we first convert Equation 12 to a discretized form:

(34)

This equation specifies that a force is to be found such that applying the force between one of the robot’s links and an object, already in contact at , over the interval yields a relative velocity indicating sustained contact or separation at . We next incorporate the signed distance between the bodies:

(35)

The removal of the conditional makes the constraint always active. Introducing a constraint of this form means that forces may be applied in some scenarios when they should not be (see Figure 3 for an example). Alternatively, constraints introduced before bodies contact can be contradictory, making the problem infeasible. Existing proofs for time stepping simulation approaches indicate that such issues disappear for sufficiently small integration steps (or, in the context of inverse dynamics, sufficiently high frequency control loops); see Anitescu & Hart (2004), which proves that such errors are uniformly bounded in terms of the size of the time step and the current value of the velocity.

Figure 3: An example of a contact constraint that overly constrains the motion between two disjoint bodies (the right foot and the skateboard). The contact constraint will keep the foot from moving below the dotted line. However, if the foot moves quickly downward and the skateboard moves quickly to the right (in time), inverse dynamics may predict, incorrectly, that a contact force will be applied to the foot. It should be apparent that these problems disappear as , i.e., as the control loop frequency becomes sufficiently high.

3.3 Computing points of contact between geometries

Given object poses data and geometric models, points of contact between robot links and environment objects can be computed using closest features. The particular algorithm used for computing closest features is dependent upon both the representation (e.g., implicit surface, polyhedron, constructive solid geometry) and the shape (e.g., sphere, cylinder, box). Algorithms and code can be found in sources like Ericson (2005) and http://geometrictools.com. Figure 4 depicts the procedure for determining contact points and normals for two examples: a sphere vs. a half-space and for a sphere vs. a sphere.

For disjoint bodies like those depicted in Figure 4, the contact point can be placed anywhere along the line segment connecting the closest features on the two bodies. Although the illustration depicts the contact point as placed at the midpoint of this line segment, this selection is arbitrary. Whether the contact point is located on the first body, on the second body, or midway between the two bodies, no formula is “correct” while the bodies are separated and every formula yields the same result when the bodies are touching.

Figure 4: Depiction of the manner of selecting points of contact and surface normals for disjoint bodies with spherical/half-space (left) and spherical/spherical geometries (right). Closest points on the objects are connected by dotted line segments. Surface normals are depicted with an arrow. Contact points are drawn as white circles with black outlines.

4 Inverse dynamics with no-slip constraints

Some contexts where inverse dynamics may be used (biomechanics, legged locomotion) may assume absence of slip (see, e.g.Righetti, et al., 2011; Zhao, et al., 2014). This section describes an inverse dynamics approach that computes reaction forces from contact using the non-impacting rigid contact model with no-slip constraints. Using no-slip constraints results in a symmetric, positive semidefinite LCP. Such problems are equivalent to convex QPs by duality theory in optimization (see Cottle et al., 1992), which implies polynomial time solvability. Convexity also permits mitigating indeterminate contact configurations, as will be seen in Section 4.6. This formulation inverts the rigid contact problem in a practical sense and is derived from first principles.

We present two algorithms in this section: Algorithm 1 ensures the no-slip constraints on the contact model are non-singular and thus guarantees that the inverse dynamics problem with contact is invertible; Algorithm 2 presents a method of mitigating torque chatter from indeterminate contact (for contexts of inverse dynamics based control) by warm-starting (Nocedal & Wright, 2006) the solution for the LCP solver with the last solution.

4.1 Normal contact constraints

The equation below extends Equation 12 to multiple points of contact (via the relationship ), where is the matrix of generalized wrenches along the contact normals (see Appendix B):

(36)

Because is clearly time-dependent and the control loop executes at discrete points in time, must be treated as constant over a time interval. indicates that points of contact are drawn from the current configuration of the environment and multi-body. Analogous to time-stepping approaches for rigid body simulations with rigid contact, all possible points of contact between rigid bodies over the interval and can be incorporated into as well: as in time stepping approaches for simulation, it may not be necessary to apply forces at all of these points (the approaches implicitly can treat unnecessary points of contact as inactive, though additional computation will be necessary). Stewart (1998) showed that such an approach will converge to the solution of the continuous time dynamics as . Given a sufficiently high control rate, will be small and errors from assuming constant over this interval should become negligible.

4.2 Discretized rigid body dynamics equation

The discretized version of Equation 8, now separating contact forces into normal ) and tangential wrenches ( and are matrices of generalized wrenches along the first and second contact tangent directions for all contacts) is:

(37)

Treating inverse dynamics at the velocity level is necessary to avoid the inconsistent configurations that can arise in the rigid contact model when forces are required to be non-impulsive (Stewart, 2000a, as also noted in Section 2.3.1). As noted above, Stewart has shown that for sufficiently small , converges to the solution of the continuous time dynamics and contact equations (1998).

4.3 Inverse dynamics constraint

The inverse dynamics constraint is used to specify the desired velocities only at actuated joints:

(38)

Desired velocities are calculated as:

(39)

4.4 No-slip (infinite friction) constraints

Utilizing the first-order discretization (revisit Section 2.3.2 to see why this is necessary), preventing tangential slip at a contact is accomplished by using the constraints:

(40)
(41)

These constraints indicate that the velocity in the tangent plane is zero at time ; we will find the matrix representation to be more convenient for expression as quadratic programs and linear complementarity problems than:

i.e., the notation used in Section 2.3.1. All presented equations are compiled below:

\tcbset

lefttitle=2mm,righttitle=1mm,left=2mm,title=Complementarity-based inverse dynamics without slip {tcolorbox}

Combining Equations 3638, 40, and 41 into a mixed linear complementarity problem (MLCP, see Appendix C) yields:

(42)
(43)

where . We define the MLCP block matrices—in the form of Equations 116119 from Appendix C—and draw from Equations 42 and 43 to yield:

Applying Equations 121 and 122 (again see Appendix C), we transform the MLCP to LCP . Substituting in variables from the no-slip inverse dynamics model and then simplifying yields:

(44)
(45)

The definition of matrix from above may be singular, which would prevent inversion, and thereby, conversion from the MLCP to an LCP. We defined as a selection matrix with full row rank, and the generalized inertia () is symmetric, positive definite. If and have full row rank as well, or we identify the largest subset of row blocks of and such that full row rank is attained, will be invertible as well (this can be seen by applying blockwise matrix inversion identities). Algorithm 1 performs the task of ensuring that matrix is invertible. Removing the linearly dependent constraints from the matrix does not affect the solubility of the MLCP, as proved in Appendix E.

From Bhatia (2007), a matrix of ’s form must be non-negative definite, i.e., either positive-semidefinite (PSD) or positive definite (PD). is the right product of with its transpose about a symmetric PD matrix, . Therefore, is symmetric and either PSD or PD.

The singularity check on Lines 6 and 10 of Algorithm 1 is most quickly performed using Cholesky factorization; if the factorization is successful, the matrix is non-singular. Given that is non-singular (it is symmetric, PD), the maximum size of in Algorithm 1 is ; if were larger, it would be singular.

The result is that the time complexity of Algorithm 1 is dominated by Lines 6 and 10. As changes by at most one row and one column per Cholesky factorization, singularity can be checked by updates to an initial Cholesky factorization. The overall time complexity is .

1:
2:
3:for  do is the number of contacts
4:     
5:     Set
6:     if  not singular then
7:               
8:     
9:     Set
10:     if  not singular then
11:               
12:return
Algorithm 1 Find-Indices(, determines the row indices (, and ) of and such that the matrix (Equation 116 in Appendix C) is non-singular.

4.5 Retrieving the inverse dynamics forces

Once the contact forces have been determined, one solves Equations 37 and 38 for , thereby obtaining the inverse dynamics forces. While the LCP is solvable, it is possible that the desired accelerations are inconsistent. As an example, consider a legged robot standing on a ground plane without slip (such a case is similar to, but not identical to infinite friction, as noted in Section 2.3.2), and attempting to splay its legs outward while remaining in contact with the ground. Such cases can be readily identified by verifying that . If this constraint is not met, consistent desired accelerations can be determined without re-solving the LCP. For example, one could determine accelerations that deviate minimally from the desired accelerations by solving a quadratic program:

(46)
subject to: (47)
(48)
(49)
(50)
(51)

This QP is always feasible: ensures that
, , and .

4.6 Indeterminacy mitigation

We warm start a pivoting LCP solver (see Appendix D) to bias the solver toward applying forces at the same points of contact (see Figure 4.6)—tracking points of contact using the rigid body equations of motion—as were active on the previous inverse dynamics call (see Algorithm 2). Kuindersma et al. (2014) also use warm starting to solve a different QP resulting from contact force prediction. However, we use warm starting to address indeterminacy in the rigid contact model while Kuindersma et al. use it to generally speed the solution process.

\tcbset

enhanced,attach boxed title to top center=yshift=-3mm,yshifttext=-1mm, colback=white,colbacktitle=black,fonttitle=, boxed title style=size=small,colframe=black,title=Figure 5: Warm-Starting Example {tcolorbox} Iteration Iteration Iteration

Left (“cold start”): with four active contacts, the pivoting solver chooses three arbitrary non-basic indices (in , see Appendix D) to solve the LCP and then returns the solution. The solution applies the majority of upward force to two feet and applies a smaller amount of force to the third.

Center (“warm start”): With four active contacts, the pivoting solver chooses the same three non-basic indices as the last solution to attempt to solve the LCP. The warm-started solution will distribute upward forces similarly to the last solution, tending to provide consecutive solves with continuity over time.

Right (“cold start”): one foot of the robot has broken contact with the ground; there are now three active contacts. The solver returns a new solution, applying the majority of normal force to two legs, and applying a small amount of force to the third.

Using warm starting, Algorithm 2 will find a solution that predicts contact forces applied at the exact same points on the last iteration assuming that such a solution exists. Such solutions do not exist \1 when the numbers and relative samples from the contact manifold change or \2 as contacts transition from active () to inactive (), or vice versa. Case \2 implies immediate precedence or subsequence of case \1, which means that discontinuities in actuator torques will occur for at most two control loop iterations around a contact change (one discontinuity will generally occur due to the contact change itself).

4.7 Scaling inverse dynamics runtime linearly in number of contacts

The multi-body’s number of generalized coordinates () are expected to remain constant. The number of contact points, , depends on the multi-body’s link geometries, the environment, and whether the inverse dynamics approach should anticipate potential contacts in (as discussed in Section 4.1). This section describes a method to solve inverse dynamics problems with simultaneous contact force computation that scales linearly with additional contacts. This method will be applicable to all inverse dynamics approaches presented in this paper except that described in Section 5: that problem results in a copositive LCP (Cottle et al., 1992) that the algorithm we describe cannot generally solve.

To this point in the article presentation, time complexity has been dominated by the expected time solution to the LCPs. However, a system with degrees-of-freedom requires no more than positive force magnitudes applied along the contact normals to satisfy the constraints for the no-slip contact model. Proof is provided in Appendix F. Below, we describe how that proof can be leveraged to generally decrease the expected time complexity.

Modified PPM I Algorithm

We now describe a modification to the Principal Pivoting Method I (Cottle et al., 1992) (PPM) for solving LCPs (see description of this algorithm in Appendix D) that leverages the proof in Appendix F to attain expected time complexity. A brief explanation of the mechanics of pivoting algorithms is provided in Appendix D; we use the common notation of as the set of basic variables and as the set of non-basic variables.

The PPM requires few modifications toward our purpose. These modifications are presented in Algorithm 2. First, the full matrix is never constructed, because the construction is unnecessary and would require time. Instead, Line 11 of the algorithm constructs a maximum system; thus, that operation requires only operations. Similarly, Lines 12 and 13 also leverage the proof from Appendix F to compute and efficiently (though these operations do not affect the asymptotic time complexity). Expecting that the number of iterations for a pivoting algorithm is in the size of the input (Cottle et al., 1992) and assuming that each iteration requires at most two pivot operations (each rank-1 update operation to a matrix factorization will exhibit time complexity), the asymptotic complexity of the modified PPM I algorithm is . The termination conditions for the algorithm are not affected by our modifications.

Finally, we note that Baraff (1994) has proven that LCPs of the form , where , and is symmetric, PD are solvable. Thus, the inverse dynamics model will always possess a solution.

1: rows()
2:
3: Check for trivial solution
4:if  then
5:      return 
6:
7:if  then
8:       Establish initial nonbasic indices
9: Establish basic indices
10:while true do
11:      
12:      
13:       Compute non-basic components
14:      
15:      
16:       Search for index to move into non-basic set
17:      if  then
18:             No index to move into the non-basic set; search for index to move into the basic set
19:            if  then
20:                 
21:                  Move index into basic set
22:                 
23:                 continue
24:            else
25:                 
26:                 
27:                 
28:                 
29:                 return             
30:      else
31:             Move index into non-basic set
32:            
33:             Try to find an index to move into the basic set
34:            if  then
35:                 
36:                  Move index into basic set
37:                                    
Algorithm 2 PPM Solves the LCP resulting from convex, rigid contact models (the no-slip model and the complementarity-free model with Coulomb friction) . are the set of non-basic indices returned from the last call to PPM.

5 Inverse dynamics with Coulomb friction

Though control with the absence of slip may facilitate grip and traction, the assumption is often not the case in reality. Foot and manipulator geometries, and planned trajectories must be specially planned to prevent sliding contact, and assuming sticking contact may lead to disastrous results (see discussion on experimental results in Section 8.2). Implementations of controllers that limit actuator forces to keep contact forces within the bounds of a friction constraint have been suggested to reduce the occurrence of unintentional slip in walking robots (Righetti et al., 2013). These methods also limit the reachable space of accelerative trajectories that the robot may follow, as all movements yielding sliding contact would be infeasible. The model we present in this section permits sliding contact. We formulate inverse dynamics using the computational model of rigid contact with Coulomb friction developed by Stewart & Trinkle (1996) and Anitescu & Potra (1997); the equations in this section closely follow those in Anitescu & Potra (1997).

5.1 Coulomb friction constraints

Still utilizing the first-order discretization of the rigid body dynamics (Equation 37), we reproduce the linearized Coulomb friction constraints from Anitescu & Potra (1997) without explanation (identical except for slight notational differences):

(52)
(53)

where ( is the number of edges in the polygonal approximation to the friction cone) retains its definition from Anitescu & Potra (1997) as a sparse selection matrix containing blocks of “ones” vectors, is a diagonal matrix with elements corresponding to the coefficients of friction at the contacts, is a variable roughly equivalent to magnitude of tangent velocities after contact forces are applied, and (equivalent to in Anitescu & Potra, 1997) is the matrix of wrenches of frictional forces at tangents to each contact point. If the friction cone is approximated by a pyramid (an assumption we make in the remainder of the article), then:

where and . Given these substitutions, the contact model with inverse dynamics becomes:

\tcbset

lefttitle=2mm,righttitle=1mm,left=2mm,title=Complementarity-based inverse dynamics {tcolorbox}

(54)
(55)
(56)
(57)
(58)

5.2 Resulting MLCP

Combining Equations 3638 and 5253 results in the MLCP:

(59)
(60)
(61)
(62)

Vectors and correspond to the normal and tangential velocities after impulsive forces have been applied.

5.2.1 Transformation to LCP and proof of solution existence

The MLCP can be transformed to a LCP as described by Cottle et al. (1992) by solving for the unconstrained variables and . This transformation is possible because the matrix:

(63)

is non-singular. Proof comes from blockwise invertibility of this matrix, which requires only invertibility of (guaranteed because generalized inertia matrices are positive definite) and . This latter matrix selects exactly those rows and columns corresponding to the joint space inertia matrix (Featherstone, 1987), which is also positive definite. After eliminating the unconstrained variables and , the following LCP results:

(64)
(65)
(66)
(67)

The discussion in Stewart & Trinkle (1996) can be used to show that this LCP matrix is copositive (see Cottle, et al., 1992, Definition 3.8.1), since for any vector ,

(68)

because is positive definite and is a diagonal matrix with non-negative elements. The transformation from the MLCP to the LCP yields LCP variables (the per-contact allocation is: one for the normal contact magnitude, for the frictional force components, and one for an element of ) and at most unconstrained variables.

As noted by Anitescu & Potra (1997), Lemke’s Algorithm can provably solve such copositive LCPs (Cottle et al., 1992) if precautions are taken to prevent cycling through indices. After solving the LCP, joint torques can be retrieved exactly as in Section 4.5. Thus, we have shown—using elementary extensions to the work in Anitescu & Potra (1997)—that a right inverse of the non-impacting rigid contact model exists (as first broached in Section 1). Additionally, the expected running time of Lemke’s Algorithm is cubic in the number of variables, so this inverse can be computed in expected polynomial time.

5.3 Contact indeterminacy

Though the approach presented in this section produces a solution to the inverse dynamics problem with simultaneous contact force computation, the approach can converge to a vastly different, but equally valid solution at each controller iteration. However, unlike the no-slip model, it is unclear how to bias the solution to the LCP, because a means for warm starting Lemke’s Algorithm is currently unknown (our experience confirms the common wisdom that using the basis corresponding to the last solution usually leads to excessive pivoting).

Generating a torque profile that would evolve without generating torque chatter requires checking all possible solutions of the LCP if this approach is to be used for robot control. Generating all solutions requires a linear system solve for each combination of basic and non-basic indices among all problem variables. Enumerating all possible solutions yields exponential time complexity, the same as the worst case complexity of Lemke’s Algorithm (Lemke, 1965). After all solutions have been enumerated, torque chatter would be eliminated by using the solution that differs minimally from the last solution. Algorithm 3 presents this approach.

1:if  then
2:      MINDIFF
3:       Establish nonbasic indices
4:      MINDIFF