Topic/task energy

[NoelieRamuzat]

Hi all !

Here are the developments that I made for the RA-L paper "Passive Inverse Dynamics Control using a Global Energy Tank for Torque-Controlled Humanoid Robots in Multi-Contact".
I implemented a new task called energyTask in TSID and made quite some changes. I don't know where to propose this PR as it also modifies the formulation of the QP (inverse-dynamics-formulation-acc-force) to take the energy into account.
If perhaps you can tell me where to propose it @andreadelprete, @nim65s ? Perhaps in a new branch ? Thanks !

The energy task is used to transform the classical whole-body controller using inverse dynamics of TSID into a passive scheme. The passivity is a stability criterion based on the power flow exchanged by the components of the system. A storage function H(x) is chosen (often as the energy of the system) and to have a passive system, the system internal power (d_H: derivative of the energy in the system) must be lesser or equal to the power transferred to the system through its port. For a system controller+robot (controlled in torque) it is the velocity and the torque.

The energy task adds an energy tank to the controller to monitor the energy flow of the system. This tank regulates the task gains using coefficients (alpha, beta and gamma in [0, 1]) to respect the passivity of the system. These coefficients are multiplied to the task vectors in the QP formulation.
Thus, if one of the coefficient is lowered to zero (when there is no more energy in the tank), the tasks are penalized because have a decreased desired acceleration or even a null one. Moreover, as the tank is computed without taking into account the constraints of the QP, a passivity constraint is added in the QP formulation in addition to the regulating coefficient.
For more information, see the paper (soon to be published).

[andreadelprete]

Hi @NoelieRamuzat , thanks for your contribution, it looks really interesting. At a first sight, I am not against having this PR directly on the devel branch. It will probably take me a while to review, as it's a lot of changes, but I think we can leave it here. Unless of course @nim65s has a better idea.

[nim65s]

Hi,

No, this looks really clean to me, I also think we should put that on devel.
The only issue I see is the API changes.

@NoelieRamuzat : could you document (and maybe motivate) the modifications you made ? I'm not talking about anything you added, just the cases where you changed a return type, or removed some const. This will bu useful in the next version release notes, and if this break anyone's code, this will help them to upgrade.

Also, maybe, a test for this new feature would be nice ;)

[andreadelprete]

Hi @NoelieRamuzat , just for your information, I am reading your paper to understand the theory in details before reviewing this PR. I'll probably send you an email to ask a few questions about the theory.

[NoelieRamuzat]

Hi @nim65s, @andreadelprete,

Thanks for your comments, I am currently working on the API changes.
No problem for the theory details @andreadelprete, the early access of the paper is at the following link:
https://ieeexplore.ieee.org/document/9691905

[NoelieRamuzat]

Hi ! Sorry I made a miss-click on the PR.

Here are the explanations for the changes in the API:

• I created the TaskLevelMotion structure in the formulation because I needed the vector of motion tasks in the QP (m_taskMotions) to be of class TaskMotion and not simply TaskBase (to have access to the specific methods of TaskMotion).

• I added in the class TaskMotion the virtual setter and getter Kp, Kd and the methods getJacobian and acceleration_ref as I need them for the computation of the energy.
  As these functions are now in the parent class, the inherited classes have to have the same return types.
  This is why, in the CoM and angular-momentum tasks, I changed the return types of the Kp and Kd getters form Vector3 to VectorXd.
  In particular, for the angular-momentum task, I added the methods velocity_ref, velocity_err, acceleration_ref, acceleration_err to have the generic methods of the TaskMotion class defined and use them in the computation of the energy. They return the momentum_ref and momentum_error of the AM class.
  However, as the return types of the methods of the TaskMotion class were VectorXd I had to change the return types of the momentum methods (and also the ones of the private members).
  Moreover, in the SE3_equality task, I had to remove the const of the getter Kp, Kd as they are not const in the parent class.
  And then, because the contact classes implement the getter Kp, Kd for their m_motionTask, I also had to remove the const there.

• Finally, in the contact classes I had to remove the const of the getMotionTask method.
  I call this method in the energy task :

TaskSE3Equality& contact_motion = cl->contact.getMotionTask();
auto tl = std::make_shared<TaskLevelMotion>(contact_motion, 1);

And if it is const it does not work because the TaskLevelMotion takes a reference.
I implemented it the same way as the other TaskLevel formulations.
I did not found conflicting use of contact.getMotionTask which needed this const, thus I removed it.

[NoelieRamuzat]

Hi !
I just fixed some unnecessary changes in the files and add a python test for the energy task.

The python test creates a Romeo robot using the TsidBiped class and add the energy task to the stack of task.
Then the robot does a sinusoidal motion on its CoM.
If the CoM error does not diverge, the energy tank does not fall under its minimal value (0.1J) and the QP is solved then the test is ok.