Modify WBI-T according to the new ID-System

[jeljaik]

• [x] Robot number of DOF specified at configuration time via the ResourceFinder
  • [x] Add robots dir in yarpWholeBodyInterface's app dir with the ini file for iCubGenova03.
  • [x] Change CMakeLists of yarpWholeBodyInterface it as in https://github.com/robotology/icub-main/blob/master/app/robots/CMakeLists.txt to install all robots available.
  • [x] Find .ini file in the right context defined by yarpWholeBodyInterface.
• [x] Proper abortion when initialization isn't finished correctly. (The fix worked perfectly in this branch @traversaro Thanks!!)
• [x] icubWholeBody* => yarpWholeBody*
• [x] wbi::LocalId => wbi::wbiId
• [x] Write a yarpWholeBodyInterfaceTest module for the branch new_wbi_ID https://github.com/robotology/codyco-modules/commit/aa681228657a315a0aff6868bae1c03732273b7c
• [x] Test COMPoseTesting works correctly.
• [x] Stopping the simulation is not finishing correctly as it makes Matlab crash right after maxJntLimits deleted.
• [x] Perform single tests for each block in the Toolbox:
  • [x] jointAngles
  • [x] jointVelocities
  • [x] forwardKinematics
  • [x] jacobian
  • :warning: set velocities
  • :warning: set angles
  • [x] set torques
  • [x] computeh
  • [x] massMatrix
  • [x] dJdq
  • [x] jointAccelerations
  • [x] jointTorques
  • [x] inverseDynamics
  • [x] jointLimits
  • [x] angularMomentum
  • :warning: EEWrench @traversaro Please let me know when this part is ready in yarpWholeBodyInterface
  • [x] setWorldRefFrame
  • [x] param Forward Kinematics
  • [x] param Jacobians
  • [x] param dJdq
• [x] Remove this DEBUG message: [DEBUG] yarpWholeBodyStates::getEstimates: called getEstimates ESTIMATE_JOINT_POS
• [x] Test WBI-Toolbox with different robots, e.g. COMAN, iCubHeidelberg01
  • [x] The variable ROBOT_DOF is necessary also outside the c++ code, i.e. in the MATLAB environment for those blocks that have null inputs at the most inner level. The config file for each robot must be read somehow by MATLAB as well... :weary: For now let the user specify ROBOT_DOF from command line, along with robotName, Ts and localName
  • [x] Change all blocks of the toolbox so that they use the MATLAB variable ROBOT_DOF.
  • [x] Get COMPoseTest working with iCubHeidelberg01 @traversaro yayyy!! :grinning:
  • [x] Get COMPoseTest working with COMAN
  • [x] Write configuration file for COMAN
• [x] Change the logic of the MATLAB variables needed for simulation: robotName, 'localName' and 'Ts'. From now on, robotName should correspond to the usual robot name variable in iCub modules, i.e. either icub or icubGazeboSim, while localName is the prefix of the ports opened by the WBI. https://github.com/robotology-playground/WBI-Toolbox/commit/78b96086bcb005281ff840b511cd85d9ce2ce70f Besides this change, remember that the robotName and localName input by the user from MATLAB's command line overwrites the default ones from configuration file.
• [x] Update all blocks from the toolbox to take into account ROBOT_DOF https://github.com/robotology-playground/WBI-Toolbox/commit/78b96086bcb005281ff840b511cd85d9ce2ce70f
• [x] Update README.md with migration instructions. https://github.com/robotology-playground/WBI-Toolbox/blob/new_wbi_ID/README.md
• [x] Test yarpWholeBodyInterfaceIcubTest with COMAN
• [x] :warning: Get WBI-Toolbox working with the double pendulum model.

Pending Improvementes

• :warning: Test Torque Balancing Demo with the WBI-Toolbox in branch new_wbi_ID
• :warning: Make interface configuration happen only once by the first block that attempts to do so.
• :warning: Parse configuration files in MATLAB to create list of possible links from param jacobians, etc.
• :warning: Perform properly block input size checking. e.g. If input to a block does not have the proper size, Matlab is simply crashing with a hard to diagnose error message.

[jeljaik]

Corresponding WBI-Toolbox Issue ref. robotology-playground/WBI-Toolbox#27

[traversaro]

Ref : https://github.com/robotology/codyco-modules/issues/6#issuecomment-61452164

[jeljaik]

@arocchi, @EnricoMingo as you can see from the TODO List above I'm writing the configuration file for COMAN to be used by yarpWholeBodyInterface in our branch new_wbi_ID. After many improvements and modifications done to the WBI, The WBI-Toolbox is now ready to be tested with COMAN. To do so, I need the URDF file of the robot. Can you provide me with it without installing ROS and so on? :bow:

[jeljaik]

COMAN URDF model.urdf | uploaded via ZenHub

[traversaro]

"Copy the robots configuration files in the default context wbit. Perhaps change name to WBI-Toolbox" <--- I don't think this is a good idea, robots directories can't be part of a context!

[jeljaik]

Oh right, that was before our discussion on how the ResourceFinder should just fail if there's nothing in app/robots/

[jeljaik]

Things should be working just fine with the doublePendulum model @iron76. Please update and test if you like.