DCM_VRPTask

This is an extension of the DCM-VRP tracking I used in my RA-L paper. It is in the form of a task that can be added in any controller easily, and provides a layer of abstraction over the force tracking tasks it wraps to execute the desired VRP. The Tasks implementations are left mostly as templates, the main work was done on the MetaTasks and the TVM functions.

Organisation and logic

There are three different implementations of the task, that can all be loaded separately:

• DCM_VRPTask is the first version, implemented as a MetaTask combining a CoM task and CoP tasks for each contact. It is expected to run in combination with the polytope calculations of mc_dynamic_polytope and a QP uses these constraints to distribute the forces across contacts, that are given as CoP targets for the full-body QP run. As it uses the observer it works well for external perturbation but the main drawback is that is does not take the coupling of the EoM into account in the distribution QP.

• DCMTask is a wrapper for the DCM_VRPFunction tvm function, implemented after updating mc_rtc and tvm to support low-level polytope constraints. This one uses the jacobian of the DCM, and uses the force related VRP to formulate the DCM velocity. As it is implemented as a whole-body task it takes the EoM coupling into account but using the DCM jacobian defeats the purpose of having a simple force driven control, and requires a second order function in tvm.

• VRPTask is a wrapper for the VRPFunction tvm function, corresponding the most to what whole-body VRP control should be: it does not control the DCM but the VRP directly, using the contact forces jacobians, resulting in accurate DCM tracking using a simple tvm LinearFunction. The last problem for this one is it lacks external perturbations resistance because the mc_rtc formulation closes the loop on the control robot only, not the real one, but everything is consistent for multi-contact balance otherwise.

The low level task implementations are found in the Tasks and mc_tvm respectively, while the high level MetaTasks wrappers are found in the mc_tasks folder. The DCM_VRPUtils folder contains "observers" and QP formulations that are mostly used in DCM_VRPTask but are independant classes. This means they may also be used on their own to keep track of the force that should be applied to an object or robot in a controller. This was the case for the RA-L, where the observer was used to determine the missing force for the human, and this force given as a target to the robot.

The independent format of these observers is so they can be easily extended to plug their output (state of a robot or object) to the input of the task of an executor robot directly. The point would also be the possibility of choosing the centroidal dynamic measurements source arbitrarily for the update, not necessarily the realRobot (again, for example the motion capture data for the human).

Dependencies

• mc_rtc

• Gram-Savitzky-Golay for filtered derivation using polynomials (for variable height frequency)

• mc_dynamic_polytope is not a direct dependency for installation, but the DCM_VRPTask expects being fed polytopes at runtime.

Usage

DCM_VRPTask

To use in on a controller: add in the yaml file a configuration of the form

DCM_VRPTask:
  type: DCM-VRP
  possibleContacts: [RightFoot, LeftFoot, RightHand]
  withPolytopeConstraints: true
  withAutoBalance: false
  zeroMomentAutoBalance: true
  tracker:
    P: 10
    I: 0
    D: 0

with the set of possible interaction contacts the robot can make with its environment. By default the LeftFoot and RightFoot surfaces are considered. Other options can be added such as more specific parameters of the CoM and CoP tasks using the tasks tag.

The expected contact constraints are of the same form as for the LIPM stabilizer, for example

contacts:
- r2: ground
  r1Surface: LeftFoot
  r2Surface: AllGround
  dof: [0.0, 0.0, 1.0, 1.0, 1.0, 0.0]
- r2: ground
  r1Surface: RightFoot
  r2Surface: AllGround
  dof: [0.0, 0.0, 1.0, 1.0, 1.0, 0.0]

So that the CoP tasks can push the reference pose of the contacts in the ground.

Load it in your controller with

DCM_VRPTask_ = mc_tasks::MetaTaskLoader::load<mc_tasks::DCM_VRP::DCM_VRPTask>(solver(), config("DCM_VRPTask"));
solver().addTask(DCM_VRPTask_);

and when running set the static DCM target with an Eigen::Vector3d using

DCM_VRPTask_->setDCMTarget(robotDCMtarget_);

and the polytopes constraints using

DCMTask_->setContactPlanes(contactName, Polytope_->getForcePolyPlanes(contactName));

This task depends on the realRobot instance to be updated so observers such as the kinematicInertial must be setup.

DCMTask

yaml configuration:

DCMTask:
  type: DCM
  above: [LeftFoot, RightFoot]
  weight: 100000
  stiffness: 10

The contact dofs can all be locked as this task doesn't rely on the CoP tasks.

Loading:

DCMFunction_ = mc_tasks::MetaTaskLoader::load<mc_tasks::DCM_VRP::DCMTask>(solver(), config("DCMTask"));
solver().addTask(DCMFunction_);

Running:

DCMFunction_->targetDCM(robotDCMtarget_);

VRPTask

yaml configuration:

VRPTask:
  type: VRP
  above: [LeftFoot, RightFoot]
  weight: 100000
  stiffness: 10

The contact dofs can all be locked as this task doesn't rely on the CoP tasks.

Loading:

VRPFunction_ = mc_tasks::MetaTaskLoader::load<mc_tasks::DCM_VRP::VRPTask>(solver(), config("VRPTask"));
solver().addTask(VRPFunction_);

Running:

VRPFunction_->targetDCM(robotDCMtarget_);