Correct the construction of the actuated joints list.

[francois-keith]

The order of the joints is important, it should be the same that the one uses in jrl-dynamics, not the alphabetical order.

[francois-keith]

The problem comes from one (the only (*) ?) application that is made of the function getActuatedJoints, in jrl-walkgen.

When one loads the robot using a urdf rather than a vrml, the order of the joints in the vector of dofs corresponds to the order by depth first. The problem here lies in the fact than the actuactedJoints vector was not specified in this order (before this commit, it was classified by alphabetical order).

This led to some issues when the actuatedJoint vector is used by jrl-walkgen to remap the vector of dofs provided by the python interface to each of the joints of the multibody:

  std::vector<CjrlJoint *> ActuatedJoints = 
    getHumanoidDynamicRobot()->getActuatedJoints();

https://github.com/jrl-umi3218/jrl-walkgen/blob/master/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp#L274-L275

It is used later on:

  // Initialize the configuration vector.
  for(unsigned int i=0;i<m_GlobalVRMLIDtoConfiguration.size();i++)
    {
      CurrentConfig[m_GlobalVRMLIDtoConfiguration[i]] = BodyAnglesIni[i];
    }  

  CjrlHumanoidDynamicRobot *aDMB =  getHumanoidDynamicRobot();
  aDMB->currentConfiguration(CurrentConfig);

https://github.com/jrl-umi3218/jrl-walkgen/blob/master/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp#L386

Whitout this modification, the initialization of the walkgen goes wrong.

(*) Using grep confirms that this is the only of the method getActuatedJoints() (in the whole SoT)

[olivier-stasse]

The problem comes from one (the only (*) ?) application that is made of the function getActuatedJoints, in jrl-walkgen.

When one loads the robot using a urdf rather than a vrml, the order of the joints in the vector of > dofs corresponds to the order by depth first. The problem here lies in the fact than the actuactedJoints vector was not specified in this order (before this commit, it was classified by alphabetical order).

To me this is the other way around.

The previous code was simply giving a list of joints specified by an upper mechanism (setActuatedJoints).

Loading URDF model provides a list of alphabetical joints. What you are doing in the new code is imposing a depth first.

If you read a full model containaing fixed joints (which are not actuated), or parallel joint they will be included in the depth first exploration.

There is a reason why there is no getActuatedJoints in the whole SoT code. It is because people use the SoT with a trimmed model without any unactuated joint (with the major exception of the root of the kinematic tree). This strategy will reach its limits if we consider models with passive part which have to be evaluated in the underlying control problems. But this is unlikely to happen in the present code.

To summarize:

• I agree with this modification as long it is understood that the user HAS TO provide a trimmed model.
• We do not generalize this strategy for further codes. Mapping the state vector of the robot model with the actuated part seems to me a better strategy.

Best, Olivier.

[francois-keith]

Loading URDF model provides a list of alphabetical joints.
What you are doing in the new code is imposing a depth first

In fact, this order is specified by the methods

addBodiesToJoints()
robot_->initialize();

After those calls, the actuated joints vector is defined in the robot_ attribute, and is already in the depth first order. What I did is modifying the overwrite of this vector in order to keep this order.

Looking back to my modification, I know understand that the call to Parser::actuatedJoints (in which the vector of joints is re-created) could be removed and replaced by a simple filter on the actuatedJoints vector, in which the free-floatting and the fixed joints are removed.

After what it is still required to overwrite the vector of actuated joints in the humanoid robot. I've made the corresponding modification on my fork, I don't know if you can reopen the PR to integrate it.