25 #include <yarp/os/Time.h>
26 #include <yarp/dev/PolyDriver.h>
27 #include <ace/config.h>
28 #include <ace/Log_Msg.h>
29 #include <yarp/os/LogStream.h>
30 #include <yarp/os/Log.h>
45 #include <yarp/os/Log.h>
46 #include <yarp/os/LogStream.h>
47 #include <yarp/dev/ControlBoardInterfacesImpl.h>
53 #pragma warning(once:4355)
61 using namespace yarp::os;
68 yDebug(
" S:%d R:%d Ch:%d M:%d\n",
getSender(m),
getRcp(m), m.getData()[0]&0x80, m.getData()[0]&0x7F);
74 yError(
"%s not yet implemented for CanBusMotionControl\n", txt);
81 yWarning(
"%s not yet implemented for CanBusMotionControl\n", txt);
88 yError(
"%s has been deprecated for CanBusMotionControl\n", txt);
96 bool validate(Bottle &input, Bottle &
out,
const std::string &key1,
const std::string &txt,
int size)
98 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
101 yError(
"%s not found\n", key1.c_str());
106 yError(
"%s incorrect number of entries\n", key1.c_str());
115 Bottle &
tmp=input.findGroup(key1.c_str(), txt.c_str());
122 yError(
"%s incorrect number of entries\n", key1.c_str());
188 double tmpDt=st-_stamp;
289 case icubCanProto_controlmode_idle:
290 yInfo (
"[%d] board %d MODE_IDLE \r\n", net, addr);
292 case icubCanProto_controlmode_position:
293 yInfo (
"[%d] board %d MODE_POSITION \r\n", net, addr);
295 case icubCanProto_controlmode_velocity:
296 yInfo (
"[%d] board %d MODE_VELOCITY \r\n", net, addr);
298 case icubCanProto_controlmode_torque:
299 yInfo (
"[%d] board %d MODE_TORQUE \r\n", net, addr);
301 case icubCanProto_controlmode_impedance_pos:
302 yInfo (
"[%d] board %d MODE_IMPEDANCE_POS \r\n", net, addr);
304 case icubCanProto_controlmode_impedance_vel:
305 yInfo (
"[%d] board %d MODE_IMPEDANCE_VEL \r\n", net, addr);
308 yError (
"[%d] board %d MODE_UNKNOWN \r\n", net, addr);
316 _position_joint.
_value = 0;
317 _position_rotor.
_value = 0;
325 _controlmodeStatus=0;
326 _interactionmodeStatus=0;
345 _mainLoopOverflowCounter=0;
358 PolyDriver polyDriver;
369 bool initialize (yarp::os::Searchable &config);
370 bool uninitialize ();
374 bool addMessage (
int msg_id,
int joint);
376 bool addMessage (
int id,
int joint,
int msg_id);
380 bool printMessage (
const CanMessage &m);
381 bool dumpBuffers (
void);
394 yDebug(
"%s", buffer);
400 enum { CAN_TIMEOUT = 20, CAN_POLLING_INTERVAL = 10 };
439 char _printBuffer[16384];
447 std::recursive_mutex *backdoor_mutex;
456 canEchoEnabled=
false;
457 backdoor_mutex=
nullptr;
466 canEchoEnabled = echo;
469 virtual void onRead(Bottle &b);
478 double dval[6] = {0,0,0,0,0,0};
480 std::lock_guard<std::recursive_mutex> lck(*backdoor_mutex);
483 int commandId = b.get(0).asInt32();
486 static double timePrev=Time::now();
487 static int count_timeout=0;
488 static int count_saturation_i[6]={0,0,0,0,0,0};
489 static int count_saturation=0;
490 double timeNow=Time::now();
491 double diff = timeNow - timePrev;
496 yWarning(
"PORT: %s **** TIMEOUT : %f COUNT: %d \n", this->getName().c_str(), diff, count_timeout);
498 timePrev=Time::now();
503 dval[0] = b.get(1).asFloat64();
504 dval[1] = b.get(2).asFloat64();
505 dval[2] = b.get(3).asFloat64();
506 dval[3] = b.get(4).asFloat64();
507 dval[4] = b.get(5).asFloat64();
511 dval[0] = b.get(1).asFloat64();
512 dval[1] = b.get(2).asFloat64();
513 dval[2] = b.get(3).asFloat64();
514 dval[3] = b.get(4).asFloat64();
515 dval[4] = b.get(5).asFloat64();
516 dval[5] = b.get(6).asFloat64();
519 dval[0] = b.get(6).asFloat64();
520 dval[1] = b.get(7).asFloat64();
527 dval[0] = b.get(1).asFloat64();
528 dval[1] = b.get(2).asFloat64();
529 dval[2] = b.get(3).asFloat64();
535 yWarning(
"Got unexpected message on backdoor: %s\n", this->getName().c_str());
540 if (bus && ownerSensor)
543 int boardId = ownerSensor->getId();
544 short int val[6] = {0,0,0,0,0,0};
545 static double curr_time = Time::now();
548 double fullScale = ownerSensor->getScaleFactor()[i];
549 if (dval[i] > fullScale)
551 if (Time::now() - curr_time > 2)
553 yWarning(
"PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
554 curr_time = Time::now();
559 else if (dval[i] < -fullScale)
561 if (Time::now() - curr_time > 2)
563 yWarning(
"PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
564 curr_time = Time::now();
566 dval[i] = -fullScale;
569 val[i] = (
short int)(dval[i] / fullScale * 0x7fff)+0x8000;
573 fakeId = 0x300 + (boardId<<4)+ 0x0A;
574 bus->_writeBuffer[0].setId(fakeId);
575 bus->_writeBuffer[0].getData()[1]=(val[0] >> 8) & 0xFF;
576 bus->_writeBuffer[0].getData()[0]= val[0] & 0xFF;
577 bus->_writeBuffer[0].getData()[3]=(val[1] >> 8) & 0xFF;
578 bus->_writeBuffer[0].getData()[2]= val[1] & 0xFF;
579 bus->_writeBuffer[0].getData()[5]=(val[2] >> 8) & 0xFF;
580 bus->_writeBuffer[0].getData()[4]= val[2] & 0xFF;
581 bus->_writeBuffer[0].setLen(6);
582 bus->_writeMessages++;
589 bus->_echoBuffer[bus->_echoMessages].setId(fakeId);
590 bus->_echoBuffer[bus->_echoMessages].setLen(6);
592 bus->_echoBuffer[bus->_echoMessages].getData()[i]=bus->_writeBuffer[0].getData()[i];
593 bus->_echoMessages++;
597 yError(
"Echobuffer full \n");
602 fakeId = 0x300 + (boardId<<4)+ 0x0B;
603 bus->_writeBuffer[0].setId(fakeId);
604 bus->_writeBuffer[0].getData()[1]=(val[3] >> 8) & 0xFF;
605 bus->_writeBuffer[0].getData()[0]= val[3] & 0xFF;
606 bus->_writeBuffer[0].getData()[3]=(val[4] >> 8) & 0xFF;
607 bus->_writeBuffer[0].getData()[2]= val[4] & 0xFF;
608 bus->_writeBuffer[0].getData()[5]=(val[5] >> 8) & 0xFF;
609 bus->_writeBuffer[0].getData()[4]= val[5] & 0xFF;
610 bus->_writeBuffer[0].setLen(6);
611 bus->_writeMessages++;
618 bus->_echoBuffer[bus->_echoMessages].setId(fakeId);
619 bus->_echoBuffer[bus->_echoMessages].setLen(6);
621 bus->_echoBuffer[bus->_echoMessages].getData()[i]=bus->_writeBuffer[0].getData()[i];
622 bus->_echoMessages++;
626 yError(
"Echobuffer full \n");
637 if (estim_parameters!=0)
656 helper =
new ControlBoardHelper(njoints, aMap, angToEncs, 0, 0);
657 maxHwStep =
new double [jointsNum];
658 maxUserStep =
new double [jointsNum];
659 for (
int i=0; i<jointsNum; i++)
663 helper->posA2E(_stepLimit[i], i, hw_ang, hw_i);
664 maxHwStep[hw_i] = fabs(hw_ang);
665 maxUserStep[i] = _stepLimit[i];
672 double hw_maxval = hw_curr_value + getMaxHwStep(hw_j);
673 double hw_minval = hw_curr_value - getMaxHwStep(hw_j);
679 CanBusMotionControl::torqueControlHelper::torqueControlHelper(
int njoints,
double* p_angleToEncoders,
double* p_newtonsTosens )
682 newtonsToSensor =
new double [jointsNum];
683 angleToEncoders =
new double [jointsNum];
685 if (p_angleToEncoders!=0)
686 memcpy(angleToEncoders, p_angleToEncoders,
sizeof(
double)*jointsNum);
688 for (
int i=0; i<jointsNum; i++) {angleToEncoders[i]=1.0;}
690 if (p_newtonsTosens!=0)
691 memcpy(newtonsToSensor, p_newtonsTosens,
sizeof(
double)*jointsNum);
693 for (
int i=0; i<jointsNum; i++) {newtonsToSensor[i]=1.0;}
699 torqueSensorId =
new int [jointsNum];
700 torqueSensorChan =
new int [jointsNum];
701 maximumTorque =
new double [jointsNum];
702 newtonsToSensor =
new double [jointsNum];
704 memcpy(torqueSensorId,
id,
sizeof(
int)*jointsNum);
706 memset(torqueSensorId, 0,
sizeof(
int)*jointsNum);
708 memcpy(torqueSensorChan, chan,
sizeof(
int)*jointsNum);
710 memset(torqueSensorChan, 0,
sizeof(
int)*jointsNum);
712 memcpy(maximumTorque, maxTrq,
sizeof(
double)*jointsNum);
714 memset(maximumTorque, 0,
sizeof(
double)*jointsNum);
716 memcpy(newtonsToSensor, newtons2sens,
sizeof(
double)*jointsNum);
718 memset(newtonsToSensor, 0,
sizeof(
double)*jointsNum);
725 isVirtualSensor=
false;
727 status=IAnalogSensor::AS_OK;
758 scaleFactor=
new double[channels];
760 for (i=0; i<channels; i++) scaleFactor[i]=1;
763 useCalibration=useCalib;
764 isVirtualSensor=isVirtualS;
787 std::lock_guard<std::mutex> lck(mtx);
794 if (status!=IAnalogSensor::AS_OK)
798 case IAnalogSensor::AS_OVF:
803 case IAnalogSensor::AS_ERROR:
808 case IAnalogSensor::AS_TIMEOUT:
822 for(
int k=0;k<data->
size();k++)
840 bool TBR_AnalogSensor::decode16(
const unsigned char *msg,
int id,
double *
data)
843 const char groupId=(
id&0x00f);
852 data[k]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
853 if (useCalibration==1)
855 data[k]=
data[k]*scaleFactor[k]/float(0x8000);
864 data[k+3]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
865 if (useCalibration==1)
867 data[k+3]=
data[k+3]*scaleFactor[k+3]/float(0x8000);
879 yError(
"Got unexpected class 0x3 msg(s)\n");
890 bool TBR_AnalogSensor::decode8(
const unsigned char *msg,
int id,
double *
data)
892 const char groupId=(
id&0x00f);
899 for(
int k=0;k<=6;k++)
905 for(
int k=0;k<=7;k++)
916 yWarning(
"Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
934 std::lock_guard<std::mutex> lck(mtx);
936 double timeNow=Time::now();
937 for (
unsigned int buff_num=0; buff_num<2; buff_num++)
939 unsigned int size = 0;
940 CanBuffer* buffer_pointer=0;
952 for (i = 0; i < size; i++)
955 unsigned int msgid=0;
956 unsigned char *buff=0;
957 CanMessage& m = (*buffer_pointer)[i];
962 status=IAnalogSensor::AS_OK;
963 const char type=((msgid&0x700)>>8);
964 const char id=((msgid&0x0f0)>>4);
970 timeStamp=Time::now();
974 ret=decode8(buff, msgid,
data->getBuffer());
975 status=IAnalogSensor::AS_OK;
980 ret=decode16(buff, msgid,
data->getBuffer());
981 status=IAnalogSensor::AS_OK;
985 if (len==7 && buff[6] == 1)
987 status=IAnalogSensor::AS_OVF;
991 status=IAnalogSensor::AS_ERROR;
993 ret=decode16(buff, msgid,
data->getBuffer());
1005 if (timeStamp+0.1<timeNow)
1007 status=IAnalogSensor::AS_TIMEOUT;
1014 bool CanBusMotionControlParameters:: setBroadCastMask(Bottle &list,
int MASK)
1018 if (list.get(1).asInt32()==1)
1030 if ((list.get(k+1).asInt32())==1)
1048 sprintf(
tmp,
"Pid%d", j);
1050 Bottle &xtmp = pidsGroup.findGroup(
tmp);
1051 myPid[j].kp = xtmp.get(1).asFloat64();
1052 myPid[j].kd = xtmp.get(2).asFloat64();
1053 myPid[j].ki = xtmp.get(3).asFloat64();
1055 myPid[j].max_int = xtmp.get(4).asFloat64();
1056 myPid[j].max_output = xtmp.get(5).asFloat64();
1058 myPid[j].scale = xtmp.get(6).asFloat64();
1059 myPid[j].offset = xtmp.get(7).asFloat64();
1061 if (xtmp.size()==10)
1063 myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1064 myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1076 sprintf(
tmp,
"TPid%d", j);
1078 Bottle &xtmp = pidsGroup.findGroup(
tmp);
1079 myPid[j].kp = xtmp.get(1).asFloat64();
1080 myPid[j].kd = xtmp.get(2).asFloat64();
1081 myPid[j].ki = xtmp.get(3).asFloat64();
1083 myPid[j].max_int = xtmp.get(4).asFloat64();
1084 myPid[j].max_output = xtmp.get(5).asFloat64();
1086 myPid[j].scale = xtmp.get(6).asFloat64();
1087 myPid[j].offset = xtmp.get(7).asFloat64();
1089 if (xtmp.size()==10)
1091 myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1092 myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1102 if (!
validate(pidsGroup, xtmp,
"kp",
"Pid kp parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].kp = xtmp.get(j+1).asFloat64();
1103 if (!
validate(pidsGroup, xtmp,
"kd",
"Pid kd parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].kd = xtmp.get(j+1).asFloat64();
1104 if (!
validate(pidsGroup, xtmp,
"ki",
"Pid kp parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].ki = xtmp.get(j+1).asFloat64();
1105 if (!
validate(pidsGroup, xtmp,
"maxInt",
"Pid maxInt parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].max_int = xtmp.get(j+1).asFloat64();
1106 if (!
validate(pidsGroup, xtmp,
"maxOutput",
"Pid maxOutput parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].max_output = xtmp.get(j+1).asFloat64();
1107 if (!
validate(pidsGroup, xtmp,
"shift",
"Pid shift parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].scale = xtmp.get(j+1).asFloat64();
1108 if (!
validate(pidsGroup, xtmp,
"ko",
"Pid ko parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].
offset = xtmp.get(j+1).asFloat64();
1109 if (!
validate(pidsGroup, xtmp,
"stictionUp",
"Pid stictionUp",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
1110 if (!
validate(pidsGroup, xtmp,
"stictionDwn",
"Pid stictionDwn",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
1113 xtmp = pidsGroup.findGroup(
"kff");
1116 for (j=0; j<
_njoints; j++) myPid[j].kff = xtmp.get(j+1).asFloat64();
1120 for (j=0; j<
_njoints; j++) myPid[j].kff = 0;
1130 if (!
validate(pidsGroup, xtmp,
"stiffness",
"Pid stiffness parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].stiffness = xtmp.get(j+1).asFloat64();
1131 if (!
validate(pidsGroup, xtmp,
"damping",
"Pid damping parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].damping = xtmp.get(j+1).asFloat64();
1140 if (!
validate(pidsGroup, xtmp,
"debug0",
"debug0",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[0] = xtmp.get(j+1).asFloat64();
1141 if (!
validate(pidsGroup, xtmp,
"debug1",
"debug1",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[1] = xtmp.get(j+1).asFloat64();
1142 if (!
validate(pidsGroup, xtmp,
"debug2",
"debug2",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[2] = xtmp.get(j+1).asFloat64();
1143 if (!
validate(pidsGroup, xtmp,
"debug3",
"debug3",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[3] = xtmp.get(j+1).asFloat64();
1144 if (!
validate(pidsGroup, xtmp,
"debug4",
"debug4",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[4] = xtmp.get(j+1).asFloat64();
1145 if (!
validate(pidsGroup, xtmp,
"debug5",
"debug5",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[5] = xtmp.get(j+1).asFloat64();
1146 if (!
validate(pidsGroup, xtmp,
"debug6",
"debug6",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[6] = xtmp.get(j+1).asFloat64();
1147 if (!
validate(pidsGroup, xtmp,
"debug7",
"debug7",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[7] = xtmp.get(j+1).asFloat64();
1153 if (!
p.check(
"GENERAL",
"section for general motor control parameters")) {
1154 yError(
"Cannot understand configuration parameters\n");
1158 std::string dbg_string =
p.toString().c_str();
1160 int nj =
p.findGroup(
"GENERAL").check(
"Joints",Value(1),
1161 "Number of degrees of freedom").asInt32();
1165 bool useRawJointEncoderData =
false;
1166 bool useRawMotorEncoderData =
false;
1167 Value use_jnt_raw =
p.findGroup(
"GENERAL").find(
"useRawEncoderData");
1168 Value use_mot_raw =
p.findGroup(
"GENERAL").find(
"useRawMotorEncoderData");
1170 if(use_jnt_raw.isNull())
1172 useRawJointEncoderData =
false;
1176 if(!use_jnt_raw.isBool())
1178 yError(
"useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1179 useRawJointEncoderData =
false;
1183 useRawJointEncoderData = use_jnt_raw.asBool();
1184 if(useRawJointEncoderData)
1186 yWarning(
"canBusMotionControl using raw data from encoders! Be careful See 'useRawEncoderData' param in config file \n");
1187 yWarning(
"DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION! \n");
1188 yWarning(
"CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue \n");
1194 if(use_mot_raw.isNull())
1196 useRawMotorEncoderData =
false;
1200 if(!use_mot_raw.isBool())
1202 yError(
"useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1203 useRawMotorEncoderData =
false;
1207 useRawMotorEncoderData = use_mot_raw.asBool();
1208 if(useRawMotorEncoderData)
1210 yWarning(
"canBusMotionControl using raw data from motor encoders");
1215 Value use_limitedPWM =
p.findGroup(
"GENERAL").find(
"useLimitedPWM");
1216 if(use_limitedPWM.isNull())
1222 if(!use_limitedPWM.isBool())
1233 Bottle& canGroup =
p.findGroup(
"CAN");
1237 if (canGroup.check(
"CanForcedDeviceNum"))
1239 _networkN=canGroup.find(
"CanForcedDeviceNum").asInt32();
1242 _networkN=canGroup.check(
"CanDeviceNum",Value(-1),
"numeric identifier of device").asInt32();
1247 yError(
"can network id not valid, skipping device\n");
1251 _my_address=canGroup.check(
"CanMyAddress",Value(0),
1252 "numeric identifier of my address").asInt32();
1255 "polling period").asInt32();
1257 _timeout=canGroup.check(
"CanTimeout",Value(20),
"timeout period").asInt32();
1259 _txTimeout=canGroup.check(
"CanTxTimeout", Value(20),
"tx timeout").asInt32();
1260 _rxTimeout=canGroup.check(
"CanRxTimeout", Value(20),
"rx timeout").asInt32();
1265 if (canGroup.check(
"CanTxQueueSize"))
1266 _txQueueSize=canGroup.find(
"CanTxQueueSize").asInt32();
1270 if (canGroup.check(
"CanRxQueueSize"))
1271 _rxQueueSize=canGroup.find(
"CanRxQueueSize").asInt32();
1275 Bottle &canAddresses = canGroup.findGroup(
"CanAddresses",
1276 "a list of numeric identifiers");
1277 if (canAddresses.isNull())
1279 yError(
"CanAddresses group not found in config file\n");
1282 for (i = 1; i < canAddresses.size(); i++) {
1283 _destinations[i-1] = (
unsigned char)(canAddresses.get(i).asInt32());
1287 Bottle& general =
p.findGroup(
"GENERAL");
1290 if (!
validate(general, xtmp,
"AxisMap",
"a list of reordered indices for the axes", nj+1))
1293 for (i = 1; i < xtmp.size(); i++)
1294 _axisMap[i-1] = xtmp.get(i).asInt32();
1296 if (!
validate(general, xtmp,
"AxisName",
"a list of strings representing the axes names", nj + 1))
1301 for (i = 1; i < xtmp.size(); i++)
1304 if (!
validate(general, xtmp,
"AxisType",
"a list of strings representing the axes types (revolute/prismatic)", nj + 1))
1309 for (i = 1; i < xtmp.size(); i++)
1312 if (!
validate(general, xtmp,
"Encoder",
"a list of scales for the joint encoders", nj+1))
1315 int test = xtmp.size();
1316 for (i = 1; i < xtmp.size(); i++)
1319 if (!
validate_optional(general, xtmp,
"Rotor",
"a list of scales for the rotor encoders", nj+1))
1321 yWarning(
"Rotor: Using default value = 1\n");
1322 for(i=1;i<nj+1; i++)
1327 int test = xtmp.size();
1328 for (i = 1; i < xtmp.size(); i++)
1332 if (!
validate(general, xtmp,
"fullscalePWM",
" list of scales for the fullscalePWM conversion factor", nj + 1))
1334 yError(
"fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1337 for (i = 1; i < xtmp.size(); i++)
1340 if (!
validate(general, xtmp,
"ampsToSensor",
"a list of scales for the ampsToSensor conversion factor", nj + 1))
1342 yError(
"ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1345 for (i = 1; i < xtmp.size(); i++)
1348 if (!
validate(general, xtmp,
"Zeros",
"a list of offsets for the zero point", nj+1))
1351 for (i = 1; i < xtmp.size(); i++)
1352 _zeros[i-1] = xtmp.get(i).asFloat64();
1354 if (useRawJointEncoderData)
1356 for(i=1;i<nj+1; i++)
1367 if (useRawMotorEncoderData)
1369 for(i=1;i<nj+1; i++)
1378 if (!
validate_optional(general, xtmp,
"TorqueId",
"a list of associated joint torque sensor ids", nj+1))
1380 yWarning(
"TorqueId: Using default value = 0 (disabled)\n");
1381 for(i=1;i<nj+1; i++)
1386 for (i = 1; i < xtmp.size(); i++)
_torqueSensorId[i-1] = xtmp.get(i).asInt32();
1389 if (!
validate_optional(general, xtmp,
"TorqueChan",
"a list of associated joint torque sensor channels", nj+1))
1391 yWarning(
"TorqueChan: Using default value = 0 (disabled)\n");
1392 for(i=1;i<nj+1; i++)
1397 for (i = 1; i < xtmp.size(); i++)
_torqueSensorChan[i-1] = xtmp.get(i).asInt32();
1400 if (!
validate(general, xtmp,
"TorqueMax",
"full scale value for a joint torque sensor", nj+1))
1406 for (i = 1; i < xtmp.size(); i++)
1416 Bottle posPidsGroup;
1417 posPidsGroup=
p.findGroup(
"POS_PIDS",
"Position Pid parameters new format");
1418 if (posPidsGroup.isNull() ==
false)
1420 yInfo(
"Position Pids section found, new format\n");
1423 yError() <<
"Position Pids section: error detected in parameters syntax";
1428 yInfo(
"Position Pids successfully loaded\n");
1433 Bottle posPidsGroup2=
p.findGroup(
"PIDS",
"Position Pid parameters old format");
1434 if (posPidsGroup2.isNull()==
false)
1436 yWarning(
"Position Pids section found, old format\n");
1441 yError () <<
"no PIDS group found in config file, returning";
1449 Bottle trqPidsGroup;
1450 Bottle trqPidsOldGroup;
1451 Bottle trqControlGroup;
1452 trqPidsGroup =
p.findGroup(
"TRQ_PIDS",
"Torque Pid parameters new format");
1453 trqPidsOldGroup =
p.findGroup(
"TORQUE_PIDS",
"Torque Pid parameters old format");
1454 trqControlGroup =
p.findGroup(
"TORQUE_CONTROL",
"Torque Control parameters");
1456 if (trqPidsGroup.isNull()==
false)
1458 yInfo(
"TRQ_PIDS: Torque Pids section found, new format\n");
1461 yError () <<
"TRQ_PIDS: error detected in parameters syntax";
1466 xtmp = trqPidsGroup.findGroup(
"kbemf");
1467 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; }
1469 xtmp = trqPidsGroup.findGroup(
"ktau");
1470 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1; }
1472 xtmp = trqPidsGroup.findGroup(
"filterType");
1475 xtmp = trqPidsGroup.findGroup(
"controlUnits");
1481 else if (trqPidsOldGroup.isNull()==
false)
1483 yWarning (
"TORQUE_PIDS: Torque Pids section found, old format\n");
1486 yError () <<
"TORQUE_PIDS: error detected in parameters syntax";
1491 xtmp = trqPidsGroup.findGroup(
"kbemf");
1492 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; }
1494 xtmp = trqPidsGroup.findGroup(
"ktau");
1495 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1; }
1497 xtmp = trqPidsGroup.findGroup(
"filterType");
1500 xtmp = trqPidsGroup.findGroup(
"controlUnits");
1506 else if (trqControlGroup.isNull()==
false)
1508 yInfo(
"TORQUE_CONTROL section found\n");
1511 yError () <<
"TORQUE_CONTROL: error detected in parameters syntax";
1516 xtmp = trqControlGroup.findGroup(
"kbemf");
1518 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = xtmp.get(j+1).asFloat64();}
1520 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; yWarning (
"TORQUE_PIDS: 'kbemf' param missing");}
1522 xtmp = trqControlGroup.findGroup(
"ktau");
1524 {
for (j=0;j<nj;j++) this->
_ktau[j] = xtmp.get(j+1).asFloat64();}
1526 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1.0; yWarning (
"TORQUE_PIDS: 'ktau' param missing");}
1528 xtmp = trqControlGroup.findGroup(
"filterType");
1530 {
for (j=0;j<nj;j++) this->
_filterType[j] = xtmp.get(j+1).asInt32();}
1532 {
for (j=0;j<nj;j++) this->
_filterType[j] = 3; yWarning (
"TORQUE_PIDS: 'filterType' param missing");}
1534 xtmp = trqControlGroup.findGroup(
"controlUnits");
1539 else {yError() <<
"invalid controlUnits value";
return false;}
1543 yError (
"TORQUE_PIDS: 'controlUnits' param missing. Cannot continue");
1553 yWarning(
"Torque control parameters not found for part %s, skipping...\n",
_networkName.c_str());
1559 Bottle &debugGroup=
p.findGroup(
"DEBUG_PARAMETERS",
"DEBUG parameters");
1560 if (debugGroup.isNull()==
false)
1562 yDebug(
"DEBUG_PARAMETERS section found\n");
1565 yError(
"DEBUG_PARAMETERS section: error detected in parameters syntax\n");
1570 yInfo(
"DEBUG_PARAMETERS section: parameters successfully loaded\n");
1575 yDebug(
"DEBUG parameters section NOT found, skipping...\n");
1581 Bottle &impedanceGroup=
p.findGroup(
"IMPEDANCE",
"IMPEDANCE parameters");
1582 if (impedanceGroup.isNull()==
false)
1584 yDebug(
"IMPEDANCE parameters section found\n");
1587 yError(
"IMPEDANCE section: error detected in parameters syntax\n");
1592 yInfo(
"IMPEDANCE section: parameters successfully loaded\n");
1597 yDebug(
"IMPEDANCE parameters section NOT found, skipping...\n");
1614 Bottle &limits=
p.findGroup(
"LIMITS");
1615 if (limits.isNull())
1617 yError(
"Group LIMITS not found in configuration file\n");
1622 if (!
validate(limits, xtmp,
"motorOverloadCurrents",
"a list of current limits", nj+1))
1624 for(i=1;i<xtmp.size(); i++)
_currentLimits[i-1]=xtmp.get(i).asFloat64();
1627 if (!
validate_optional(limits, xtmp,
"motorPwmLimit",
"a list of motor PWM limits", nj + 1))
1629 yWarning(
"motorPwmLimit: Using default motorPwmLimit=1333\n");
1630 for (i = 1; i<nj + 1; i++)
1635 for (i = 1; i < xtmp.size(); i++)
1640 yError() <<
"motorPwmLimit should be a positive value";
1647 if (!
validate_optional(limits, xtmp,
"jntVelMax",
"the maximum velocity command of a joint", nj + 1))
1649 yWarning(
"jntVelMax: Using default jntVelMax=100 deg/s\n");
1650 for (i = 1; i<nj + 1; i++)
1655 for (i = 1; i<xtmp.size(); i++)
1660 yError() <<
"Invalid jntVelMax parameter <0\n";
1666 if (!
validate_optional(limits, xtmp,
"maxPosStep",
"the maximum amplitude of a position direct step", nj+1))
1668 yWarning(
"maxPosStep: Using default MaxPosStep=10 degs\n");
1669 for(i=1;i<nj+1; i++)
1674 for(i=1;i<xtmp.size(); i++)
1676 _maxStep[i-1]=xtmp.get(i).asFloat64();
1679 yError () <<
"Invalid MaxPosStep parameter <0\n";
1686 if (!
validate(limits, xtmp,
"jntPosMax",
"a list of maximum angles (in degrees)", nj+1))
1688 for(i=1;i<xtmp.size(); i++)
_limitsMax[i-1]=xtmp.get(i).asFloat64();
1691 if (!
validate(limits, xtmp,
"jntPosMin",
"a list of minimum angles (in degrees)", nj+1))
1693 for(i=1;i<xtmp.size(); i++)
_limitsMin[i-1]=xtmp.get(i).asFloat64();
1694 for (i = 0; i<nj; i++)
1698 yError(
"invalid limit on joint %d : Max value (%f) < Min value(%f)\n", i,
_limitsMax[i],
_limitsMin[i]);
1704 Bottle &velocityGroup=
p.findGroup(
"VELOCITY");
1705 if (!velocityGroup.isNull())
1708 if (!
validate(velocityGroup, xtmp,
"Shifts",
"a list of shifts to be used in the vmo control", nj+1))
1710 yWarning(
"Shifts: Using default Shifts=4\n");
1711 for(i=1;i<nj+1; i++)
1716 for(i=1;i<xtmp.size(); i++)
1722 if (!
validate(velocityGroup, xtmp,
"Timeout",
"a list of timeout to be used in the vmo control", nj+1))
1724 yWarning(
"Timeout: Using default Timeout=1000, i.e 1s\n");
1725 for(i=1;i<nj+1; i++)
1730 for(i=1;i<xtmp.size(); i++)
1736 if (!
validate_optional(velocityGroup, xtmp,
"JNT_speed_estimation",
"a list of shift factors used by the firmware joint speed estimator", nj+1))
1738 yWarning(
"JNT_speed_estimation: Using default value=5\n");
1739 for(i=1;i<nj+1; i++)
1744 for(i=1;i<xtmp.size(); i++)
1745 _estim_params[i-1].jnt_Vel_estimator_shift = xtmp.get(i).asInt32();
1750 if (!
validate_optional(velocityGroup, xtmp,
"MOT_speed_estimation",
"a list of shift factors used by the firmware motor speed estimator", nj+1))
1752 yWarning(
"MOT_speed_estimation: Using default value=5\n");
1753 for(i=1;i<nj+1; i++)
1758 for(i=1;i<xtmp.size(); i++)
1759 _estim_params[i-1].mot_Vel_estimator_shift = xtmp.get(i).asInt32();
1764 if (!
validate_optional(velocityGroup, xtmp,
"JNT_accel_estimation",
"a list of shift factors used by the firmware joint speed estimator", nj+1))
1766 yWarning(
"JNT_accel_estimation: Using default value=5\n");
1767 for(i=1;i<nj+1; i++)
1772 for(i=1;i<xtmp.size(); i++)
1773 _estim_params[i-1].jnt_Acc_estimator_shift = xtmp.get(i).asInt32();
1778 if (!
validate_optional(velocityGroup, xtmp,
"MOT_accel_estimation",
"a list of shift factors used by the firmware motor speed estimator", nj+1))
1780 yWarning(
"MOT_accel_estimation: Using default value=5\n");
1781 for(i=1;i<nj+1; i++)
1786 for(i=1;i<xtmp.size(); i++)
1787 _estim_params[i-1].mot_Acc_estimator_shift = xtmp.get(i).asInt32();
1793 yWarning(
"A suitable value for [VELOCITY] Shifts was not found. Using default Shifts=4\n");
1794 for(i=1;i<nj+1; i++)
1797 yWarning(
"A suitable value for [VELOCITY] Timeout was not found. Using default Timeout=1000, i.e 1s.\n");
1798 for(i=1;i<nj+1; i++)
1801 yWarning(
"A suitable value for [VELOCITY] speed estimation was not found. Using default shift factor=5.\n");
1802 for(i=1;i<nj+1; i++)
1812 if (!canGroup.findGroup(
"broadcast_pos").isNull())
1814 xtmp=canGroup.findGroup(
"broadcast_pos");
1818 if (!canGroup.findGroup(
"broadcast_pid").isNull())
1820 xtmp=canGroup.findGroup(
"broadcast_pid");
1823 if (!canGroup.findGroup(
"broadcast_fault").isNull())
1825 xtmp=canGroup.findGroup(
"broadcast_fault");
1828 if (!canGroup.findGroup(
"broadcast_current").isNull())
1830 xtmp=canGroup.findGroup(
"broadcast_current");
1833 if (!canGroup.findGroup(
"broadcast_overflow").isNull())
1835 xtmp=canGroup.findGroup(
"broadcast_overflow");
1838 if (!canGroup.findGroup(
"broadcast_canprint").isNull())
1840 xtmp=canGroup.findGroup(
"broadcast_canprint");
1843 if (!canGroup.findGroup(
"broadcast_vel_acc").isNull())
1845 xtmp=canGroup.findGroup(
"broadcast_vel_acc");
1849 if (!canGroup.findGroup(
"broadcast_pid_err").isNull())
1851 xtmp=canGroup.findGroup(
"broadcast_pid_err");
1855 if (!canGroup.findGroup(
"broadcast_rotor_pos").isNull())
1857 xtmp=canGroup.findGroup(
"broadcast_rotor_pos");
1861 if (!canGroup.findGroup(
"broadcast_rotor_vel_acc").isNull())
1863 xtmp=canGroup.findGroup(
"broadcast_rotor_vel_acc");
1868 yError(
"Invalid configuration file, check broadcast_* parameters\n");
1928 _zeros = allocAndCheck<double>(nj);
1936 _ktau=allocAndCheck<double>(nj);
1937 _maxStep=allocAndCheck<double>(nj);
1944 _pids=allocAndCheck<Pid>(nj);
1945 _tpids=allocAndCheck<Pid>(nj);
1963 memset(
_maxStep, 0,
sizeof(
double)*nj);
1978 for(
int j=0;j<nj;j++)
1988 checkAndDestroy<double>(
_zeros);
2003 checkAndDestroy<double>(
_ktau);
2008 checkAndDestroy<std::string>(
_axisName);
2009 checkAndDestroy<std::string>(
_axisType);
2011 checkAndDestroy<Pid>(
_pids);
2012 checkAndDestroy<Pid>(
_tpids);
2036 _timeout = CAN_TIMEOUT;
2037 _polling_interval = CAN_POLLING_INTERVAL;
2044 memset (_destinations, 0,
sizeof(
unsigned char) *
CAN_MAX_CARDS);
2053 _bcastRecvBuffer = NULL;
2055 _error_status =
true;
2072 polyDriver.open(config);
2073 if (!polyDriver.isValid())
2075 yError(
"Could not instantiate CAN device\n");
2079 polyDriver.view(iCanBus);
2080 polyDriver.view(iBufferFactory);
2081 polyDriver.view(iCanErrors);
2083 if ((iCanBus==0) || (iBufferFactory==0))
2085 yError(
"CanBusResources::initialize() could not get ICanBus or ICanBufferFactory interface");
2089 ret=initialize(params);
2095 yTrace(
"Calling CanBusResources::initialize\n");
2105 _error_status =
true;
2111 _velShifts[k] = (1 << ((
int) _velShifts[k]));
2120 _jointNames =
new std::string[_njoints];
2121 for (
int i = 0; i < _njoints; i++) _jointNames[i] = parms.
_axisName[i];
2131 printf(
"printing destinations and inverted map\n");
2132 for(
int jj=0;jj<_njoints;jj+=2)
2134 _destInv[_destinations[jj/2]]=jj;
2135 printf(
"%d %d\n",jj,_destinations[jj/2]);
2138 _bcastRecvBuffer = allocAndCheck<BCastBufferElement> (_njoints);
2139 for (
int j=0; j<_njoints ;j++)
2141 _bcastRecvBuffer[j]._update_e=Time::now();
2142 _bcastRecvBuffer[j]._position_joint.resetStats();
2143 _bcastRecvBuffer[j]._position_rotor.resetStats();
2144 _bcastRecvBuffer[j]._speed_rotor.resetStats();
2145 _bcastRecvBuffer[j]._accel_rotor.resetStats();
2149 iCanBus->canSetBaudRate(_speed);
2152 #if ICUB_CANMASKS_STRICT_FILTER
2153 yInfo(
"using ICUB_CANMASKS_STRICT_FILTER option\n");
2157 unsigned int sent_addr = 0x000 + ( 0x000<<4) + _destinations[j];
2158 unsigned int recv_addr = 0x000 + (_destinations[j]<<4) + 0x000;
2159 iCanBus->canIdAdd(sent_addr);
2160 iCanBus->canIdAdd(recv_addr);
2162 yDebug(
"class 0 set\n");
2167 unsigned int start_addr = 0x100 + (_destinations[j]<<4) + 0x000;
2168 unsigned int end_addr = 0x100 + (_destinations[j]<<4) + 0x00F;
2169 for (i = start_addr; i < end_addr; i++)
2170 iCanBus->canIdAdd(i);
2172 yDebug(
"class 1 set\n");
2175 yInfo(
"opening all CAN masks\n");
2177 for (i = 0x000; i < 0x0ff; i++)
2178 iCanBus->canIdAdd(i);
2179 yDebug(
"class 0 set\n");
2182 for (i = 0x100; i < 0x1ff; i++)
2183 iCanBus->canIdAdd(i);
2184 yDebug(
"class 1 set\n");
2188 for (i = 0x200; i < 0x2ff; i++)
2189 iCanBus->canIdAdd(i);
2190 yDebug(
"class 2 set\n");
2193 for (i = 0x300; i < 0x3ff; i++)
2194 iCanBus->canIdAdd(i);
2195 yDebug(
"class 3 set\n");
2197 _readBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2198 _writeBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2199 _replyBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2200 _echoBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2201 yDebug(
"Can read/write buffers created, buffer size: %d\n",
BUF_SIZE);
2203 requestsQueue =
new RequestsQueue(_njoints, ICUBCANPROTO_POL_MC_CMD_MAXNUM);
2207 yInfo(
"CanBusResources::initialized correctly\n");
2220 checkAndDestroy<BCastBufferElement> (_bcastRecvBuffer);
2224 iBufferFactory->destroyBuffer(_readBuffer);
2225 iBufferFactory->destroyBuffer(_writeBuffer);
2226 iBufferFactory->destroyBuffer(_replyBuffer);
2227 iBufferFactory->destroyBuffer(_echoBuffer);
2231 if (requestsQueue!=0)
2233 delete requestsQueue;
2243 if (_jointNames != 0)
2245 delete[] _jointNames;
2260 res=iCanBus->canRead(_readBuffer, messages, &_readMessages);
2272 unsigned int id = _destinations[joint/2] & 0x0f;
2275 if ((joint % 2) == 1)
2278 _writeBuffer[_writeMessages].setId(
id);
2279 _writeBuffer[_writeMessages].setLen(1);
2280 _writeBuffer[_writeMessages].getData()[0]=
data;
2288 unsigned char *
data=_writeBuffer[_writeMessages].getData();
2289 unsigned int destId= _destinations[joint/2] & 0x0f;
2292 if ((joint % 2) == 1)
2295 _writeBuffer[_writeMessages].setId(destId);
2296 _writeBuffer[_writeMessages].setLen(1);
2303 requestsQueue->append(rq);
2310 if (_writeMessages < 1)
2313 unsigned int sent=0;
2316 for(
unsigned int k=0; k<_writeMessages;k++)
2322 res=iCanBus->canWrite(_writeBuffer, _writeMessages, &sent);
2335 const unsigned char *
data=m.getData();
2341 int ret = sprintf (_printBuffer,
"class: %2d s: %2x d: %2x c: %1d msg: %3d (%x) ",
2345 ((
data[0] & 0x80)==0)?0:1,
2351 ret += sprintf (_printBuffer+ret,
"x: ");
2355 for (j = 1; j < len; j++)
2357 ret += sprintf (_printBuffer+ret,
"%x ",
data[j]);
2360 printf(
"%s", _printBuffer);
2370 yDebug(
"CAN: write buffer\n");
2371 for (j = 0; j < _writeMessages; j++)
2372 printMessage (_writeBuffer[j]);
2374 yDebug(
"CAN: reply buffer\n");
2375 for (j = 0; j < _writeMessages; j++)
2376 printMessage (_replyBuffer[j]);
2378 yDebug(
"CAN: echo buffer\n");
2379 for (j = 0; j < _echoMessages; j++)
2380 printMessage (_echoBuffer[j]);
2382 yDebug(
"CAN: read buffer\n");
2383 for (j = 0; j < _readMessages; j++)
2384 printMessage (_readBuffer[j]);
2385 yDebug(
"CAN: -------------\n");
2392 PeriodicThread(0.01),
2393 ImplementPositionControl(this),
2394 ImplementVelocityControl(this),
2395 ImplementPidControl(this),
2396 ImplementEncodersTimed(this),
2397 ImplementControlCalibration(this),
2398 ImplementAmplifierControl(this),
2399 ImplementControlLimits(this),
2400 ImplementTorqueControl(this),
2401 ImplementImpedanceControl(this),
2402 ImplementControlMode(this),
2403 ImplementPositionDirect(this),
2404 ImplementInteractionMode(this),
2405 ImplementMotorEncoders(this),
2406 ImplementMotor(this),
2407 ImplementRemoteVariables(this),
2408 ImplementAxisInfo(this),
2409 ImplementPWMControl(this),
2410 ImplementCurrentControl(this)
2427 mServerLogger = NULL;
2449 yTrace(
"Opening CanBusMotionControl Control\n");
2450 std::string dbg_string = config.toString().c_str();
2457 if(!
p.fromConfig(config))
2460 yError() <<
"One or more errors found while parsing device configuration";
2464 std::string str=config.toString().c_str();
2466 prop.fromString(str.c_str());
2467 networkName=config.find(
"NetworkId").asString();
2468 canDevName=config.find(
"canbusdevice").asString();
2469 if (canDevName==
"") canDevName=config.findGroup(
"CAN").find(
"canbusdevice").asString();
2470 if(canDevName==
"") { yError() <<
"cannot find parameter 'canbusdevice'\n";
return false;}
2471 prop.unput(
"device");
2472 prop.unput(
"subdevice");
2473 prop.put(
"device", canDevName.c_str());
2474 std::string canPhysDevName = config.find(
"physDevice").asString();
2475 if (canPhysDevName==
"") canPhysDevName = config.findGroup(
"CAN").find(
"physDevice").asString();
2476 prop.put(
"physDevice",canPhysDevName.c_str());
2477 prop.put(
"canDeviceNum",
p._networkN);
2478 prop.put(
"canTxTimeout",
p._txTimeout);
2479 prop.put(
"canRxTimeout",
p._rxTimeout);
2480 if (
p._txQueueSize!=-1)
2481 prop.put(
"canTxQueueSize",
p._txQueueSize);
2482 if (
p._rxQueueSize!=-1)
2483 prop.put(
"canRxQueueSize",
p._rxQueueSize);
2490 yError() <<
"Unable to uninitialize CAN driver";
2499 double *tmpZeros =
new double [
p._njoints];
2500 double *tmpOnes =
new double [
p._njoints];
2501 for (
int i=0; i<
p._njoints; i++)
2507 ImplementPositionControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2508 ImplementVelocityControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2516 ImplementPidControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder, NULL,
p._newtonsToSensor,
p._ampsToSensor,
p._dutycycleToPwm);
2518 ImplementEncodersTimed::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2520 ImplementMotorEncoders::initialize(
p._njoints,
p._axisMap,
p._rotToEncoder, tmpZeros);
2522 ImplementMotor::initialize(
p._njoints,
p._axisMap);
2524 ImplementControlCalibration::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2526 ImplementAmplifierControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2528 ImplementControlLimits::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2530 ImplementControlMode::initialize(
p._njoints,
p._axisMap);
2531 ImplementTorqueControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros,
p._newtonsToSensor,
p._ampsToSensor,
nullptr,
nullptr,
nullptr);
2536 else {yError() <<
"Invalid _torqueControlUnits value: %d" <<
p._torqueControlUnits;
return false;}
2539 ImplementImpedanceControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros,
p._newtonsToSensor);
2540 ImplementPositionDirect::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2541 ImplementInteractionMode::initialize(
p._njoints,
p._axisMap);
2543 ImplementAxisInfo::initialize(
p._njoints,
p._axisMap);
2544 ImplementRemoteVariables::initialize(
p._njoints,
p._axisMap);
2545 ImplementCurrentControl::initialize(
p._njoints,
p._axisMap,
p._ampsToSensor);
2546 ImplementPWMControl::initialize(
p._njoints,
p._axisMap,
p._dutycycleToPwm);
2548 delete [] tmpZeros; tmpZeros=0;
2549 delete [] tmpOnes; tmpOnes=0;
2557 _ref_accs = allocAndCheck<double>(
p._njoints);
2562 for (
int i = 0; i<
p._njoints; i++)
2572 yarp::os::Time::delay(0.005);
2573 setPids(VOCAB_PIDTYPE_POSITION,
p._pids);
2575 if (
p._torqueControlEnabled==
true)
2578 yarp::os::Time::delay(0.005);
2579 setPids(VOCAB_PIDTYPE_TORQUE,
p._tpids);
2581 for (
int i=0; i<
p._njoints; i++)
2583 MotorTorqueParameters ps;
2584 ps.bemf =
p._bemfGain[i];
2585 this->setMotorTorqueParams(i,ps);
2587 yarp::os::Time::delay(0.002);
2589 yarp::os::Time::delay(0.002);
2595 for (
int j=0; j<
p._njoints; j++)
2597 yarp::os::Time::delay(0.001);
2603 for (
int j = 0; j<
p._njoints; j++)
2605 yarp::os::Time::delay(0.002);
2606 setRefSpeed(j, 10.0);
2610 for (
int j=0; j<
p._njoints; j++)
2612 yarp::os::Time::delay(0.001);
2613 setImpedance(j,
p._impedance_params[j].stiffness,
p._impedance_params[j].damping);
2617 for(i = 0; i <
p._njoints; i++)
2619 yarp::os::Time::delay(0.001);
2624 for(i = 0; i <
p._njoints; i++)
2626 yarp::os::Time::delay(0.001);
2627 setLimits(i,
p._limitsMin[i],
p._limitsMax[i]);
2628 yarp::os::Time::delay(0.001);
2629 setVelLimits(i, 0,
p._maxJntCmdVelocity[i]);
2630 yarp::os::Time::delay(0.001);
2631 setMaxCurrent(i,
p._currentLimits[i]);
2635 for(i = 0; i <
p._njoints; i++)
2637 yarp::os::Time::delay(0.001);
2639 yarp::os::Time::delay(0.001);
2641 yarp::os::Time::delay(0.001);
2642 setPWMLimit(i,
p._motorPwmLimits[i]);
2646 for(i = 0; i <
p._njoints; i++)
2648 yarp::os::Time::delay(0.001);
2650 p._estim_params[i].jnt_Acc_estimator_shift,
2651 p._estim_params[i].mot_Vel_estimator_shift,
2652 p._estim_params[i].mot_Acc_estimator_shift);
2657 for (i = 0; i <
p._njoints; i++)
2659 yarp::os::Time::delay(0.001);
2660 setControlMode(i, VOCAB_CM_IDLE);
2662 const Bottle &analogList=config.findGroup(
"analog").tail();
2664 if (analogList.size()>0)
2666 for(
int k=0;k<analogList.size();k++)
2668 std::string analogId=analogList.get(k).asString().c_str();
2673 analogSensors.push_back(as);
2681 PeriodicThread::setPeriod((
double)
p._polling_interval/1000.0);
2682 PeriodicThread::start();
2689 for (
int j=0; j<
p._njoints; j++)
2691 yarp::os::Time::delay(0.001);
2693 if (b==
false) yError() <<
"Unable to read firmware version";
2698 #ifdef ICUB_CANPROTOCOL_STRICT
2702 PeriodicThread::stop();
2704 yError() <<
"checkFirmwareVersions() failed. CanBusMotionControl::open returning false,";
2709 yWarning(
"*********************************************************************************\n");
2710 yWarning(
"**** WARNING: ICUB_CANPROTOCOL_STRICT not defined, skipping firmware check! *****\n");
2711 yWarning(
"*********************************************************************************\n");
2715 yDebug() <<
"CanBusMotionControl::open returned true\n";
2719 bool CanBusMotionControl::readFullScaleAnalog(
int analog_address,
int ch,
double* fullScale)
2722 int destId=0x0200|analog_address;
2734 bool full_scale_read=
false;
2741 unsigned int currId=m.getId();
2742 if (currId==(0x0200|(analog_address<<4)))
2743 if (m.getLen()==4 &&
2744 m.getData()[0]==0x18 &&
2747 *fullScale=m.getData()[2]<<8 | m.getData()[3];
2748 full_scale_read=
true;
2752 yarp::os::Time::delay(0.002);
2755 while(timeout<32 && full_scale_read==
false);
2757 if (full_scale_read==
false)
2759 yError() <<
"Trying to get fullscale data from sensor: no answer received or message lost (ch:" << ch <<
")";
2766 TBR_AnalogSensor *CanBusMotionControl::instantiateAnalog(yarp::os::Searchable& config, std::string deviceid)
2774 Bottle &analogConfig=config.findGroup(deviceid.c_str());
2775 if (analogConfig.size()>0)
2777 yDebug(
"Initializing analog device %s\n", deviceid.c_str());
2782 bool isVirtualSensor=
false;
2783 char analogId=analogConfig.find(
"CanAddress").asInt32();
2784 char analogFormat=analogConfig.find(
"Format").asInt32();
2785 int analogChannels=analogConfig.find(
"Channels").asInt32();
2786 int analogCalibration=analogConfig.find(
"UseCalibration").asInt32();
2789 if (analogConfig.check(
"PortName"))
2791 isVirtualSensor =
true;
2792 std::string virtualPortName = analogConfig.find(
"PortName").asString();
2793 bool canEchoEnabled = analogConfig.find(
"CanEcho").asInt32();
2796 std::string rn(
"/");
2797 rn += config.find(
"robotName").asString().c_str();
2799 rn+=virtualPortName;
2800 analogSensor->
backDoor->open(rn.c_str());
2804 switch (analogFormat)
2814 int destId=0x0200|analogSensor->
getId();
2816 if (analogConfig.check(
"Period"))
2818 int period=analogConfig.find(
"Period").asInt32();
2832 if (analogChannels==16 && analogFormat==8)
2843 else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==
false)
2846 if (analogCalibration==1)
2849 for (
int ch=0; ch<6; ch++)
2858 if (attempts>0) yWarning(
"Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
2862 yarp::os::Time::delay(0.020);
2866 yError(
"Trying to get fullscale data from sensor %s: all attempts failed (ch:%d)\n", deviceid.c_str(), ch);
2867 yError(
"Device %s cannot be opened.\n", deviceid.c_str());
2874 yDebug(
"Sensor Fullscale Id %#4X: %f %f %f %f %f %f ",analogId,
2904 else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==
true)
2911 if (cfgId == analogId)
2913 for (
int ch=0; ch<6; ch++)
2927 yDebug(
"Sensor Fullscale Id %#4X: %f %f %f %f %f %f",analogId,
2937 return analogSensor;
2944 if (analogSensor->
isOpen())
2947 int destId=0x0200|analogSensor->
getId();
2958 yDebug(
"---> Len:%d %x %x %x\n",
2983 setControlMode(i, VOCAB_CM_IDLE);
2994 PeriodicThread::stop ();
2996 ImplementPositionControl::uninitialize();
2997 ImplementVelocityControl::uninitialize();
3002 ImplementPidControl::uninitialize();
3003 ImplementEncodersTimed::uninitialize();
3004 ImplementMotorEncoders::uninitialize();
3005 ImplementControlCalibration::uninitialize();
3006 ImplementAmplifierControl::uninitialize();
3007 ImplementControlLimits::uninitialize();
3009 ImplementControlMode::uninitialize();
3010 ImplementTorqueControl::uninitialize();
3011 ImplementImpedanceControl::uninitialize();
3012 ImplementPositionDirect::uninitialize();
3013 ImplementInteractionMode::uninitialize();
3014 ImplementRemoteVariables::uninitialize();
3015 ImplementAxisInfo::uninitialize();
3016 ImplementCurrentControl::uninitialize();
3017 ImplementPWMControl::uninitialize();
3020 std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
3021 while(it!=analogSensors.end())
3055 void CanBusMotionControl::handleBroadcasts()
3059 double before=Time::now();
3063 for (
unsigned int buff_num=0; buff_num<2; buff_num++)
3065 unsigned int size = 0;
3066 CanBuffer* buffer_pointer=0;
3078 for (i = 0; i < size; i++)
3082 unsigned char *
data=0;
3083 CanMessage& m = (*buffer_pointer)[i];
3088 if ((
id & 0x700) == 0x300)
3091 const unsigned int addr = ((
id & 0x0f0) >> 4);
3092 const unsigned int type =
id & 0x00f;
3093 if (type == 0x0A || type == 0x0B)
3095 int off = (type-0x0A)*3;
3096 for (
int axis=0; axis<r.
getJoints(); axis++)
3099 if ( attached_board == addr)
3101 for(
int chan=0;chan<3;chan++)
3104 if ( attached_channel == chan+off)
3117 else if ((
id & 0x700) == 0x100)
3123 const int addr = ((
id & 0x0f0) >> 4);
3140 yError(
"%s [%d] Warning, got unexpected broadcast msg(s), last one from address %d, (original) id 0x%x, len %d\n", canDevName.c_str(), _networkN, addr,
id, len);
3141 char tmp1 [255]; tmp1[0]=0;
3142 char tmp2 [255]; tmp2[0]=0;
3148 yError(
"%s [%d] valid addresses are (%s)\n",canDevName.c_str(), _networkN,tmp2);
3160 case ICUBCANPROTO_PER_MC_MSG__OVERFLOW:
3162 yError (
"CAN PACKET LOSS, board %d buffer full\r\n", (((
id & 0x0f0) >> 4)-1));
3166 case ICUBCANPROTO_PER_MC_MSG__PRINT:
3168 if (
data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT ||
3169 data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT + 128)
3171 int addr = (((
id & 0x0f0) >> 4)-1);
3174 if (string_id != -1)
3182 case ICUBCANPROTO_PER_MC_MSG__POSITION:
3200 case ICUBCANPROTO_PER_MC_MSG__MOTOR_POSITION:
3218 case ICUBCANPROTO_PER_MC_MSG__MOTOR_SPEED:
3239 case ICUBCANPROTO_PER_MC_MSG__TORQUE:
3255 case ICUBCANPROTO_PER_MC_MSG__PID_VAL:
3267 case ICUBCANPROTO_PER_MC_MSG__STATUS:
3280 for(
int m=0;m<8;m++)
3281 yDebug(
"%.2x ",
data[m]);
3289 logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((
int)bFlag));
3297 logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((
int)bFlag));
3300 logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((
int)bFlag));
3303 logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((
int)bFlag));
3306 logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((
int)bFlag));
3309 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3312 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3315 logJointData(canDevName.c_str(),_networkN,j,16,yarp::os::Value((
int)bFlag));
3318 logJointData(canDevName.c_str(),_networkN,j,13,yarp::os::Value((
int)bFlag));
3327 logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((
int)bFlag));
3332 logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((
int)bFlag));
3339 logJointData(canDevName.c_str(),_networkN,j,18,yarp::os::Value((
int)bFlag));
3341 if ((bFlag=r.
_bcastRecvBuffer[j].
isOverTempCh1())) yError (
"%s [%d] board %d OVER TEMPERATURE CH 1 \n", canDevName.c_str(), _networkN, addr);
3342 logJointData(canDevName.c_str(),_networkN,j,19,yarp::os::Value((
int)bFlag));
3344 if ((bFlag=r.
_bcastRecvBuffer[j].
isOverTempCh2())) yError (
"%s [%d] board %d OVER TEMPERATURE CH 2 \n", canDevName.c_str(), _networkN, addr);
3345 logJointData(canDevName.c_str(),_networkN,j+1,19,yarp::os::Value((
int)bFlag));
3348 logJointData(canDevName.c_str(),_networkN,j,20,yarp::os::Value((
int)bFlag));
3351 logJointData(canDevName.c_str(),_networkN,j+1,20,yarp::os::Value((
int)bFlag));
3366 logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((
int)bFlag));
3369 logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((
int)bFlag));
3372 logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((
int)bFlag));
3375 logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((
int)bFlag));
3378 logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((
int)bFlag));
3381 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3384 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3389 case ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS:
3400 case ICUBCANPROTO_PER_MC_MSG__CURRENT:
3411 case ICUBCANPROTO_PER_MC_MSG__PID_ERROR:
3424 case ICUBCANPROTO_PER_MC_MSG__VELOCITY:
3459 errorstring.reserve(errorstringsize);
3462 averageThreadTime=0;
3465 lastReportTime=Time::now();
3481 double before = Time::now();
3484 averagePeriod+=(currentRun-previousRun)*1000;
3488 std::list<CanRequest> timedout;
3494 ACE_ASSERT (fifo!=0);
3495 std::list<ThreadId>::iterator it=fifo->begin();
3496 std::list<ThreadId>::iterator end=fifo->end();
3508 timedout.push_back(rq);
3510 yError(
"%s [%d] thread:%d msg:%d joint:%d timed out\n",
3525 std::list<CanRequest>::iterator it=timedout.begin();
3526 std::list<CanRequest>::iterator end=timedout.end();
3529 int tid=(*it).threadId;
3538 double avPeriod=1000.0*getEstimatedPeriod();
3539 double avThTime=1000.0*getEstimatedUsed();
3540 unsigned int it=getIterations();
3542 yDebug(
"%s [%d] thread ran %d times, req.dT:%d[ms], av.dT:%.2lf[ms] av.loopT :%.2lf[ms]\n",
3550 const char *can=canDevName.c_str();
3559 yDebug(
" Can Errors -- Device Rx:%u, Device Tx:%u, RxOvf: %u, TxOvf: %u, BusOff: %d -- Driver Fifo Rx:%u, Driver Fifo Tx:%u\n",
3562 errors.rxCanFifoOvr,
3563 errors.txCanFifoOvr,
3566 errors.txBufferOvr);
3581 yDebug(
" Device has no ICanBusErr interface\n");
3587 char tmp[255] = {0};
3588 errorstring.clear();
3590 snprintf(
tmp,
sizeof(
tmp),
"%s [%d] printing boards infos:\n", canDevName.c_str(), r.
_networkN);
3591 errorstring.append(
tmp);
3617 errorstring.append(
tmp);
3631 snprintf(
tmp,
sizeof(
tmp),
"None");
3632 errorstring.append(
tmp);
3635 yDebug(
"%s\n", errorstring.c_str());
3648 yWarning(
"%s [%d] have not heard from board %d (channel %d) since %.2lf seconds\n",
3651 addr, ch, currentRun-lastRecv);
3673 double POS_LATENCY_WARN_THR=averagePeriod*1.5;
3674 if (
max>POS_LATENCY_WARN_THR)
3676 yWarning(
"%s [%d] jnt %d, warning encoder latency above threshold (lat: %.2lf [ms], received %d msgs)\n",
3686 yWarning(
"%s [%d] joint %d, warning not enough encoder messages (received %d msgs)\n",
3701 std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3702 while(analogIt!=analogSensors.end())
3712 const char* devName=canDevName.c_str();
3713 int id=pAnalog->
getId();
3715 #ifdef _USE_INTERFACEGUI
3716 logAnalogData(devName,r.
_networkN,
id,3,yarp::os::Value((
int)
sat));
3717 logAnalogData(devName,r.
_networkN,
id,4,yarp::os::Value((
int)err));
3718 logAnalogData(devName,r.
_networkN,
id,5,yarp::os::Value((
int)tout));
3721 if (
sat+err+tout!=0)
3723 yWarning(
"%s [%d] analog %s saturated:%u errors: %u timeout:%u\n",
3735 lastReportTime=currentRun;
3737 averageThreadTime=0;
3744 if (r.
read () !=
true)
3752 std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3753 while(analogIt!=analogSensors.end())
3760 yWarning(
"%s [%d] analog sensor received unexpected class 0x03 messages\n", canDevName.c_str(), r.
_networkN);
3764 yWarning(
"Got null pointer this is unusual\n");
3776 DEBUG_FUNC(
"There are %d pending messages, read msgs: %d\n",
3780 unsigned char *msgData;
3783 msgData=m.getData();
3793 yWarning(
"%s [%d] Received message but no threads waiting for it. (id: 0x%x, Class:%d MsgData[0]:%d)\n ", canDevName.c_str(), r.
_networkN, m.getId(),
getClass(m), msgData[0]);
3799 yWarning(
"Asked a bad thread id, this is probably a bug, check threadPool\n");
3805 yError(
"error while pushing a reply, this is probably an error\n");
3826 double now = Time::now();
3827 averageThreadTime+=(now-before)*1000;
3839 std::lock_guard<std::recursive_mutex> lck(
_mutex);
3873 switch (interactionvocab)
3875 case VOCAB_IM_STIFF:
3876 return icubCanProto_interactionmode_stiff;
3878 case VOCAB_IM_COMPLIANT:
3879 return icubCanProto_interactionmode_compliant;
3881 case VOCAB_IM_UNKNOWN:
3883 return icubCanProto_interactionmode_unknownError;
3890 switch (interactionint)
3892 case icubCanProto_interactionmode_stiff:
3893 return VOCAB_IM_STIFF;
3895 case icubCanProto_interactionmode_compliant:
3896 return VOCAB_IM_COMPLIANT;
3898 case icubCanProto_interactionmode_unknownError:
3900 return icubCanProto_interactionmode_unknownError;
3911 return icubCanProto_controlmode_idle;
3913 case VOCAB_CM_POSITION:
3914 return icubCanProto_controlmode_position;
3916 case VOCAB_CM_MIXED:
3917 return icubCanProto_controlmode_mixed;
3919 case VOCAB_CM_POSITION_DIRECT:
3920 return icubCanProto_controlmode_direct;
3922 case VOCAB_CM_VELOCITY:
3923 return icubCanProto_controlmode_velocity;
3925 case VOCAB_CM_TORQUE:
3926 return icubCanProto_controlmode_torque;
3928 case VOCAB_CM_IMPEDANCE_POS:
3929 return icubCanProto_controlmode_impedance_pos;
3931 case VOCAB_CM_IMPEDANCE_VEL:
3932 return icubCanProto_controlmode_impedance_vel;
3935 return icubCanProto_controlmode_openloop;
3937 case VOCAB_CM_CURRENT:
3938 return icubCanProto_controlmode_unknownError;
3939 yError(
"'VOCAB_CM_CURRENT' error condition detected");
3942 case VOCAB_CM_FORCE_IDLE:
3943 return icubCanProto_controlmode_forceIdle;
3947 return icubCanProto_controlmode_unknownError;
3948 yError (
"'VOCAB_CM_UNKNOWN' error condition detected");
3957 case icubCanProto_controlmode_idle:
3958 return VOCAB_CM_IDLE;
3960 case icubCanProto_controlmode_position:
3961 return VOCAB_CM_POSITION;
3963 case icubCanProto_controlmode_mixed:
3964 return VOCAB_CM_MIXED;
3966 case icubCanProto_controlmode_direct:
3967 return VOCAB_CM_POSITION_DIRECT;
3969 case icubCanProto_controlmode_velocity:
3970 return VOCAB_CM_VELOCITY;
3972 case icubCanProto_controlmode_torque:
3973 return VOCAB_CM_TORQUE;
3975 case icubCanProto_controlmode_impedance_pos:
3976 return VOCAB_CM_IMPEDANCE_POS;
3978 case icubCanProto_controlmode_impedance_vel:
3979 return VOCAB_CM_IMPEDANCE_VEL;
3981 case icubCanProto_controlmode_openloop:
3982 return VOCAB_CM_PWM;
3986 case icubCanProto_controlmode_hwFault:
3987 return VOCAB_CM_HW_FAULT;
3989 case icubCanProto_controlmode_notConfigured:
3990 return VOCAB_CM_NOT_CONFIGURED;
3992 case icubCanProto_controlmode_configured:
3993 return VOCAB_CM_CONFIGURED;
3995 case icubCanProto_controlmode_unknownError:
3996 yError (
"'icubCanProto_controlmode_unknownError' error condition detected");
3997 return VOCAB_CM_UNKNOWN;
4000 case icubCanProto_controlmode_calibration:
4009 return VOCAB_CM_CALIBRATING;
4013 yError (
"'VOCAB_CM_UNKNOWN' error condition detected");
4014 return VOCAB_CM_UNKNOWN;
4024 *v=VOCAB_CM_UNKNOWN;
4033 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4043 DEBUG_FUNC(
"Calling GET_CONTROL_MODE MULTIPLE JOINTS \n");
4044 if (joints==0)
return false;
4045 if (modes==0)
return false;
4049 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4050 for (i = 0; i < n_joints; i++)
4062 if (mode == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
4064 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW SINGLE JOINT\n");
4068 if (v==icubCanProto_controlmode_unknownError)
return false;
4069 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,j,v);
4071 int current_mode = VOCAB_CM_UNKNOWN;
4077 if (current_mode==mode) {ret =
true;
break;}
4078 if (current_mode==VOCAB_CM_IDLE && mode==VOCAB_CM_FORCE_IDLE) {ret =
true;
break;}
4079 if (current_mode==VOCAB_CM_HW_FAULT)
4081 if (mode!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), j);}
4084 yarp::os::Time::delay(0.010);
4085 if (timeout >0) yWarning (
"setControlModeRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4088 while (timeout < 10);
4092 yError (
"100ms Timeout occured in setControlModeRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4100 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW MULTIPLE JOINTS\n");
4101 if (n_joints==0)
return false;
4102 if (joints==0)
return false;
4104 for (
int i=0;i<n_joints; i++)
4106 if (modes[i] == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4109 if (v==icubCanProto_controlmode_unknownError) ret =
false;
4110 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,joints[i],v);
4112 int current_mode = VOCAB_CM_UNKNOWN;
4117 if (current_mode==modes[i]) {ret =
true;
break;}
4118 if (current_mode==VOCAB_CM_IDLE)
4120 if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), joints[i]);}
4123 yarp::os::Time::delay(0.010);
4124 if (timeout >0) yWarning (
"setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4127 while (timeout < 10);
4131 yError (
"100ms Timeout occured in setControlModesRaw(M) (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4140 listOfKeys->clear();
4141 listOfKeys->addString(
"filterType");
4142 listOfKeys->addString(
"PWMLimit");
4150 if (key ==
"filterType")
4155 if (key ==
"PWMLimit")
4160 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
4169 std::string s1 = val.toString();
4170 if (val.size() != _njoints)
4172 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
4176 if (key ==
"filterType")
4180 int filter_type = val.get(i).asInt32();
4185 if (key ==
"PWMLimit")
4189 double limit = val.get(i).asFloat64();
4194 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
4219 type = VOCAB_JOINTTYPE_REVOLUTE;
4230 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW ALL JOINT\n");
4236 if (modes[i] == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4239 if (v==icubCanProto_controlmode_unknownError)
return false;
4240 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,i,v);
4242 int current_mode = VOCAB_CM_UNKNOWN;
4247 if (current_mode==modes[i]) {ret =
true;
break;}
4248 if (current_mode==VOCAB_CM_IDLE && modes[i]==VOCAB_CM_FORCE_IDLE) {ret =
true;
break;}
4249 if (current_mode==VOCAB_CM_HW_FAULT)
4251 if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), i);}
4254 yarp::os::Time::delay(0.010);
4255 if (timeout >0) yWarning (
"setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4258 while (timeout < 10);
4262 yError (
"100ms Timeout occured in setControlModesRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4285 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_ILIM_GAIN, axis,
S_16(pid.max_int));
4286 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_OFFSET, axis,
S_16(pid.offset));
4288 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_TLIM, axis,
S_16(pid.max_output));
4289 _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS, axis,
S_16(pid.stiction_up_val),
S_16(pid.stiction_down_val),
false);
4301 case VOCAB_PIDTYPE_POSITION:
4304 case VOCAB_PIDTYPE_VELOCITY:
4307 case VOCAB_PIDTYPE_CURRENT:
4310 case VOCAB_PIDTYPE_TORQUE:
4314 yError()<<
"Invalid pidtype:"<<pidtype;
4345 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS);
4355 yError(
"getImpedanceRaw: message timed out\n");
4362 CanMessage *m=t->
get(0);
4371 unsigned char *
data;
4372 data=m->getData()+1;
4373 *stiff= *((
short *)(
data));
4375 *damp= *((
short *)(
data));
4390 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4396 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4424 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET);
4434 yError(
"getImpedanceOffset: message timed out\n");
4440 CanMessage *m=t->
get(0);
4448 unsigned char *
data;
4449 data=m->getData()+1;
4450 *off= *((
short *)(
data));
4469 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4471 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS, axis);
4495 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4497 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_SOURCE, axis);
4499 *((
char *)(r.
_writeBuffer[0].getData()+2)) = board_chan;
4522 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4524 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET, axis);
4541 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_P_GAIN, axis, s);
out->kp = double(s);
4543 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_D_GAIN, axis, s);
out->kd = double(s);
4545 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_I_GAIN, axis, s);
out->ki = double(s);
4547 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_ILIM_GAIN, axis, s);
out->max_int = double(s);
4549 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_OFFSET, axis, s);
out->offset= double(s);
4551 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_SCALE, axis, s);
out->scale = double(s);
4553 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_TLIM, axis, s);
out->max_output = double(s);
4554 DEBUG_FUNC(
"Calling CAN_GET_POS_STICTION_PARAMS\n");
4555 _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS, axis, s, s2 );
out->stiction_up_val = double(s);
out->stiction_down_val = double(s2);
4569 case VOCAB_PIDTYPE_POSITION:
4572 case VOCAB_PIDTYPE_VELOCITY:
4575 case VOCAB_PIDTYPE_CURRENT:
4578 case VOCAB_PIDTYPE_TORQUE:
4582 yError()<<
"Invalid pidtype:"<<pidtype;
4616 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID, axis);
4624 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS, axis);
4632 _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS, axis,
S_16(pid.stiction_up_val),
S_16(pid.stiction_down_val),
false);
4635 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_MODEL_PARAMS, axis);
4673 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID);
4683 yError(
"getTorquePid: message timed out\n");
4689 CanMessage *m=t->
get(0);
4697 unsigned char *
data;
4698 data=m->getData()+1;
4705 out->scale= *((
char *)(
data));
4711 DEBUG_FUNC(
"Calling CAN_GET_TORQUE_PIDLIMITS\n");
4714 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS);
4724 yError(
"getTorquePidLimits: message timed out\n");
4738 data=m->getData()+1;
4739 out->offset= *((
short *)(
data));
4741 out->max_output= *((
short *)(
data));
4743 out->max_int= *((
short *)(
data));
4749 DEBUG_FUNC(
"Calling CAN_GET_MODEL_PARAMS\n");
4752 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_MODEL_PARAMS);
4762 yError(
"CAN_GET_MODEL_PARAMS: message timed out\n");
4776 data=m->getData()+1;
4781 DEBUG_FUNC(
"Calling CAN_GET_TORQUE_STICTION_PARAMS\n");
4784 _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS, axis, s1, s2 );
4785 out->stiction_up_val = double(s1);
4786 out->stiction_down_val = double(s2);
4794 if (pids==0)
return false;
4815 if (refs==0)
return false;
4835 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, axis,
S_16(ref_trq));
4841 for(
int j=0; j< n_joint; j++)
4856 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4857 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4871 if (
_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, i,
S_16(ref_trqs[i])) !=
true)
4883 for (
int j = 0; j < r.
getJoints() && ret; j++)
4897 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4901 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4903 std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
4904 while(it!=analogSensors.end())
4908 int id = (*it)->getId();
4936 if (!(axis >= 0 && axis <= r.
getJoints()))
4939 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4942 case VOCAB_PIDTYPE_POSITION:
4945 case VOCAB_PIDTYPE_TORQUE:
4948 case VOCAB_PIDTYPE_VELOCITY:
4952 case VOCAB_PIDTYPE_CURRENT:
4963 if (!(axis >= 0 && axis <= r.
getJoints()))
4965 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4974 for (
int j = 0; j < r.
getJoints() && ret; j++)
4984 for (
int j = 0; j < nj && ret; j++)
4994 if (!(axis >= 0 && axis <= r.
getJoints()))
4996 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5005 for (
int j = 0; j < r.
getJoints() && ret; j++)
5015 for (
int j = 0; j < nj && ret; j++)
5025 if (!(axis >= 0 && axis <= r.
getJoints()))
5035 for (
int j = 0; j < r.
getJoints() && ret; j++)
5045 for (
int j = 0; j < nj && ret; j++)
5096 yError(
"getParameterRaw: message timed out\n");
5102 CanMessage *m=t->
get(0);
5110 unsigned char *
data;
5111 data=m->getData()+1;
5112 *value= *((
short *)(
data));
5143 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_DEBUG_PARAM);
5144 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = index;
5155 yError(
"getDebugParameterRaw: message timed out\n");
5161 CanMessage *m=t->
get(0);
5169 unsigned char *
data;
5170 data=m->getData()+1;
5171 *value= *((
short *)(
data));
5184 if (
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, val) ==
true)
5185 *value = double (val);
5217 fw_info->
joint=axis;
5222 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION);
5223 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = (
unsigned char)(icub_interface_protocol.
major & 0xFF);
5224 *((
unsigned char *)(r.
_writeBuffer[0].getData()+2)) = (
unsigned char)(icub_interface_protocol.
minor & 0xFF);
5235 yError(
"getFirmwareVersion: message timed out\n");
5243 CanMessage *m=t->
get(0);
5246 yError(
"getFirmwareVersion: message error\n");
5254 unsigned char *
data;
5255 data=m->getData()+1;
5268 fw_info->
ack = *((
char *)(
data));
5284 case VOCAB_PIDTYPE_POSITION:
5287 if (
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, value) ==
true)
5288 { *ref = double (value);}
5290 { *ref =0; yError() <<
"Invalid _readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION)";
return false; }
5293 case VOCAB_PIDTYPE_VELOCITY:
5299 case VOCAB_PIDTYPE_CURRENT:
5305 case VOCAB_PIDTYPE_TORQUE:
5314 yError()<<
"Invalid pidtype:"<<pidtype;
5325 if (refs==0)
return false;
5389 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5391 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_DEBUG_PARAM, axis);
5392 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = (
unsigned char)(index & 0xFF);
5407 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION, axis,
S_32(value));
5413 if (!(axis >= 0 && axis <= r.
getJoints()))
5416 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5419 case VOCAB_PIDTYPE_POSITION:
5422 case VOCAB_PIDTYPE_VELOCITY:
5425 case VOCAB_PIDTYPE_CURRENT:
5428 case VOCAB_PIDTYPE_TORQUE:
5432 yError()<<
"Invalid pidtype:"<<pidtype;
5443 getPidOutput(pidtype, i, &outs[i]);
5469 yWarning() <<
"Performance warning: You are using positionMove commands at high rate (<"<<
MAX_POSITION_MOVE_INTERVAL*1000.0 <<
" ms). Probably position control mode is not the right control mode to use.";
5475 if (mode != VOCAB_CM_POSITION &&
5476 mode != VOCAB_CM_MIXED &&
5477 mode != VOCAB_CM_IMPEDANCE_POS &&
5478 mode != VOCAB_CM_IDLE)
5480 yError() <<
"positionMoveRaw: skipping command because " << networkName.c_str() <<
" joint " << axis <<
"is not in VOCAB_CM_POSITION mode";
5484 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5487 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__POSITION_MOVE, axis);
5502 if (refs == 0)
return false;
5530 if (!
_readWord16 (ICUBCANPROTO_POL_MC_CMD__MOTION_DONE, axis, value))
5563 r.
addMessage (
id, i, ICUBCANPROTO_POL_MC_CMD__MOTION_DONE);
5584 for (i = 0, j = 0; i < r.
getJoints(); i++)
5589 if ( (m!=0) && (m->getId() != 0xffff) )
5591 value = *((
short *)(m->getData()+1));
5655 return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_ACCELER, axis, s);
5671 acc = accs[i]/1000.0;