Controlling the CoM

The objective of this tutorial is to control the robot's center of mass (CoM). As in the previous tutorial, we will write a function that moves the CoM down 20 centimeters and then moves it back to its original position.

Contacts

Before we start moving the CoM we need to consider contacts. In fact, in our previous controller we decided to keep an empty contact set and everything worked well. However, if we were trying to control the CoM now, the robot would simply sink into the ground or float in the air to follow our command. The reason is simply that without contacts, this is a perfect solution to moving the CoM while keeping a minimal posture error.

Therefore, we will add contacts between the robot's feet and the ground:

C++
Python

// In the constructor
addContact({robot().name(), "ground", "LeftFoot", "AllGround"});
addContact({robot().name(), "ground", "RightFoot", "AllGround"});

# In the constructor
self.addContact(self.robot().name(), "ground", "LeftFoot", "AllGround")
self.addContact(self.robot().name(), "ground", "RightFoot", "AllGround")

Note: see the tutorial on surfaces visualization to check out how you can "discover" existing surfaces in a robot.

Dynamics constraint

Until now we have run the controller in kinematics mode. We can switch to dynamics mode, this will enable us to:

Compute external forces and associated torques;
These forces remain in the contact friction cone;
The torques remain within the robot's torque limits;

To switch, simply change kinematicsConstraint into dynamicsConstraint in your code. All constraints enabled by kinematicsConstraints are also enabled by dynamicsConstraint.

Create the CoM task and add it to the problem

We need to take care of a few things:

Create the CoM task;
Add it to the optimization problem;
Make sure its objective is correctly set;

C++
Python

// Include the CoM task header (header)
#include <mc_tasks/CoMTask.h>
// In the class private members (header)
std::shared_ptr<mc_tasks::CoMTask> comTask;
// In the constructor, create the task and add it to the problem
comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0, 10.0, 1000.0);
solver().addTask(comTask);
// Reduce the posture task stiffness
postureTask->stiffness(1);
// In the reset function, reset the task to the current CoM
comTask->reset();

# Import the mc_tasks module
import mc_tasks

# In the constructor, create the task and add it to the problem
self.comTask = mc_tasks.CoMTask(self.robots(), 0, 10.0, 1000.0)
self.qpsolver.addTask(self.comTask)
# Reduce the posture task stiffness
self.postureTask.stiffness(1)
# In the reset callback, reset the task to the current CoM
self.comTask.reset()

In that particular example, we can say a few things about the task creation:

It depends on robots();
It will be applied to the robot with index 0, this is always the main robot loaded by mc_rtc;
It is attracted to the objective with a stiffness of 10.0; this parameter governs the strength of the "spring" that pulls the robot towards the objective;
It is associated a weight of 1000; this weight is the priority of the task in the optimization problem. In our case, the default posture weight is 5.0, thus the CoM task will have high priority. Please note that task errors are not normalized, and thus weights have to be tuned accordingly;
The posture task stiffness is decreased to make sure it doens't interfere with the CoM task;

Moving the CoM up and down

We will implement a method similar to the previous tutorial, using a switch_com_target() function. We will assume we have added a comDown boolean and a comZero which is an Eigen::Vector3d representing the initial CoM position.

C++
Python

void MyFirstController::switch_com_target()
{
  // comZero is obtained by doing:
  // comZero = comTask->com();
  // in the reset function
  if(comDown) { comTask->com(comZero - Eigen::Vector3d{0, 0, 0.2}); }
  else { comTask->com(comZero); }
  comDown = !comDown;
}

def switch_com_target(self):
    # self.comZero is obtained by doing:
    # self.comZero = self.comTask.com()
    # in the reset function after doing the CoM task reset
    if self.comDown:
        self.comTask.com(self.comZero - eigen.Vector3d(0, 0, 0.2))
    else:
        self.comTask.com(self.comZero)
    self.comDown = not self.comDown

Finally, we will monitor the CoM task error to trigger target changes:

C++
Python

bool MyFirstController::run()
{
  if(comTask->eval().norm() < 0.01)
  {
    switch_com_target();
  }
  return mc_control::MCController::run();
}

def run_callback(self):
    if self.comTask.eval().norm() < 0.01:
        self.switch_com_target()
    return True

Et voilà! You can run this controller and see that the robot is moving down and up. In the next tutorial we will see how to move an end-effector and to load the task configuration from the disk.

The full sources for this controller are available here.

Next tutorial: Controlling an end-effector (and loading)