[pinocchio-device] Checked unit test for one rotation joint.

include/sot-dynamic-pinocchio/pinocchio-device.hh

@@ -131,7 +131,10 @@ namespace dynamicgraph {
         return CLASS_NAME;
       }
       static const double TIMESTEP_DEFAULT;
-      
+
+      /// Maps of joint devices.
+      std::map<std::string,dgsot::JointSoTHWControlType> jointPinocchioDevices_;
+
     protected:
       /// \brief Current integration step.
       double timestep_;
@@ -159,8 +162,6 @@ namespace dynamicgraph {
       std::map<std::string,ControlType> sotControlType_;
       std::map<std::string,ControlType> hwControlType_;
 
-      /// Maps of joint devices.
-      std::map<std::string,JointSoTHWControlType> jointPinocchioDevices_;
       /// 
       PeriodicCall periodicCallBefore_;
       PeriodicCall periodicCallAfter_;

src/pinocchio-device.cpp

@@ -29,7 +29,7 @@ using namespace std;
 using namespace dynamicgraph::sot;
 using namespace dynamicgraph;
 
-#define DBGFILE "/tmp/sot-talos-device.txt"
+#define DBGFILE "/tmp/pinocchio-device.txt"
 
 #if 0
 #define RESETDEBUG5() { std::ofstream DebugFile;	\
@@ -295,8 +295,17 @@ setURDFModel(const std::string &aURDFModel)
 
   /// Build the map between pinocchio and the alphabetical order.
   for(unsigned int i=0;i<model_.names.size();i++)
-    jointPinocchioDevices_[model_.names[i]].pinocchio_index=i;
-
+    {
+      if (model_.joints[i].id()<model_.nq)
+	{
+	  jointPinocchioDevices_[model_.names[i]].pinocchio_index=i;
+	  std::cout << "jointPinocchioDevices_ index: " << i
+		    << " model_.joints[i].id(): " << model_.joints[i].id()
+		    << " model_.names[i]: " << model_.names[i]
+		    << " model_.joints[i].shortname() : "<< model_.joints[i].shortname()
+		    << std::endl;
+	}
+    }
   // Initialize pinocchio vector.
   position_.resize(model_.nq);
   velocity_.resize(model_.nv);
@@ -400,6 +409,14 @@ void PinocchioDevice::integrate( const double & dt )
       int lctl_index = it_control_type->second.control_index;
       int pino_index = it_control_type->second.pinocchio_index;
 
+      if (lctl_index==-1)
+	{
+	  if (debug_mode_>1)
+	    std::cerr << "No control index for joint "
+		      << model_.names[pino_index] << std::endl;
+	  break;
+	}
+      
       if (pino_index!=-1)
 	{
 	  int lpos_index = model_.joints[pino_index].idx_q();
@@ -410,6 +427,8 @@ void PinocchioDevice::integrate( const double & dt )
 	  double lvelocityLimit = model_.velocityLimit[lvel_index];
 	  	  
 	  // Integration.
+
+	  // Set acceleration from control and integrates to find velocity.
 	  if (it_control_type->second.SoTcontrol==ACCELERATION)
 	    {
 	      acceleration_[lvel_index] = controlIN[lctl_index];
@@ -423,6 +442,7 @@ void PinocchioDevice::integrate( const double & dt )
 	      
 	      shouldIntegrateVelocity=true;
 	    }
+	  // Set velocity from control and set boolean to perform velocity integration later on.
 	  else if (it_control_type->second.SoTcontrol==VELOCITY)
 	    {
 	      acceleration_[lvel_index]=0;
@@ -434,6 +454,7 @@ void PinocchioDevice::integrate( const double & dt )
 	      
 	      shouldIntegrateVelocity=true;
 	    }
+	  // Position from control is directly provided.
 	  else if (it_control_type->second.SoTcontrol==POSITION)
 	    {
 	      acceleration_[lvel_index]=0;
@@ -509,6 +530,7 @@ ParseYAMLString(const std::string & aYamlString)
 	  if (r<0) return r;
 	}
     }
+  UpdateSignals();
   return 0;
 }
 
@@ -720,20 +742,25 @@ void PinocchioDevice::CreateAnImuSignal(const std::string &imu_sensor_name)
 int PinocchioDevice::
 UpdateSignals()
 {
-  pseudoTorqueSOUT_ = new Signal<Vector, int>
-    ("PinocchioDevice("+getName()+")::output(vector)::ptorque" );
-  
-  currentsSOUT_ = new Signal<Vector, int>
-    ("PinocchioDevice("+getName()+")::output(vector)::currents" );
+  if ((torque_index_!=0) && (pseudoTorqueSOUT_!=0))
+    pseudoTorqueSOUT_ = new Signal<Vector, int>
+      ("PinocchioDevice("+getName()+")::output(vector)::ptorque" );
   
-  temperatureSOUT_ = new Signal<Vector, int>
-    ("PinocchioDevice("+getName()+")::output(vector)::temperatures");
+  if ((current_index_!=0) && (currentsSOUT_!=0))
+    currentsSOUT_ = new Signal<Vector, int>
+      ("PinocchioDevice("+getName()+")::output(vector)::currents" );
 
-  motor_anglesSOUT_ = new Signal<Vector, int>
-    ("PinocchioDevice("+getName()+")::output(vector)::motor_angles");
+  if ((temperature_index_!=0) && (temperatureSOUT_!=0))
+    temperatureSOUT_ = new Signal<Vector, int>
+      ("PinocchioDevice("+getName()+")::output(vector)::temperatures");
 
-  joint_anglesSOUT_ = new Signal<Vector, int>
-    ("PinocchioDevice("+getName()+")::output(vector)::joint_angles");
+  if ((motor_angle_index_!=0) && (motor_anglesSOUT_!=0))
+    motor_anglesSOUT_ = new Signal<Vector, int>
+      ("PinocchioDevice("+getName()+")::output(vector)::motor_angles");
+
+  if ((joint_angle_index_!=0) && (joint_anglesSOUT_!=0))
+    joint_anglesSOUT_ = new Signal<Vector, int>
+      ("PinocchioDevice("+getName()+")::output(vector)::joint_angles");
   
   return 0;
 }
@@ -1000,8 +1027,9 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
 {
   ODEBUG5FULL("start");
   sotDEBUGIN(25) ;
-  vector<double> anglesOut;
-  anglesOut.resize(state_.size());
+  const Vector & controlIN = controlSIN.accessCopy();
+  vector<double> lcontrolOut;
+  lcontrolOut.resize(controlIN.size());
   
   // Integrate control
   increment(timestep_);
@@ -1010,13 +1038,24 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
   ODEBUG5FULL("state = "<< state_);
 
   // Specify the joint values for the controller.
-  if ((int)anglesOut.size()!=state_.size()-6)
-    anglesOut.resize(state_.size()-6);
-
-  for(unsigned int i=6; i < state_.size();++i)
-    anglesOut[i-6] = state_(i);
-  controlOut["control"].setValues(anglesOut);
-
+  JointSHWControlType_iterator it_control_type;
+  for(it_control_type  = jointPinocchioDevices_.begin();
+      it_control_type != jointPinocchioDevices_.end();
+      it_control_type++)
+    {
+      int lctl_index = it_control_type->second.control_index;
+      if (it_control_type->second.HWcontrol==TORQUE)
+	lcontrolOut[lctl_index] = controlIN[lctl_index];
+      else if (it_control_type->second.HWcontrol==POSITION)
+	{
+	  int pino_index = it_control_type->second.pinocchio_index;
+	  int lpos_index = model_.joints[pino_index].idx_q();
+	  lcontrolOut[lctl_index] = position_[lpos_index];
+	}
+    }
+  
+  controlOut["control"].setValues(lcontrolOut);
+  
   ODEBUG5FULL("end");
   sotDEBUGOUT(25) ;
 }

unitTesting/test_pinocchio_device.cpp

@@ -29,220 +29,188 @@ void CreateYAMLFILE()
 {
   YAML::Emitter yaml_out;
   YAML::Node aNode,yn_map_hw_sot_c,yn_map_sensors;
-  unsigned int index_vec_ctl=0;
   yn_map_hw_sot_c = aNode["map_hardware_sot_control"];
   yn_map_sensors = aNode["sensors"];
+  /*
   yn_map_hw_sot_c["waist"];
   yn_map_hw_sot_c["waist"]["hw"] = "POSITION";
   yn_map_hw_sot_c["waist"]["sot"] = "POSITION";
-  yn_map_hw_sot_c["waist"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["waist"]["controlPos"] = 0;
   yn_map_hw_sot_c["waist"]["sensors"] = "";
-  index_vec_ctl+=6;
+  */
+  yn_map_hw_sot_c["LLEG_HIP_P"];
+  yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = 6;
+  yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
+  
+  yn_map_hw_sot_c["LLEG_HIP_R"];
+  yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = 7;
+  yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
+  
+  yn_map_hw_sot_c["LLEG_HIP_Y"];
+  yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = 8;
+  yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
+  
+  yn_map_hw_sot_c["LLEG_KNEE"];
+  yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = 9;
+  yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
+  
+  yn_map_hw_sot_c["LLEG_ANKLE_P"];
+  yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = 10;
+  yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
+
+  yn_map_hw_sot_c["LLEG_ANKLE_R"];
+  yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = 11;
+  yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
   
   yn_map_hw_sot_c["RLEG_HIP_P"];
   yn_map_hw_sot_c["RLEG_HIP_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_HIP_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = 12;
   yn_map_hw_sot_c["RLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
   
   yn_map_hw_sot_c["RLEG_HIP_R"];
   yn_map_hw_sot_c["RLEG_HIP_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_HIP_R"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["RLEG_HIP_R"]["controlPos"] = 13;
   yn_map_hw_sot_c["RLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RLEG_HIP_Y"];
   yn_map_hw_sot_c["RLEG_HIP_Y"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_HIP_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = 14;
   yn_map_hw_sot_c["RLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
-
 
   yn_map_hw_sot_c["RLEG_KNEE"];
   yn_map_hw_sot_c["RLEG_KNEE"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_KNEE"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = 15;
   yn_map_hw_sot_c["RLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RLEG_ANKLE_P"];
   yn_map_hw_sot_c["RLEG_ANKLE_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_ANKLE_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = 16;
   yn_map_hw_sot_c["RLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
   
   yn_map_hw_sot_c["RLEG_ANKLE_R"];
   yn_map_hw_sot_c["RLEG_ANKLE_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RLEG_ANKLE_R"]["sot"] = "VELOCITY";  
-  yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = 17;
   yn_map_hw_sot_c["RLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
-  yn_map_hw_sot_c["LLEG_HIP_P"];
-  yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
-  
-  
-  yn_map_hw_sot_c["LLEG_HIP_R"];
-  yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
-  
-  yn_map_hw_sot_c["LLEG_HIP_Y"];
-  yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
-  index_vec_ctl+=1;
-  
-  yn_map_hw_sot_c["LLEG_KNEE"];
-  yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
-  index_vec_ctl+=1;
-  
-  yn_map_hw_sot_c["LLEG_ANKLE_P"];
-  yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
+  yn_map_hw_sot_c["WAIST_P"];
+  yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["WAIST_P"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["WAIST_P"]["controlPos"] = 18;
+  yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
 
-  yn_map_hw_sot_c["LLEG_ANKLE_R"];
-  yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
+  yn_map_hw_sot_c["WAIST_R"];
+  yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["WAIST_R"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["WAIST_R"]["controlPos"] = 19;
+  yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
+  
+  yn_map_hw_sot_c["CHEST"];
+  yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION";
+  yn_map_hw_sot_c["CHEST"]["sot"] = "VELOCITY";
+  yn_map_hw_sot_c["CHEST"]["controlPos"] = 20;
+  yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
 
   yn_map_hw_sot_c["RARM_SHOULDER_P"];
   yn_map_hw_sot_c["RARM_SHOULDER_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_SHOULDER_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = 21;
   yn_map_hw_sot_c["RARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RARM_SHOULDER_R"];
   yn_map_hw_sot_c["RARM_SHOULDER_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_SHOULDER_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = 22;
   yn_map_hw_sot_c["RARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; 
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RARM_SHOULDER_Y"];
   yn_map_hw_sot_c["RARM_SHOULDER_Y"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = 23;
   yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RARM_ELBOW"];
   yn_map_hw_sot_c["RARM_ELBOW"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_ELBOW"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = 24;
   yn_map_hw_sot_c["RARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
   
   yn_map_hw_sot_c["RARM_WRIST_Y"];
   yn_map_hw_sot_c["RARM_WRIST_Y"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_WRIST_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = 25;
   yn_map_hw_sot_c["RARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
   
   yn_map_hw_sot_c["RARM_WRIST_P"];
   yn_map_hw_sot_c["RARM_WRIST_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_WRIST_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = 26;
   yn_map_hw_sot_c["RARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["RARM_WRIST_R"];
   yn_map_hw_sot_c["RARM_WRIST_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["RARM_WRIST_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = 27;
   yn_map_hw_sot_c["RARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";   
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["LARM_SHOULDER_P"];
   yn_map_hw_sot_c["LARM_SHOULDER_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_SHOULDER_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = 28;
   yn_map_hw_sot_c["LARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["LARM_SHOULDER_R"];
   yn_map_hw_sot_c["LARM_SHOULDER_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_SHOULDER_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = 29;
   yn_map_hw_sot_c["LARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["LARM_SHOULDER_Y"];
   yn_map_hw_sot_c["LARM_SHOULDER_Y"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = 30;
   yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
-
   
   yn_map_hw_sot_c["LARM_ELBOW"];
   yn_map_hw_sot_c["LARM_ELBOW"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_ELBOW"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = 31;
   yn_map_hw_sot_c["LARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["LARM_WRIST_Y"];
   yn_map_hw_sot_c["LARM_WRIST_Y"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_WRIST_Y"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = 32;
   yn_map_hw_sot_c["LARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_hw_sot_c["LARM_WRIST_P"];
   yn_map_hw_sot_c["LARM_WRIST_P"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_WRIST_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = 33;
   yn_map_hw_sot_c["LARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";  
-  index_vec_ctl+=1;  
 
   yn_map_hw_sot_c["LARM_WRIST_R"];
   yn_map_hw_sot_c["LARM_WRIST_R"]["hw"] = "POSITION";
   yn_map_hw_sot_c["LARM_WRIST_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = index_vec_ctl;
+  yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = 34;
   yn_map_hw_sot_c["LARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";
-  index_vec_ctl+=1;
-
-  yn_map_hw_sot_c["WAIST_P"];
-  yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["WAIST_P"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["WAIST_P"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;  
-
-  yn_map_hw_sot_c["WAIST_R"];
-  yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["WAIST_R"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["WAIST_R"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
-  
-  yn_map_hw_sot_c["CHEST"];
-  yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION";
-  yn_map_hw_sot_c["CHEST"]["sot"] = "VELOCITY";
-  yn_map_hw_sot_c["CHEST"]["controlPos"] = index_vec_ctl;
-  yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
-  index_vec_ctl+=1;
 
   yn_map_sensors["force_torque"];
   yn_map_sensors["force_torque"]["left_ankle_ft"];
@@ -353,7 +321,7 @@ int ReadYAMLFILE(dg::sot::PinocchioDevice &aDevice, unsigned int debug_mode)
 namespace dg = dynamicgraph;
 int main(int, char **)
 {
-  unsigned int debug_mode=0;
+  unsigned int debug_mode=5;
     
   std::string robot_description;
   ifstream urdfFile;
@@ -395,16 +363,33 @@ int main(int, char **)
   const se3::Model & aModel = aDevice.getModel();
 
   const dg::Vector & aPosition = aDevice.stateSOUT_(2001);
-  double diff=0;
-  for (unsigned int j=0;j<aPosition.size();j++)
+  double diff=0,ldiff;
+
+  dgsot::JointSHWControlType_iterator it_control_type;
+  for(it_control_type  = aDevice.jointPinocchioDevices_.begin();
+      it_control_type != aDevice.jointPinocchioDevices_.end();
+      it_control_type++)
     {
-      diff += fabs(aPosition(j) - aModel.lowerPositionLimit[j]);
-      if (debug_mode>1)
-	std::cout << diff << " " << aModel.names[j] << " " 
-		  << aPosition(j) << " "<<  aModel.lowerPositionLimit[j]
-		  << std::endl;
+      int lctl_index = it_control_type->second.control_index;
+      if (it_control_type->second.HWcontrol==dgsot::POSITION)
+	{
+	  int pino_index = it_control_type->second.pinocchio_index;
+	  if (pino_index!=-1)
+	    {
+	      int lpos_index = aModel.joints[pino_index].idx_q();
+	      int lvel_index = aModel.joints[pino_index].idx_v();
+	      ldiff = (aPosition[lpos_index] - aModel.lowerPositionLimit[lpos_index]);
+	      diff += ldiff;
+	      std::cout << ldiff << " " << aModel.names[pino_index] << " "
+			<< aPosition[lpos_index]  << " "
+			<< aModel.lowerPositionLimit[lpos_index] << " "
+			<< -aModel.velocityLimit[lvel_index]
+			<< std::endl;
+	    }
+	}
     }
+
   if (diff>1e-3)
     return -1;
-  return 1;
+  return 0;
 }