22 #pragma warning(disable:4355)
25 #include <yarp/os/Time.h>
33 #include <yarp/dev/ControlBoardHelper.h>
34 #include <yarp/dev/ControlBoardInterfacesImpl.h>
35 #include <yarp/os/LogStream.h>
37 using namespace yarp::os;
42 yError(
"%s not yet implemented for iCubSimulationControl\n", txt);
49 yError() << txt <<
" has been deprecated for embObjMotionControl";
55 iCubSimulationControl::iCubSimulationControl() :
57 ImplementPositionControl(this),
58 ImplementVelocityControl(this),
59 ImplementPidControl(this),
60 ImplementEncodersTimed(this),
61 ImplementTorqueControl(this),
62 ImplementControlMode(this),
63 ImplementControlCalibration(this),
64 ImplementAmplifierControl(this),
65 ImplementControlLimits(this),
66 ImplementInteractionMode(this),
67 ImplementPositionDirect(this),
68 ImplementMotorEncoders(this),
69 ImplementRemoteVariables(this),
70 ImplementAxisInfo(this),
72 ImplementPWMControl(this),
73 ImplementCurrentControl(this)
83 lock_guard<mutex> lck(odeinit.
mtx);
89 Searchable&
p = config;
91 if (!
p.check(
"GENERAL",
"section for general motor control parameters")) {
92 yError(
"Cannot understand configuration parameters");
96 int TypeArm =
p.findGroup(
"GENERAL").check(
"Type",Value(1),
97 "what did the user select?").asInt32();
99 int numTOTjoints =
p.findGroup(
"GENERAL").check(
"TotalJoints",Value(1),
100 "Number of total joints").asInt32();
102 double velocity =
p.findGroup(
"GENERAL").check(
"Vel",Value(1),
103 "Default velocity").asFloat64();
182 for(
int axis = 0;axis<
njoints;axis++)
191 Bottle& xtmp =
p.findGroup(
"GENERAL").findGroup(
"Encoder",
"a list of scales for the encoders");
192 if (xtmp.size() !=
njoints+1) {
193 yError(
"Encoder does not have the right number of entries\n");
196 for (
int i = 1; i < xtmp.size(); i++)
angleToEncoder[i-1] = xtmp.get(i).asFloat64();
198 xtmp =
p.findGroup(
"GENERAL").findGroup(
"fullscalePWM",
"a list of scales for the fullscalePWM param");
199 if (xtmp.size() !=
njoints + 1) {
200 yError(
"fullscalePWM does not have the right number of entries\n");
203 for (
int i = 1; i < xtmp.size(); i++)
dutycycleToPwm[i - 1] = xtmp.get(i).asFloat64()/100.0;
205 xtmp =
p.findGroup(
"GENERAL").findGroup(
"ampsToSensor",
"a list of scales for the ampsToSensor param");
206 if (xtmp.size() !=
njoints + 1) {
207 yError(
"ampsToSensor does not have the right number of entries\n");
210 for (
int i = 1; i < xtmp.size(); i++)
ampsToSensor[i - 1] = xtmp.get(i).asFloat64();
212 xtmp =
p.findGroup(
"GENERAL").findGroup(
"Zeros",
"a list of offsets for the zero point");
213 if (xtmp.size() !=
njoints+1) {
214 yError(
"Zeros does not have the right number of entries\n");
217 for (
int i = 1; i < xtmp.size(); i++)
zeros[i-1] = xtmp.get(i).asFloat64();
219 int mj_size =
p.findGroup(
"GENERAL").check(
"Kinematic_mj_size",Value(0),
"Default velocity").asInt32();
222 xtmp =
p.findGroup(
"GENERAL").findGroup(
"Kinematic_mj");
223 if (xtmp.size() != mj_size*mj_size) {
224 yError(
"Invalid Kinematic_mj size\n");
227 for (
int c = 0; c < mj_size; c++)
228 for (
int r = 0; r < mj_size; r++)
244 xtmp =
p.findGroup(
"LIMITS").findGroup(
"jntPosMax",
"access the joint limits max");
247 yError(
"Not enough max joint limits\n");
250 for(
int i =1;i<xtmp.size();i++ )
254 for (
int i = 1; i < xtmp.size(); i++)
260 xtmp =
p.findGroup(
"LIMITS").findGroup(
"jntPosMin",
"access the joint limits min");
263 yError(
"Not enough min joint limits\n");
266 for(
int i =1;i<xtmp.size();i++)
269 xtmp =
p.findGroup(
"LIMITS").findGroup(
"error_tol",
"error tolerance during tracking");
272 yError(
"Not enough error_tol\n");
275 for(
int i=1;i<xtmp.size();i++)
278 for(
int axis =0;axis<
njoints;axis++)
318 if (!
p.check(
"joint_device")) {
319 yError(
"Need a device to access the joints\n");
322 if (!
joints.open(
p.find(
"joint_device").asString().c_str())) {
323 yError(
"Failed to create a device to access the joints\n");
329 yError(
"Wrong type for device to access the joints\n");
336 odeinit.
mtx.unlock();
355 ImplementPositionControl::uninitialize();
356 ImplementVelocityControl::uninitialize();
357 ImplementTorqueControl::uninitialize();
358 ImplementPidControl::uninitialize();
359 ImplementEncodersTimed::uninitialize();
360 ImplementMotorEncoders::uninitialize();
361 ImplementControlCalibration::uninitialize();
362 ImplementAmplifierControl::uninitialize();
363 ImplementControlLimits::uninitialize();
364 ImplementControlMode::uninitialize();
365 ImplementInteractionMode::uninitialize();
366 ImplementPositionDirect::uninitialize();
367 ImplementRemoteVariables::uninitialize();
368 ImplementAxisInfo::uninitialize();
369 ImplementMotor::uninitialize();
370 ImplementCurrentControl::uninitialize();
371 ImplementPWMControl::uninitialize();
378 checkAndDestroy<double>(
pwm);
379 checkAndDestroy<double>(
pwm_ref);
400 checkAndDestroy<double>(
zeros);
409 checkAndDestroy<int>(
inputs);
410 checkAndDestroy<double>(
vels);
425 checkAndDestroy<double>(
gearbox);
444 void iCubSimulationControl::compute_mot_pos_from_jnt_pos(
double *mot_pos,
const double *jnt_pos,
int size_joints)
446 for (
int i = 0; i < size_joints; i++)
448 mot_pos[i] = jnt_pos[i];
452 void iCubSimulationControl::compute_mot_vel_and_acc(
double *mot_vel,
double *mot_acc,
const double *mot_pos,
int size_joints)
457 for (
int i = 0; i < size_joints; i++) mot_vel[i] = speeds[i];
458 for (
int i = 0; i < size_joints; i++) mot_acc[i] = accs[i];
461 void iCubSimulationControl::compute_jnt_vel_and_acc(
double *jnt_vel,
double *jnt_acc,
const double *jnt_pos,
int size_joints)
466 for (
int i = 0; i < size_joints; i++) jnt_vel[i] = speeds[i];
467 for (
int i = 0; i < size_joints; i++) jnt_acc[i] = accs[i];
471 lock_guard<mutex> lck(
_mutex);
477 for (
int axis=0; axis<
njoints; axis++)
510 else if (
controlMode[axis]==VOCAB_CM_POSITION_DIRECT)
571 for (
int axis = 0; axis <
njoints; axis++)
591 case VOCAB_PIDTYPE_POSITION:
594 case VOCAB_PIDTYPE_VELOCITY:
597 case VOCAB_PIDTYPE_CURRENT:
600 case VOCAB_PIDTYPE_TORQUE:
616 case VOCAB_PIDTYPE_POSITION:
619 case VOCAB_PIDTYPE_VELOCITY:
622 case VOCAB_PIDTYPE_CURRENT:
625 case VOCAB_PIDTYPE_TORQUE:
656 if( (axis>=0) && (axis<
njoints) )
662 case VOCAB_PIDTYPE_POSITION:
665 case VOCAB_PIDTYPE_VELOCITY:
668 case VOCAB_PIDTYPE_CURRENT:
671 case VOCAB_PIDTYPE_TORQUE:
681 yError(
"setReferenceRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
689 for(
int axis = 0; axis<
njoints; axis++)
708 if ((axis >= 0) && (axis<
njoints))
710 lock_guard<mutex> lck(
_mutex);
713 case VOCAB_PIDTYPE_POSITION:
716 case VOCAB_PIDTYPE_TORQUE:
719 case VOCAB_PIDTYPE_VELOCITY:
722 case VOCAB_PIDTYPE_CURRENT:
729 yError(
"getErrorRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
736 for (
int axis = 0; axis<
njoints; axis++)
745 if( (axis>=0) && (axis<
njoints) )
751 case VOCAB_PIDTYPE_POSITION:
752 if (
mode == VOCAB_CM_POSITION_DIRECT ||
753 mode == VOCAB_CM_POSITION ||
754 mode == VOCAB_CM_MIXED)
756 lock_guard<mutex> lck(
_mutex);
761 lock_guard<mutex> lck(
_mutex);
765 case VOCAB_PIDTYPE_VELOCITY:
766 if (
mode == VOCAB_CM_VELOCITY)
768 lock_guard<mutex> lck(
_mutex);
773 lock_guard<mutex> lck(
_mutex);
777 case VOCAB_PIDTYPE_CURRENT:
778 if (
mode == VOCAB_CM_CURRENT)
780 lock_guard<mutex> lck(
_mutex);
785 lock_guard<mutex> lck(
_mutex);
789 case VOCAB_PIDTYPE_TORQUE:
790 if (
mode == VOCAB_CM_TORQUE)
792 lock_guard<mutex> lck(
_mutex);
797 lock_guard<mutex> lck(
_mutex);
803 lock_guard<mutex> lck(
_mutex);
811 yError(
"getOutputRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
818 lock_guard<mutex> lck(
_mutex);
819 for(
int axis = 0; axis<
njoints; axis++)
820 outs[axis] =
pwm[axis];
897 if( (axis>=0) && (axis<
njoints) )
901 lock_guard<mutex> lck(
_mutex);
904 case VOCAB_PIDTYPE_POSITION:
907 case VOCAB_PIDTYPE_VELOCITY:
910 case VOCAB_PIDTYPE_CURRENT:
913 case VOCAB_PIDTYPE_TORQUE:
923 yError(
"getReferenceRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
930 for(
int axis = 0; axis<
njoints; axis++)
969 if( (axis >=0) && (axis<
njoints) )
973 if (
mode != VOCAB_CM_POSITION &&
974 mode != VOCAB_CM_MIXED )
976 yError() <<
"positionMoveRaw: skipping command because part " <<
partSelec <<
" joint " << axis <<
"is not in VOCAB_CM_POSITION mode";
980 lock_guard<mutex> lck(
_mutex);
1016 if ( axis == 10 || axis == 12 || axis == 14 )
1018 else if ( axis == 15 )
1020 else if ( axis == 7 )
1034 yDebug(
"moving joint %d of part %d to pos %f\n",axis,
partSelec,
next_pos[axis]);
1038 yError(
"positionMoveRaw joint access too high %d \n",axis);
1045 for(
int axis = 0; axis<
njoints; axis++)
1059 for(
int axis = 0; axis<
njoints; axis++) {
1067 lock_guard<mutex> lck(
_mutex);
1069 for(
int axis = 0;axis<
njoints;axis++)
1084 if( (axis >=0) && (axis<
njoints) )
1086 lock_guard<mutex> lck(
_mutex);
1094 yError(
"checkMotionDoneRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1099 if( (axis >=0) && (axis<
njoints) )
1101 lock_guard<mutex> lck(
_mutex);
1106 yDebug(
"setting joint %d of part %d to reference velocity %f\n",axis,
partSelec,
vels[axis]);
1115 for (
int axis = 0; axis<
njoints; axis++)
1131 if((axis>=0) && (axis<
njoints)) {
1132 lock_guard<mutex> lck(
_mutex);
1137 yError(
"getRefSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1143 for (
int i = 0; i<
njoints; i++)
1159 if( (axis>=0) && (axis<
njoints) )
1161 lock_guard<mutex> lck(
_mutex);
1167 yError(
"stopRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1172 lock_guard<mutex> lck(
_mutex);
1173 for(
int axis=0;axis<
njoints;axis++)
1183 if ((axis >= 0) && (axis<
njoints)) {
1184 lock_guard<mutex> lck(
_mutex);
1189 yError(
"getTargetPositionRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1196 for (
int i = 0; i<
njoints; i++)
1206 for (
int i = 0; i<nj; i++)
1215 lock_guard<mutex> lck(
_mutex);
1223 for (
int i = 0; i<
njoints; i++)
1233 for (
int i = 0; i<nj; i++)
1242 lock_guard<mutex> lck(
_mutex);
1250 for (
int i = 0; i<
njoints; i++)
1261 for (
int i = 0; i<nj; i++)
1271 for (
int i = 0; i<nj; i++)
1281 for (
int i = 0; i<nj; i++)
1291 bool jDone(
true), tmpDone(
true);
1293 for (
int i = 0; i<nj; i++)
1305 for (
int i = 0; i<nj; i++)
1315 for (
int i = 0; i<nj; i++)
1326 if( (axis >=0) && (axis<
njoints) )
1330 if (
mode != VOCAB_CM_VELOCITY &&
1331 mode != VOCAB_CM_MIXED)
1333 yError() <<
"velocityMoveRaw: skipping command because part " <<
partSelec <<
" joint " << axis <<
"is not in VOCAB_CM_VELOCITY mode";
1336 lock_guard<mutex> lck(
_mutex);
1343 yError(
"velocityMoveRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,
njoints);
1352 for (
int axis=0; axis<
njoints; axis++)
1381 lock_guard<mutex> lck(
_mutex);
1382 for(
int axis = 0;axis<
njoints;axis++)
1384 if ( axis == 10 || axis == 12 || axis == 14 )
1386 else if ( axis == 15 )
1388 else if ( axis == 7 )
1398 if((axis>=0) && (axis<
njoints)) {
1399 lock_guard<mutex> lck(
_mutex);
1401 if ( axis == 10 || axis == 12 || axis == 14 )
1403 else if ( axis == 15 )
1405 else if ( axis == 7 )
1413 yError(
"getEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1419 double timeNow = Time::now();
1420 for(
int axis = 0;axis<
njoints;axis++)
1422 stamps[axis] = timeNow;
1429 *stamp = Time::now();
1436 lock_guard<mutex> lck(
_mutex);
1437 for(
int axis = 0; axis<
njoints; axis++)
1444 if( (axis>=0) && (axis<
njoints) ) {
1445 lock_guard<mutex> lck(
_mutex);
1450 yError(
"getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1456 lock_guard<mutex> lck(
_mutex);
1457 for (
int axis = 0; axis<
njoints; axis++)
1464 if ((axis >= 0) && (axis<
njoints)) {
1465 lock_guard<mutex> lck(
_mutex);
1470 yError(
"getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1496 for(
int axis = 0;axis<
njoints;axis++)
1505 if((axis>=0) && (axis<
njoints))
1507 lock_guard<mutex> lck(
_mutex);
1512 yError(
"getMotorEncoderRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1535 double timeNow = Time::now();
1536 for(
int axis = 0;axis<
njoints;axis++)
1538 stamps[axis] = timeNow;
1545 *stamp = Time::now();
1552 for(
int axis = 0; axis<
njoints; axis++)
1561 if( (axis>=0) && (axis<
njoints) ) {
1562 lock_guard<mutex> lck(
_mutex);
1567 yError(
"getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1573 for (
int axis = 0; axis<
njoints; axis++)
1582 if ((axis >= 0) && (axis<
njoints)) {
1583 lock_guard<mutex> lck(
_mutex);
1588 yError(
"getEncoderSpeedRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1605 lock_guard<mutex> lck(
_mutex);
1606 for(
int axis = 0; axis<
njoints; axis++)
1614 if( (axis>=0) && (axis<
njoints) )
1616 lock_guard<mutex> lck(
_mutex);
1622 yError(
"getCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1629 if( (axis>=0) && (axis<
njoints) )
1631 lock_guard<mutex> lck(
_mutex);
1637 yError(
"setMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1644 if( (axis>=0) && (axis<
njoints) )
1646 lock_guard<mutex> lck(
_mutex);
1652 yError(
"getMaxCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1670 lock_guard<mutex> lck(
_mutex);
1671 for(
int axis =0;axis<
njoints;axis++)
1678 if( (axis>=0) && (axis<
njoints))
1680 lock_guard<mutex> lck(
_mutex);
1685 yError(
"getAmpStatusRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1691 if( (axis >=0) && (axis <
njoints) ){
1692 lock_guard<mutex> lck(
_mutex);
1698 yError(
"setLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1704 if( (axis >=0) && (axis <
njoints)) {
1705 lock_guard<mutex> lck(
_mutex);
1718 if (key ==
"kinematic_mj")
1720 val.addString(
"1 2 3");
return true;
1722 else if (key ==
"rotor")
1724 Bottle& r = val.addList();
for (
int i=0; i<
njoints; i++) r.addFloat64(
rotToEncoder[i]);
return true;
1726 else if (key ==
"gearbox")
1728 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addFloat64(
gearbox[i]);
return true;
1730 else if (key ==
"zeros")
1732 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addFloat64(
zeros[i]);
return true;
1734 else if (key ==
"hasHallSensor")
1736 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addInt32(
hasHallSensor[i]);
return true;
1738 else if (key ==
"hasTempSensor")
1740 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addInt32(
hasTempSensor[i]);
return true;
1742 else if (key ==
"hasRotorEncoder")
1746 else if (key ==
"rotorIndexOffset")
1750 else if (key ==
"motorPoles")
1752 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addInt32(
motorPoles[i]);
return true;
1754 else if (key ==
"pidCurrentKp")
1756 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addFloat64(
current_pid[i].kp);
return true;
1758 else if (key ==
"pidCurrentKi")
1760 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addFloat64(
current_pid[i].ki);
return true;
1762 else if (key ==
"pidCurrentShift")
1764 Bottle& r = val.addList();
for (
int i = 0; i<
njoints; i++) r.addFloat64(
current_pid[i].scale);
return true;
1766 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
1772 string s1 = val.toString();
1773 Bottle* bval = val.get(0).asList();
1776 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
1780 string s2 = bval->toString();
1781 if (key ==
"kinematic_mj")
1785 else if (key ==
"rotor")
1787 for (
int i = 0; i <
njoints; i++)
1791 else if (key ==
"gearbox")
1793 for (
int i = 0; i <
njoints; i++)
gearbox[i] = bval->get(i).asFloat64();
return true;
1795 else if (key ==
"zeros")
1797 for (
int i = 0; i <
njoints; i++)
zeros[i] = bval->get(i).asFloat64();
return true;
1799 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
1805 listOfKeys->clear();
1806 listOfKeys->addString(
"kinematic_mj");
1807 listOfKeys->addString(
"rotor");
1808 listOfKeys->addString(
"gearbox");
1809 listOfKeys->addString(
"hasHallSensor");
1810 listOfKeys->addString(
"hasTempSensor");
1811 listOfKeys->addString(
"hasRotorEncoder");
1812 listOfKeys->addString(
"rotorIndexOffset");
1813 listOfKeys->addString(
"motorPoles");
1814 listOfKeys->addString(
"pidCurrentKp");
1815 listOfKeys->addString(
"pidCurrentKi");
1816 listOfKeys->addString(
"pidCurrentShift");
1822 if ((axis >= 0) && (axis <
njoints)){
1823 lock_guard<mutex> lck(
_mutex);
1829 yError(
"setVelLimitsRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", axis,
njoints);
1835 if ((axis >= 0) && (axis <
njoints)) {
1836 lock_guard<mutex> lck(
_mutex);
1848 for(
int j=0; j<n_joint; j++)
1858 for(
int j=0; j<n_joint; j++)
1868 for(
int j=0; j<n_joint; j++)
1878 for(
int j=0; j<n_joint; j++)
1887 if ((axis >= 0) && (axis <
njoints))
1889 lock_guard<mutex> lck(
_mutex);
1891 sprintf(buff,
"JOINT%d", axis);
1892 name.assign(buff,strlen(buff));
1901 if ((axis >= 0) && (axis <
njoints))
1903 lock_guard<mutex> lck(
_mutex);
1904 type = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE;
1913 if( (axis >=0) && (axis <
njoints)) {
1914 lock_guard<mutex> lck(
_mutex);
1922 lock_guard<mutex> lck(
_mutex);
1923 for(
int axis = 0;axis<
njoints;axis++)
1938 for (
int i = 0; i<
njoints; i++)
1947 if( (axis >=0) && (axis<
njoints) ) {
1948 lock_guard<mutex> lck(
_mutex);
1955 yError(
"setRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,
njoints);
1962 for (
int j = 0; j<n_joint; j++)
1972 for (
int i = 0; i<
njoints; i++)
1980 if( (axis >=0) && (axis<
njoints) ) {
1981 lock_guard<mutex> lck(
_mutex);
1986 yError(
"getRefTorqueRaw: joint with index %d does not exist, valis joints indices are between 0 and %d \n",axis,
njoints);
1992 if( (axis >=0) && (axis<
njoints) )
2009 if( (axis >=0) && (axis<
njoints) )
2024 int iCubSimulationControl::ControlModes_yarp2iCubSIM(
int yarpMode)
2026 int ret = VOCAB_CM_UNKNOWN;
2029 case VOCAB_CM_FORCE_IDLE:
2033 case VOCAB_CM_POSITION:
2036 case VOCAB_CM_VELOCITY:
2039 case VOCAB_CM_TORQUE:
2042 case VOCAB_CM_IMPEDANCE_POS:
2045 case VOCAB_CM_IMPEDANCE_VEL:
2051 case VOCAB_CM_CURRENT:
2056 case VOCAB_CM_MIXED:
2057 case VOCAB_CM_POSITION_DIRECT:
2058 case VOCAB_CM_HW_FAULT:
2059 case VOCAB_CM_CALIBRATING:
2060 case VOCAB_CM_CALIB_DONE:
2061 case VOCAB_CM_NOT_CONFIGURED:
2062 case VOCAB_CM_CONFIGURED:
2073 int iCubSimulationControl::ControlModes_iCubSIM2yarp(
int iCubMode)
2075 int ret = VOCAB_CM_UNKNOWN;
2082 ret=VOCAB_CM_POSITION;
2085 ret=VOCAB_CM_VELOCITY;
2088 ret=VOCAB_CM_TORQUE;
2091 ret=VOCAB_CM_IMPEDANCE_POS;
2094 ret=VOCAB_CM_IMPEDANCE_VEL;
2100 ret = VOCAB_CM_CURRENT;
2106 case VOCAB_CM_POSITION_DIRECT:
2115 ret=VOCAB_CM_UNKNOWN;
2123 if( (j >=0) && (j <
njoints)) {
2124 lock_guard<mutex> lck(
_mutex);
2129 yError(
"getControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2135 lock_guard<mutex> lck(
_mutex);
2136 for(
int axis = 0;axis<
njoints;axis++)
2137 modes[axis] = ControlModes_iCubSIM2yarp(
controlMode[axis]);
2153 if( (j >=0) && (j <
njoints) )
2155 tmp = ControlModes_yarp2iCubSIM(
mode);
2158 yError() <<
"setControlModeRaw: unknown control mode " << yarp::os::Vocab32::decode(
mode);
2160 else if (
controlMode[j] == VOCAB_CM_HW_FAULT &&
mode != VOCAB_CM_FORCE_IDLE)
2162 yError() <<
"setControlModeRaw: unable to reset an HW_FAULT without a VOCAB_CM_FORCE_IDLE command";
2166 lock_guard<mutex> lck(
_mutex);
2175 yError(
"setControlModeRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2257 if( (axis >=0) && (axis<
njoints) )
2261 if (
mode != VOCAB_CM_POSITION_DIRECT)
2263 yError() <<
"setPositionRaw: skipping command because part " <<
partSelec <<
" joint" << axis <<
"is not in VOCAB_CM_POSITION_DIRECT mode";
2266 lock_guard<mutex> lck(
_mutex);
2302 if ( axis == 10 || axis == 12 || axis == 14 )
2304 else if ( axis == 15 )
2306 else if ( axis == 7 )
2320 yDebug(
"moving joint %d of part %d to pos %f (pos direct)\n",axis,
partSelec,
next_pos[axis]);
2324 yError(
"setPositionRaw joint access too high %d \n",axis);
2331 for(
int i=0; i<n_joint; i++)
2353 lock_guard<mutex> lck(
_mutex);
2358 yError(
"setRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2364 lock_guard<mutex> lck(
_mutex);
2365 for (
int axis = 0; axis<
njoints; axis++)
2374 lock_guard<mutex> lck(
_mutex);
2379 yError(
"getRefDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2385 lock_guard<mutex> lck(
_mutex);
2386 for (
int axis = 0; axis<
njoints; axis++)
2395 lock_guard<mutex> lck(
_mutex);
2400 yError(
"getDutyCycleRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2406 lock_guard<mutex> lck(
_mutex);
2407 for (
int axis = 0; axis<
njoints; axis++)
2408 v[axis] =
pwm[axis];
2428 lock_guard<mutex> lck(
_mutex);
2434 yError(
"setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2440 lock_guard<mutex> lck(
_mutex);
2441 for (
int axis = 0; axis <
njoints; axis++)
2451 lock_guard<mutex> lck(
_mutex);
2452 for (
int axis = 0; axis<
njoints; axis++)
2461 lock_guard<mutex> lck(
_mutex);
2466 yError(
"setRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
2473 for (
int j = 0; j<n_joint; j++)
2482 lock_guard<mutex> lck(
_mutex);
2483 for (
int axis = 0; axis<
njoints; axis++)
2492 lock_guard<mutex> lck(
_mutex);
2497 yError(
"getRefCurrentRaw: joint with index %d does not exist; valid joint indices are between 0 and %d\n", j,
njoints);
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
Abstract class for mapping from "physical" (simulated) joints in the model to control joints in the I...
virtual double getTorque()=0
this is just a fake torque interface for now
virtual double getAngle()=0
Get the angle of an associated joint, in ICUB units and sign.
virtual void setVelocity(double target)=0
Set velocity of joint (in ICUB units/sign).
virtual double getVelocity()=0
Get the angular velocity of an associated joint, in ICUB units and sign.
virtual void setTorque(double target)=0
Set the reference torque.
virtual void setControlParameters(double vel, double acc)=0
Set velocity and acceleration control parameters.
virtual void setPosition(double target)=0
Drive towards an angle setpoint (in ICUB units/sign).
virtual LogicalJoint & control(int part, int axis)=0
Access the control for a logical joint, based on part and axis number.
void removeSimulationControl(int part)
void setSimulationControl(iCubSimulationControl *control, int part)
Adaptive window linear fitting to estimate the first derivative.
Basic element for adaptive polynomial fitting.
yarp::sig::Vector estimate()
Execute the algorithm upon the elements list, with the max deviation threshold given by D.
Adaptive window quadratic fitting to estimate the second derivative.
virtual bool getEncoderTimedRaw(int j, double *encs, double *stamp)
iCub::ctrl::AWLinEstimator * linEstMot
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool getEncodersRaw(double *encs) override
iCub::ctrl::AWQuadEstimator * quadEstJnt
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool getRefDutyCycleRaw(int j, double *v) override
double * estimated_jnt_vel
virtual bool setRefDutyCycleRaw(int j, double v) override
virtual bool setEncodersRaw(const double *vals) override
virtual bool resetEncoderRaw(int j) override
yarp::sig::Matrix kinematic_mj
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
virtual bool getTargetPositionsRaw(double *refs) override
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
virtual bool resetMotorEncoderRaw(int m) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool velocityMoveRaw(int j, double sp) override
for each axis
virtual bool setRefAccelerationsRaw(const double *accs) override
MotorTorqueParameters * motor_torque_params
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool setPWMLimitRaw(int j, const double val) override
virtual bool setRefTorqueRaw(int, double) override
virtual bool getNumberOfMotorEncodersRaw(int *num) override
double * newtonsToSensor
encoder zeros
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool getMaxCurrentRaw(int j, double *val) override
virtual bool getDutyCyclesRaw(double *v) override
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getPWMRaw(int j, double *val) override
virtual bool getMotorEncodersTimedRaw(double *encs, double *stamps) override
double * current_ampere_ref
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool getControlModeRaw(int j, int *mode) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getRefTorquesRaw(double *) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool setControlModeRaw(const int j, const int mode) override
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getAmpStatusRaw(int *st) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis pointer with 1 double arg
virtual bool setMotorEncodersRaw(const double *vals) override
virtual bool getTorquesRaw(double *) override
virtual bool getTemperaturesRaw(double *vals) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled) override
double * current_mot_torques
virtual bool calibrationDoneRaw(int j) override
virtual bool getTorqueRangesRaw(double *, double *) override
virtual bool positionMoveRaw(int j, double ref) override
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getCurrentsRaw(double *vals) override
double * current_jnt_torques
virtual bool getTorqueRangeRaw(int, double *, double *) override
virtual bool disableAmpRaw(int j) override
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool enableAmpRaw(int j) override
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getNominalCurrentRaw(int m, double *val) override
virtual bool getAxisNameRaw(int axis, std::string &name) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getTorqueRaw(int, double *) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
virtual bool checkMotionDoneRaw(bool *flag) override
yarp::dev::PolyDriver joints
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getRefAccelerationsRaw(double *accs) override
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
virtual bool getCurrentRaw(int j, double *val) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool getRefCurrentsRaw(double *t) override
double * ref_command_positions
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool getRefSpeedsRaw(double *spds) override
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool getRefPositionsRaw(double *refs) override
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setRefDutyCyclesRaw(const double *v) override
double * estimated_mot_acc
virtual bool setPositionRaw(int j, double ref) override
virtual bool getMotorEncoderRaw(int m, double *v) override
virtual bool setRefTorquesRaw(const double *) override
virtual bool getControlModesRaw(int *modes) override
double * estimated_jnt_acc
double * ref_command_speeds
virtual ~iCubSimulationControl()
Destructor.
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool stopRaw() override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
virtual bool setRefCurrentsRaw(const double *t) override
virtual bool getEncodersTimedRaw(double *encs, double *stamps)
virtual bool resetEncodersRaw() override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool getDutyCycleRaw(int j, double *v) override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool setEncoderRaw(int j, double val) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
iCub::ctrl::AWLinEstimator * linEstJnt
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
double * estimated_mot_vel
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
virtual bool getMotorEncoderTimedRaw(int m, double *encs, double *stamp) override
virtual bool resetMotorEncodersRaw() override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool getRefTorqueRaw(int, double *) override
virtual bool setMotorEncoderRaw(int m, const double val) override
iCub::ctrl::AWQuadEstimator * quadEstMot
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getPWMLimitRaw(int j, double *val) override
static uint32_t idx[BOARD_NUM]
static bool NOT_YET_IMPLEMENTED(const char *txt)
static bool DEPRECATED(const char *txt)
This is the header file for the yarp interface of the iCubSimulation.
#define MODE_IMPEDANCE_VEL
#define MODE_IMPEDANCE_POS
#define MODE_NOT_CONFIGURED