Skip to content

Floating in Contact Systems

Jump to bottom
Carlos Mastalli edited this page Apr 1, 2019 · 1 revision

Differential Action Model for Floating in Contact Systems (DAMFIC)

I. Introduction

System dynamics

As you might know, a differential action model describes the systems dynamics and cost function in continuous-time. For multi-contact locomotion, we account for the rigid contact by applying the Gauss principle over holonomic constraints in a set of predefined contact placements, i.e.:

$\begin{aligned} & \dot{\mathbf{v}} = \underset{\mathbf{a}}{\arg\min} & & \frac{1}{2}\,\|\dot{\mathbf{v}}-\dot{\mathbf{v}}_{free}\|_{\mathbf{M}} \\ & \textrm{subject to} & & \mathbf{J}_{c} \dot{\mathbf{v}} + \mathbf{J}_c \mathbf{v} = \mathbf{0}, \end{aligned}$

This is equality-constrained quadratic problem with an analytical solution of the form:

$\left[\begin{matrix}\mathbf{M} & \mathbf{J}^{\top}_c \\{\mathbf{J}_{c}} & \mathbf{0}\end{matrix}\right] \left[\begin{matrix} \dot{\mathbf{v}} \\ -\boldsymbol{\lambda} \end{matrix}\right] = \left[\begin{matrix} \boldsymbol{\tau}_b \\ -\dot{\mathbf{J}}_c \mathbf{v}\end{matrix}\right]$

in which

  • $(\dot{\mathbf{v}},\boldsymbol{\lambda})\in(\mathbb{R}^{nv},\mathbb{R}^{nf})$ are the primal and dual solutions,
  • $\mathbf{M}\in\mathbb{R}^{nv\times nv}$ is formally the metric tensor over the configuration manifold $\mathbf{q}\in\mathbb{R}^{nq}$,
  • $\mathbf{J}_{c}= \begin{bmatrix} \mathbf{J}_{c_1} & \cdots & \mathbf{J}_{c_f}\end{bmatrix}\in\mathbb{R}^{nf\times nv}$ is a stack of $f$ contact Jacobians,
  • $\boldsymbol{\tau}_b=\mathbf{S}\boldsymbol{\tau}-\mathbf{b}\in\mathbb{R}^{nv}$ is the force-bias vector that accounts for the control $\boldsymbol{\tau}\in\mathbb{R}^{nu}$, the Coriolis and gravitational effects $\mathbf{b}$, and $\mathbf{S}$ is the selection matrix of the actuated joint coordinates, and
  • $nq$, $nv$, $nu$ and $nf$ are the number of coordinates used to describe the configuration manifold, its tangent-space dimension, control commands and contact forces, respectively.

And this equality-constrained forward dynamics can be formulated using state space representation, i.e.:

$\dot{\mathbf{x}}=\mathbf{f(x,u)}$

where $\mathbf{x}=(\mathbf{q,v})\in\mathbb{R}^{nq+nv}$ and $\mathbf{u}=\boldsymbol{\tau}\in\mathbb{R}^{nu}$ are the state and control vectors, respectively. Note that $\dot{\mathbf{x}}$ lies in the tangent-space of $\mathbf{x}$, and their dimension are not the same.

Cost function

For handling a set of cost functions, we have defined a cost manager called CostModelSum. In Crocoddyl, there is a set of predefined cost functions such as:

  • CoM tracking (CostModelCoM).
  • Frame placement and position tracking (CostModelFramePlacement and CostModelFrameTranslation).
  • Frame velocity (or linear velocity) tracking (CostModelFrameVelocity or CostModelFrameVelocityLinear).
  • State and control regularization and tracking (CostModelState and CostModelControl).
  • Force tracking (CostModelForce), among others.

Note: all these cost models use an activation function. With this, we can impose different cost function such as weighted quadratic ones. A cost model describes a residual vector, instead an activation model describe a function based on a residual.

II. Formulating a DAMFIC

As early described, the floating in contact dynamics require to predefined a set of contact placements. Other additional requirements are robot dynamics and the actuation model. First, for the robot dynamics, we use the Pinocchio model. Second, if you don't define actuator dynamics then you could use a simple floating base actuation, which is described in ActuationModelFreeFloating. Typically you can do so as follows:

damfic = DifferentialActionModelFloatingInContact(pinocchioModel,
                                                  actuationModel,
                                                  contactModel,
                                                  costModel)

where the contact model stacks a set of constrained contact placements as follows:

constrainedContact = ContactModel6D(pinocchioModel, # for 3d contacts do ContactModel6D
                                    frameID)

contactModel = ContactModelMultiple(pinocchioModel)
contactModel.addContact('contact_name', constrainedContact)

For advanced users

You can defined a PD gains that penalizes deviation in the frame position and its motion. These gains helps to numerically stabilize the dynamics rollout, and it's called Baumgarte's stabilization method. Mathematically the holonomic constraints are modified as:

$ \mathbf{J}_c\dot{\mathbf{v}} + \mathbf{J}_c\mathbf{v} + \alpha(\mathbf{R(q)} \ominus \mathbf{R}_{ref}) + \beta(\mathbf{v})=\mathbf{0}$

where

  • $\alpha$ and $\beta$ are the Baumgarte gains.
  • $\mathbf{R}_{ref}$ is the reference frame placement (or position for 3d contacts).

and it can be defined by the user as follows:

placementRef = # we use Pinocchio SE3, e.g. pinocchio.SE3(ref_rotation, ref_translation)
baumgarteGains = [alpha, beta] # alpha and beta are positive floats
constrainedContact = ContactModel6D(pinocchioModel, # for 3d contacts do ContactModel6D
                                    frameID,
                                    placementRef,
                                    baumgarteGains)
Clone this wiki locally