Commit 9abd087d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Implements a pointer base handling of the list of nodes.

parent e7283c8b
......@@ -145,6 +145,7 @@ INSTALL(FILES python/dynamic_graph/sot/torque_control/utils/__init__.py
INSTALL(FILES python/dynamic_graph/sot/torque_control/tests/__init__.py
python/dynamic_graph/sot/torque_control/tests/robot_data_test.py
python/dynamic_graph/sot/torque_control/tests/test_control_manager.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/tests)
ADD_SUBDIRECTORY(src)
......
......@@ -233,6 +233,17 @@ namespace dynamicgraph {
bool joints_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
......@@ -251,21 +262,15 @@ namespace dynamicgraph {
void display(std::ostream &os) const;
}; // struct RobotUtil
static RobotUtil VoidRobotUtil;
RobotUtil * RefVoidRobotUtil();
RobotUtil & getRobotUtil(std::string &robotName);
RobotUtil * getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtil &createRobotUtil(std::string &robotName);
RobotUtil * createRobotUtil(std::string &robotName);
bool base_se3_to_sot(Eigen::ConstRefVector pos,
Eigen::ConstRefMatrix R,
Eigen::RefVector q_sot);
bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
} // namespace torque_control
} // namespace sot
......
......@@ -169,8 +169,7 @@ namespace dynamicgraph {
}
protected:
RobotUtil & m_robot_util;
Eigen::VectorXd::Index m_nJoints; /// Number of joints
RobotUtil * m_robot_util;
pininvdyn::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
......@@ -95,6 +95,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
/* --- COMMANDS --- */
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
......@@ -121,7 +123,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
RobotUtil & m_robot_util;
RobotUtil * m_robot_util;
}; // class FreeFlyerLocator
......
......@@ -232,7 +232,7 @@ namespace dynamicgraph {
pininvdyn::math::Vector m_v_urdf;
unsigned int m_timeLast;
RobotUtil & m_robot_util;
RobotUtil * m_robot_util;
}; // class InverseDynamicsBalanceController
} // namespace torque_control
......
......@@ -39,6 +39,7 @@
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/common.hh>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
......@@ -94,7 +95,7 @@ namespace dynamicgraph {
}
protected:
unsigned int m_nJoints;
RobotUtil * m_robot_util; /// Robot Util
Eigen::VectorXd m_pwmDes;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
import numpy
testRobotPath="/opt/openrobots/share/hrp2_14_description/urdf/hrp2_14_reduced.urdf"
controlDT=0.005
maxCurrent=5
urdftosot=(12,13,14,15,23,24,25,26,27,28,29,16,17,18,19,20,21,22,6,7,8,9,10,11,0,1,2,3,4,5)
class initRobotData:
nbJoints=30
testRobotPath="/opt/openrobots/share/hrp2_14_description/urdf/hrp2_14_reduced.urdf"
controlDT=0.005
maxCurrent=5
mapJointNameToID={
urdftosot=(12,13,14,15,23,24,25,26,27,28,29,16,17,18,19,20,21,22,6,7,8,9,10,11,0,1,2,3,4,5)
mapJointNameToID={
'rhy': 0,
'rhr': 1,
'rhp': 2,
......@@ -37,9 +40,9 @@ mapJointNameToID={
'lwy': 27,
'lwp': 28,
'lh': 29
}
mapJointLimits={
}
mapJointLimits={
0 : [-0.785398, 0.523599],
1 : [-0.610865, 0.349066],
2 : [-2.18166, 0.733038],
......@@ -62,39 +65,73 @@ mapJointLimits={
19 : [-2.3911, 0.0349066],
20 : [-1.6057, 1.6057],
21 : [-1.6057, 1.6057],
22 : [-1.0, 1.0],
22 : [-1.0, 1.0],
23 : [-3.14159, 1.0472],
24 : [-0.174533, 1.65806],
25 : [-1.6057, 1.6057],
26 : [-2.3911, 0.0349066],
27 : [-1.6057, 1.6057],
28 : [-1.6057, 1.6057],
29 : [-1.0, 1.0]
}
fMax=numpy.array([100.0,100.0,300.0,80.0,80.0,30.0])
fMin=-fMax
mapForceIdToForceLimits={
29 : [-1.0, 1.0]
}
fMax=numpy.array([100.0,100.0,300.0,80.0,80.0,30.0])
fMin=-fMax
mapForceIdToForceLimits={
0: [fMin,fMax],
1: [fMin,fMax],
2: [fMin,fMax],
3: [fMin,fMax]
}
mapNameToForceId={
}
mapNameToForceId={
"rf": 0,
"lf": 1,
"rh": 2,
"lh": 3
}
indexOfForceSensors= ()
FootFrameNames= {
}
indexOfForceSensors= ()
FootFrameNames= {
"Right": "RLEG_JOINT5",
"Left" : "LLEG_JOINT5"
}
}
RightFootSensorXYZ = (0.0,0.0,-0.085)
def init_and_set_controller_manager(self, cm):
# Init should be called before addCtrlMode
# because the size of state vector must be known.
cm.init(self.controlDT,self.testRobotPath,self.maxCurrent)
# Set the map from joint name to joint ID
for key in self.mapJointNameToID:
cm.setNameToId(key,self.mapJointNameToID[key])
# Set the map joint limits for each id
for key in self.mapJointLimits:
cm.setJointLimitsFromId(key,self.mapJointLimits[key][0], \
self.mapJointLimits[key][1])
# Set the force limits for each id
for key in self.mapForceIdToForceLimits:
cm.setForceLimitsFromId(key,tuple(self.mapForceIdToForceLimits[key][0]), \
tuple(self.mapForceIdToForceLimits[key][1]))
# Set the force sensor id for each sensor name
for key in self.mapNameToForceId:
cm.setForceNameToForceId(key,self.mapNameToForceId[key])
# Set the map from the urdf joint list to the sot joint list
cm.setJointsUrdfToSot(self.urdftosot)
RightFootSensorXYZ = (0.0,0.0,-0.085)
# Set the foot frame name
for key in self.FootFrameNames:
cm.setFootFrameName(key,self.FootFrameNames[key])
cm.setRightFootSoleXYZ(self.RightFootSensorXYZ)
cm.setDefaultMaxCurrent(-10.0)
cm.setDefaultMaxCurrent(self.maxCurrent)
cm.getDefaultMaxCurrent()
......@@ -422,7 +422,7 @@ def one_foot_balance_test(dt=0.001, delay=0.01):
print "com_ref", np.array(ent.com_traj_gen.x.value),
print "f_rf %.1f"%f_rf[2,0].T, "f_lf %.1f"%f_lf[2,0].T;
com[2,0] = 0.0;
+ com[2,0] = 0.0;
ent.simulator.updateComPositionInViewer(com);
if(i==1):
......
......@@ -28,6 +28,13 @@ namespace dynamicgraph
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
RobotUtil VoidRobotUtil;
RobotUtil * RefVoidRobotUtil()
{
return & VoidRobotUtil;
}
void ForceLimits::display(std::ostream &os) const
{
......@@ -264,7 +271,106 @@ namespace dynamicgraph
q_urdf[idx]=q_sot[m_urdf_to_sot[idx]];
return true;
}
bool RobotUtil::
velocity_urdf_to_sot(Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot)
{
assert(q_urdf.size()==m_nbJoints+6);
assert(q_sot.size()==m_nbJoints+6);
if (m_nbJoints==0)
{
SEND_MSG("velocity_urdf_to_sot should be called", MSG_TYPE_ERROR);
return false;
}
v_sot.head<6>() = v_urdf.head<6>();
joints_urdf_to_sot(v_urdf.tail(m_nbJoints),
v_sot.tail(m_nbJoints));
return true;
}
bool RobotUtil::
velocity_sot_to_urdf(Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf)
{
assert(q_urdf.size()==m_nbJoints+6);
assert(q_sot.size()==m_nbJoints+6);
if (m_nbJoints==0)
{
SEND_MSG("velocity_sot_to_urdf should be called", MSG_TYPE_ERROR);
return false;
}
v_urdf.head<6>() = v_sot.head<6>();
joints_sot_to_urdf(v_sot.tail(m_nbJoints),
v_urdf.tail(m_nbJoints));
return true;
}
bool RobotUtil::
base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
{
assert(q_urdf.size()==7);
assert(q_sot.size()==6);
// ********* Quat to RPY *********
const double W = q_urdf[6];
const double X = q_urdf[3];
const double Y = q_urdf[4];
const double Z = q_urdf[5];
const Eigen::Matrix3d R = Eigen::Quaterniond(W, X, Y, Z).toRotationMatrix();
return base_se3_to_sot(q_urdf.head<3>(), R, q_sot);
}
bool RobotUtil::
base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
{
assert(q_urdf.size()==7);
assert(q_sot.size()==6);
// ********* RPY to Quat *********
const double r = q_sot[3];
const double p = q_sot[4];
const double y = q_sot[5];
const Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
const Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
const Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());
const Eigen::Quaternion<double> quat = yawAngle * pitchAngle * rollAngle;
q_urdf[0 ]=q_sot[0 ]; //BASE
q_urdf[1 ]=q_sot[1 ];
q_urdf[2 ]=q_sot[2 ];
q_urdf[3 ]=quat.x();
q_urdf[4 ]=quat.y();
q_urdf[5 ]=quat.z();
q_urdf[6 ]=quat.w();
return true;
}
bool RobotUtil::
config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot)
{
assert(q_urdf.size()==m_nbJoints+7);
assert(q_sot.size()==m_nbJoints+6);
base_urdf_to_sot(q_urdf.head<7>(), q_sot.head<6>());
joints_urdf_to_sot(q_urdf.tail(m_nbJoints), q_sot.tail(m_nbJoints));
return true;
}
bool RobotUtil::
config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf)
{
assert(q_urdf.size()==m_nbJoints+7);
assert(q_sot.size()==m_nbJoints+6);
base_sot_to_urdf(q_sot.head<6>(), q_urdf.head<7>());
joints_sot_to_urdf(q_sot.tail(m_nbJoints),
q_urdf.tail(m_nbJoints));
}
void RobotUtil::
display(std::ostream &os) const
{
......@@ -369,28 +475,38 @@ namespace dynamicgraph
return true;
}
std::map<std::string,RobotUtil> sgl_map_name_to_robot_util;
std::map<std::string,RobotUtil *> sgl_map_name_to_robot_util;
RobotUtil & getRobotUtil(std::string &robotName)
RobotUtil * getRobotUtil(std::string &robotName)
{
std::map<std::string,RobotUtil>::iterator it =
std::map<std::string,RobotUtil *>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
if (it!=sgl_map_name_to_robot_util.end())
return it->second;
return VoidRobotUtil;
return RefVoidRobotUtil();
}
bool isNameInRobotUtil(std::string &robotName)
{
std::map<std::string,RobotUtil>::iterator it =
std::map<std::string,RobotUtil *>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
if (it!=sgl_map_name_to_robot_util.end())
return false;
return true;
return true;
return false;
}
RobotUtil & createRobotUtil(std::string &robotName)
RobotUtil * createRobotUtil(std::string &robotName)
{
return sgl_map_name_to_robot_util[robotName];
std::map<std::string,RobotUtil *>::iterator it =
sgl_map_name_to_robot_util.find(robotName);
if (it==sgl_map_name_to_robot_util.end())
{
sgl_map_name_to_robot_util[robotName]= new RobotUtil;
it = sgl_map_name_to_robot_util.find(robotName);
return it->second;
}
std::cout << "Another robot is already in the map for " << robotName
<< std::endl;
return RefVoidRobotUtil();
}
} // torque_control
......
......@@ -72,11 +72,10 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(signOfControl, dynamicgraph::Vector, m_pwmDesSOUT)
,CONSTRUCT_SIGNAL_OUT(signOfControlFiltered,dynamicgraph::Vector, m_pwmDesSafeSOUT)
,CONSTRUCT_SIGNAL_OUT(pwmDesSafe,dynamicgraph::Vector, INPUT_SIGNALS << m_pwmDesSOUT)
,m_robot_util(VoidRobotUtil)
,m_robot_util(RefVoidRobotUtil())
,m_initSucceeded(false)
,m_emergency_stop_triggered(false)
,m_is_first_iter(true)
,m_nJoints(0)
{
Entity::signalRegistration( INPUT_SIGNALS << m_pwmDesSOUT << m_pwmDesSafeSOUT << m_signOfControlFilteredSOUT << m_signOfControlSOUT);
......@@ -188,26 +187,30 @@ namespace dynamicgraph
m_robot = new pininvdyn::RobotWrapper(urdfFile, package_dirs, se3::JointModelFreeFlyer());
std::string localName("control-manager-robot");
if (isNameInRobotUtil(localName))
m_robot_util = createRobotUtil(localName);
else
m_robot_util = getRobotUtil(localName);
if (!isNameInRobotUtil(localName))
{
m_robot_util = createRobotUtil(localName);
}
else
{
m_robot_util = getRobotUtil(localName);
}
m_robot_util.m_urdf_filename = urdfFile;
m_robot_util->m_urdf_filename = urdfFile;
addCommand("getJointsUrdfToSot",
makeDirectGetter(*this, &m_robot_util.m_dgv_urdf_to_sot,
makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot,
docDirectSetter("Display map Joints From URDF to SoT.",
"Vector of integer for mapping")));
m_nJoints = m_robot->nv()-6;
m_robot_util->m_nbJoints = m_robot->nv()-6;
m_jointCtrlModes_current.resize(m_nJoints);
m_jointCtrlModes_previous.resize(m_nJoints);
m_jointCtrlModesCountDown.resize(m_nJoints,0);
m_signIsPos.resize(m_nJoints, false);
m_changeSignCpt.resize(m_nJoints, 0);
m_winSizeAdapt.resize(m_nJoints, 0);
m_jointCtrlModes_current.resize(m_robot_util->m_nbJoints);
m_jointCtrlModes_previous.resize(m_robot_util->m_nbJoints);
m_jointCtrlModesCountDown.resize(m_robot_util->m_nbJoints,0);
m_signIsPos.resize(m_robot_util->m_nbJoints, false);
m_changeSignCpt.resize(m_robot_util->m_nbJoints, 0);
m_winSizeAdapt.resize(m_robot_util->m_nbJoints, 0);
}
......@@ -232,8 +235,8 @@ namespace dynamicgraph
//const Eigen::VectorXd& base6d_encoders = m_base6d_encodersSIN(iter);
if(s.size()!=(Eigen::VectorXd::Index) m_nJoints)
s.resize(m_nJoints);
if(s.size()!=(Eigen::VectorXd::Index) m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
getProfiler().start(PROFILE_PWM_DESIRED_COMPUTATION);
{
......@@ -242,7 +245,7 @@ namespace dynamicgraph
(*m_ctrlInputsSIN[i])(iter);
int cm_id, cm_id_prev;
for(unsigned int i=0; i<m_nJoints; i++)
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
{
cm_id = m_jointCtrlModes_current[i].id;
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
......@@ -289,9 +292,9 @@ namespace dynamicgraph
const dynamicgraph::Vector& bemfFactor = m_bemfFactorSIN(iter);
const dynamicgraph::Vector& percentageDriverDeadZoneCompensation = m_percentageDriverDeadZoneCompensationSIN(iter);
const dynamicgraph::Vector& signWindowsFilterSize = m_signWindowsFilterSizeSIN(iter);
if(s.size()!=m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
if(s.size()!=m_nJoints)
s.resize(m_nJoints);
if(!m_emergency_stop_triggered)
{
......@@ -302,7 +305,7 @@ namespace dynamicgraph
m_emergency_stop_triggered = true;
SEND_MSG("Emergency Stop has been triggered by an external entity",MSG_TYPE_ERROR);
}
for(unsigned int i=0; i<m_nJoints; i++)
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
{
//Trigger sign filter**********************
/* _ _ _________________________ _
......@@ -344,7 +347,7 @@ namespace dynamicgraph
{
m_emergency_stop_triggered = true;
SEND_MSG("Estimated torque "+toString(tau(i))+" > max torque "+toString(tau_max(i))+
" for joint "+ m_robot_util.get_name_from_id(i), MSG_TYPE_ERROR);
" for joint "+ m_robot_util->get_name_from_id(i), MSG_TYPE_ERROR);
SEND_MSG(", but predicted torque "+toString(tau_predicted(i))+" < "+toString(tau_max(i)), MSG_TYPE_ERROR);
SEND_MSG(", and current "+toString(pwmDes(i))+"A < "+toString(m_maxCurrent)+"A", MSG_TYPE_ERROR);
break;
......@@ -354,7 +357,7 @@ namespace dynamicgraph
{
m_emergency_stop_triggered = true;
SEND_MSG("Predicted torque "+toString(tau_predicted(i))+" > max torque "+toString(tau_max(i))+
" for joint "+m_robot_util.get_name_from_id(i), MSG_TYPE_ERROR);
" for joint "+m_robot_util->get_name_from_id(i), MSG_TYPE_ERROR);
SEND_MSG(", but estimated torque "+toString(tau(i))+" < "+toString(tau_max(i)), MSG_TYPE_ERROR);
SEND_MSG(", and current "+toString(pwmDes(i))+"A < "+toString(m_maxCurrent)+"A", MSG_TYPE_ERROR);
break;
......@@ -369,7 +372,7 @@ namespace dynamicgraph
(fabs(s(i)) > m_maxCurrent * FROM_CURRENT_TO_12_BIT_CTRL) )
{
m_emergency_stop_triggered = true;
SEND_MSG("Joint "+m_robot_util.get_name_from_id(i)+" desired current is too large: "+
SEND_MSG("Joint "+m_robot_util->get_name_from_id(i)+" desired current is too large: "+
toString(pwmDes(i))+"A > "+toString(m_maxCurrent)+"A", MSG_TYPE_ERROR);
SEND_MSG(", but estimated torque "+toString(tau(i))+" < "+toString(tau_max(i)), MSG_TYPE_ERROR);
SEND_MSG(", and predicted torque "+toString(tau_predicted(i))+" < "+toString(tau_max(i)), MSG_TYPE_ERROR);
......@@ -387,11 +390,17 @@ namespace dynamicgraph
DEFINE_SIGNAL_OUT_FUNCTION(signOfControl,dynamicgraph::Vector)
{
const dynamicgraph::Vector& pwmDes = m_pwmDesSOUT(iter);
<<<<<<< HEAD
if(s.size()!=(Eigen::VectorXd::Index)m_nJoints)
s.resize(m_nJoints);
for(unsigned int i=0; i<m_nJoints; i++)
=======
if(s.size()!=(Eigen::VectorXd::Index)m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
>>>>>>> Implements a pointer base handling of the list of nodes.
{
if (pwmDes(i)>0)
s(i)= 1;
......@@ -407,9 +416,15 @@ namespace dynamicgraph
{
const dynamicgraph::Vector& pwmDesSafe = m_pwmDesSafeSOUT(iter);
<<<<<<< HEAD
if(s.size()!=N_JOINTS)
s.resize(N_JOINTS);
for(unsigned int i=0; i<N_JOINTS; i++)
=======
if(s.size()!=m_robot_util->m_nbJoints)
s.resize(m_robot_util->m_nbJoints);
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
>>>>>>> Implements a pointer base handling of the list of nodes.
{
if (pwmDesSafe(i)==0)
s(i)=0;
......@@ -466,7 +481,7 @@ namespace dynamicgraph
if(jointName=="all")
{
for(unsigned int i=0; i<m_nJoints; i++)
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
setCtrlMode(i,cm);
}
else
......@@ -506,7 +521,7 @@ namespace dynamicgraph
}
}
else
SEND_MSG("Cannot change control mode of joint "+m_robot_util.get_name_from_id(jid)+
SEND_MSG("Cannot change control mode of joint "+m_robot_util->get_name_from_id(jid)+
" because either it has already the specified ctrl mode or its previous"+
" ctrl mode transition has not terminated yet", MSG_TYPE_ERROR);
}
......@@ -516,8 +531,8 @@ namespace dynamicgraph
if(jointName=="all")
{
stringstream ss;
for(unsigned int i=0; i<m_nJoints; i++)
ss<<m_robot_util.get_name_from_id(i) <<" "
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
ss<<m_robot_util->get_name_from_id(i) <<" "
<<m_jointCtrlModes_current[i]<<"; ";
SEND_MSG(ss.str(),MSG_TYPE_INFO);
return;
......@@ -552,7 +567,7 @@ namespace dynamicgraph
return;