ControlManager no more working with the new release

[NoelieRamuzat]

Hi all !
In the last releases a new entity in sot-core named parameter-server has been created. It reads the urdf model (found in /robot_description) and retrieve all the parameters of the robot (limits...). Now the parameter-server is more handy because it reads directly the urdf from a rostopic which is used everywhere in the pipeline to create the robot (ie: gazebo, pattern-generator...).

However, this entity has been made from the ControlManager of sot-torque-control.
In fact, in sot-talos-balance the ControlManager of sot-torque-control was duplicated and split in two entities: the parameter-server (now in sot-core) and the talos-control-manager.

Following the new release, if I try to use the ControlManager of sot-torque-control the simulations do not perform as before (the robot is falling after one step). I think it is because the ControlManager creates its own parameter-server and do not use the one of sot-core, leading to discrepancies.

But if I use the talos-control-manager of sot-talos-balance with the parameter-server of sot-core it is not working neither.
Indeed in talos-control-manager the major change is that the control vector includes the freeflyer.
I don't know why they made this choice but it leads to some problems:

First to set the control type of your control vector you can use the function setCtrlMode which wait for a string.
And this string is parsed to retrieve the jointNames.
Then the freeflyer cannot be set with this function. The only way to not obtain an error when setting the control type is to use the formulation setCtrlMode("all", "controlType") which will set the freeflyer control as well as the joints.
This was the only use of this function in sot-talos-balance, but in sot-torque-control I use different control in function of the joints.
Then we need to add a way to set the freeflyer control in the talos-control-manager:
For instance:

void TalosControlManager::setCtrlMode(const string& jointName, const string& ctrlMode) {
  CtrlMode cm;
  if (convertStringToCtrlMode(ctrlMode, cm) == false) return;

  if (jointName == "all") {
    for (unsigned int i = 0; i < m_numDofs; i++) setCtrlMode(i, cm);
  } else {
    // decompose strings like "rk-rhp-lhp-..."
    std::stringstream ss(jointName);
    std::string item;
    std::list<int> jIdList;
    unsigned int i;
    while (std::getline(ss, item, '-')) {
      SEND_MSG("parsed joint : " + item, MSG_TYPE_INFO);
      if (item == "freeflyer") {
        for (unsigned int j = 0; j < 6; j++) jIdList.push_back(j);
      } else {
        convertJointNameToJointId(item, i);
        jIdList.push_back(i);
      }
    }
    for (std::list<int>::iterator it = jIdList.begin(); it != jIdList.end(); ++it) setCtrlMode(*it, cm);
  }
  updateJointCtrlModesOutputSignal();
}

Second, because of the add of the freeflyer, the size of the control vector has to be (nbJoints + 6) for each input control.
Then I have to add an array of 6 zeros before my torque control vector input.
It is really not handy. In particular because the freeflyer cannot be controlled so it made no sense (it is not use then in the device entity).

Finally even by doing all of these changes I still have simulations that do not behave as before.
The walk on flat floor is working but the stairs climbing is not.
In all the simulations I have a strange behaviour with the wrists. They are tilted although I ask them to remain in the configuration of the half-sitting.

This issue is linked to the #20 of sot-talos-balance which ask if we can rename and move the talos-control-manager in a more general package (sot-core ?).