VOCAB_CM_TORQUE control not supported in coupled joints

[traversaro]

The code that converts desired quantities from actuated axis to physical joints and measured quantities from physical joints to actuated axis seems only consider desired positions and measured position and velocities, see:

• gz-sim-yarp-plugins/plugins/controlboard/src/ControlBoard.cpp

  Lines 382 to 390 in 1372014

  	if (m_controlBoardData.ijointcoupling){
  	m_controlBoardData.ijointcoupling->convertFromPhysicalJointsToActuatedAxesPos(m_physicalJointsPositionBuffer, m_actuatedAxesPositionBuffer);
  	m_controlBoardData.ijointcoupling->convertFromPhysicalJointsToActuatedAxesVel(m_physicalJointsPositionBuffer, m_physicalJointsVelocityBuffer, m_actuatedAxesVelocityBuffer);
  	for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++)
  	{
  	m_controlBoardData.actuatedAxes[i].commonJointProperties.position = m_actuatedAxesPositionBuffer[i];
  	m_controlBoardData.actuatedAxes[i].commonJointProperties.velocity = m_actuatedAxesVelocityBuffer[i];
  	}
  	}

• gz-sim-yarp-plugins/plugins/controlboard/src/ControlBoard.cpp

  Lines 472 to 481 in 1372014

  	for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++)
  	{
  	m_actuatedAxesPositionBuffer[i] = m_controlBoardData.actuatedAxes[i].commonJointProperties.refPosition;
  	}
  	m_controlBoardData.ijointcoupling->convertFromActuatedAxesToPhysicalJointsPos(m_actuatedAxesPositionBuffer, m_physicalJointsPositionBuffer);
  	for(auto i = 0; i < m_controlBoardData.physicalJoints.size(); i++)
  	{
  	m_controlBoardData.physicalJoints[i].commonJointProperties.refPosition = m_physicalJointsPositionBuffer[i];
  	}

So, clearly the VOCAB_CM_TORQUE control mode is not supported for coupled joints. At the moment we should document this limitation, and ideally return an error if someone asks to set in torque a coupling joint. We could also try to understand if it make sense to support this (for example if it is supported in any physical robot) and if it exists a case in which it make sense to support this, implement it.

[traversaro]

I guess the simplest solution was to implement it: #248 .

[traversaro]

I guess the simplest solution was to implement it: #248 .

This works fine for coupling devices that implement the torque mapping methods, but some do not (see #248 (review)). In that case we need to understand how to handle. For the control I think the best solution is return error if somebody tries to switch the coupled joints in torque control mode, while I am not sure how to handle the torque feedback case. In theory we could return false in getTorques, but I am afraid this would not be handled nicely by the various remappers logic. Probably the easiest solution is to return the same value returned in the real robot for the ergocub heands, @PasMarra as you are probably the person with most experience with the real robot ergocub hand, can you check that? Thanks!

[traversaro]

Another problem in the torque control mode in coupled joints is that given how it is implemented now is possible that in underactuated systems that approximate system in which there is come kind of "position constraint" among the different physical joints, controlling the coupled device in torque will result in a drift that will break the position constraints. This does not happen in other control modes as there eventually everything is converted to a reference position on the actuated axis side, that is converted via position mapping to a physical joint reference that then is passed to a position PID, so the enforcing of any position constraint is done implicitly by the PID position control on the physical joint side.

[PasMarra]

Hi @traversaro, I've tried the torque control mode for the fingers of ergoCub002, but also on the real robot this control mode is not properly handled.

It happens that yarp::dev::IControlMode::setControlMode() returns true but the control mode does not change; then, if i set a reference with yarp::dev::ITorqueControl::setRefTorques(), still the return value is true but the reference values do not change. Analogous situation for yarp::dev::ITorqueControl::getRefTorques().

[traversaro]

Hi @traversaro, I've tried the torque control mode for the fingers of ergoCub002, but also on the real robot this control mode is not properly handled.

It happens that yarp::dev::IControlMode::setControlMode() returns true but the control mode does not change; then, if i set a reference with yarp::dev::ITorqueControl::setRefTorques(), still the return value is true but the reference values do not change. Analogous situation for yarp::dev::ITorqueControl::getRefTorques().

Thanks for the test. As the torque control mode is not supported on the hand (I guess), I guess that setControlMode with an unsupported control mode should return false.