Take into account when some joints are not controlled.

  In the case of the Franka robot, the URDF model does not propose position
  control via ROS control for the fingers. In this case the control vector
  is smaller than the encoder reading vector.

  This commit makes the asumption that the not controlled joints are at the end
  of the control vector. Some work is needed to make this more general.

Author: florent-lamiraux

src/roscontrol-sot-controller.cpp

@@ -941,7 +941,8 @@ void RCSotController::readControl(
     std::string &lmapRC2Sot = it_mapRC2Sot->second;
     command_ = controlValues[lmapRC2Sot].getValues();
     ODEBUG4("angleControl_.size() = " << command_.size());
-    for (unsigned int i = 0; i < command_.size(); ++i) {
+    for (unsigned int i = 0; i < std::min(command_.size(), joints_.size());
+         ++i) {
       joints_[joints_name_[i]].joint.setCommand(command_[i]);
       DataOneIter_.controls[i] = command_[i];
     }