Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
const double eyeTiltMin, const double eyeTiltMax)
{
IEncoders *encs;
IControlLimits *lims;
double min, max;
int nJointsTorso=1;
if (drvTorso!=NULL)
{
drvTorso->view(encs);
drvTorso->view(lims);
encs->getAxes(&nJointsTorso);
for (int i=0; i<nJointsTorso; i++)
{
lims->getLimits(i,&min,&max);
(*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min); // reversed order
(*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
}
}
drvHead->view(encs);
drvHead->view(lims);
int nJointsHead;
encs->getAxes(&nJointsHead);
Matrix lim(nJointsHead,2);
for (int i=0; i<nJointsHead; i++)
{
lims->getLimits(i,&min,&max);
// limit eye's tilt due to eyelids
if (i==2)
{
min=std::max(min,eyeTiltMin);
max=std::min(max,eyeTiltMax);
}
lim(i,0)=CTRL_DEG2RAD*min;
lim(i,1)=CTRL_DEG2RAD*max;
// just one eye's got only 5 dofs
if (i<nJointsHead-1)
{
(*chain)[nJointsTorso+i].setMin(lim(i,0));
(*chain)[nJointsTorso+i].setMax(lim(i,1));
}
}
return lim;
}
int main(int argc, char *argv[])
{
// just list the devices if no argument given
if (argc <= 2) {
printf("You can call %s like this:\n", argv[0]);
printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
printf("For example:\n");
printf(" %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
printf("Here are devices listed for your system:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
// get command line options
Property options;
options.fromCommand(argc, argv);
if (!options.check("robot") || !options.check("part")) {
printf("Missing either --robot or --part options\n");
return 0;
}
Network::init();
Time::turboBoost();
std::string name;
Value& v = options.find("robot");
Value& part = options.find("part");
Value *val;
if (!options.check("device", val)) {
options.put("device", "remote_controlboard");
}
if (!options.check("local", val)) {
name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
//sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
options.put("local", name.c_str());
}
if (!options.check("remote", val)) {
name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());
//sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
options.put("remote", name.c_str());
}
fprintf(stderr, "%s", options.toString().c_str());
// create a device
PolyDriver dd(options);
if (!dd.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IPositionDirect *posDir;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
// IControlMode *icm;
IControlMode2 *iMode2;
ITorqueControl *itorque;
IOpenLoopControl *iopenloop;
IImpedanceControl *iimp;
IInteractionMode *iInteract;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
// ok &= dd.view(icm);
ok &= dd.view(itorque);
ok &= dd.view(iopenloop);
ok &= dd.view(iimp);
ok &= dd.view(posDir);
ok &= dd.view(iMode2);
ok &= dd.view(iInteract);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 1;
}
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
double *tmp = new double[jnts];
printf("Device active...\n");
while (dd.isValid()) {
std::string s;
s.resize(1024);
printf("-> ");
char c = 0;
int i = 0;
while (c != '\n') {
c = (char)fgetc(stdin);
s[i++] = c;
}
s[i-1] = s[i] = 0;
Bottle p;
Bottle response;
bool ok=false;
bool rec=false;
p.fromString(s.c_str());
printf("Bottle: %s\n", p.toString().c_str());
switch(p.get(0).asVocab()) {
case VOCAB_HELP:
printf("\n\n");
printf("Available commands:\n");
printf("-------------------\n\n");
printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str());
printf(" [set] [%s] <int> <float>\n",
Vocab::decode(VOCAB_OUTPUT).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_OUTPUT).c_str());
printf(" [get] [%s]\n\n",
Vocab::decode(VOCAB_OUTPUTS).c_str());
printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str());
printf(" [set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n",
Vocab::decode(VOCAB_CM_POSITION).c_str(),
Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(),
Vocab::decode(VOCAB_CM_VELOCITY).c_str(),
Vocab::decode(VOCAB_CM_MIXED).c_str(),
Vocab::decode(VOCAB_CM_TORQUE).c_str(),
Vocab::decode(VOCAB_CM_OPENLOOP).c_str(),
Vocab::decode(VOCAB_CM_IDLE).c_str(),
Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(),
Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(),
Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str());
printf(" [get] [%s] <int>\n\n",
Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str());
printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str());
printf(" [get] [%s] <int> to read the measured torque for a single axis\n", Vocab::decode(VOCAB_TRQ).c_str());
printf(" [get] [%s] to read the measured torque for all axes\n", Vocab::decode(VOCAB_TRQS).c_str());
printf(" [set] [%s] <int> <float> to set the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str());
printf(" [set] [%s] <float list> to set the reference torque for all axes\n", Vocab::decode(VOCAB_REFS).c_str());
printf(" [get] [%s] <int> to read the reference torque for a single axis\n", Vocab::decode(VOCAB_REF).c_str());
printf(" [get] [%s] to read the reference torque for all axes\n\n", Vocab::decode(VOCAB_REFS).c_str());
printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str());
printf(" [set] [%s] <int> <float> <float> \n",
Vocab::decode(VOCAB_IMP_PARAM).c_str());
printf(" [set] [%s] <int> <float>\n\n",
Vocab::decode(VOCAB_IMP_OFFSET).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_IMP_PARAM).c_str());
printf(" [get] [%s] <int>\n\n",
Vocab::decode(VOCAB_IMP_OFFSET).c_str());
printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str());
printf(" [set] [%s]|[%s] <int>\n",
Vocab::decode(VOCAB_IM_STIFF).c_str(),
Vocab::decode(VOCAB_IM_COMPLIANT).c_str());
printf(" [get] [%s] <int>\n",
Vocab::decode(VOCAB_INTERACTION_MODE).c_str());
printf(" [get] [%s] \n\n",
Vocab::decode(VOCAB_INTERACTION_MODES).c_str());
printf("Standard Interfaces:\n");
printf("type [get] and one of the following:\n");
printf(" [%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
printf(" [%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf(" [%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str());
printf(" [%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf(" [%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf(" [%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
printf(" [%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
printf(" [%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
printf(" [%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str());
printf(" [%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf(" [%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf(" [%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf(" [%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf(" [%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
printf("\n");
printf("type [set] and one of the following:\n");
printf(" [%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
printf(" [%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());
printf(" [%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf(" [%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf(" [%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
printf(" [%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
printf(" [%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf(" [%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf(" [%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
printf(" [%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
printf(" [%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf(" [%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf(" [%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
printf(" [%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
printf(" [%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
printf(" [%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("\n");
printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n");
printf(" - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n");
printf("\n");
break;
case VOCAB_QUIT:
goto ApplicationCleanQuit;
break;
case VOCAB_ICONTROLMODE:
{
handleControlModeMsg(iMode2, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_IMPEDANCE:
{
handleImpedanceMsg(iimp, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_TORQUE:
{
handleTorqueMsg(itorque, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_INTERFACE_INTERACTION_MODE:
{
handleInteractionModeMsg(iInteract, p, response, &rec, &ok);
printf("%s\n", response.toString().c_str());
break;
}
case VOCAB_GET:
switch(p.get(1).asVocab()) {
case VOCAB_AXES: {
int nj = 0;
enc->getAxes(&nj);
printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
}
break;
case VOCAB_ENCODERS: {
enc->getEncoders(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
pid->getPid(j, &pd);
printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
printf("kp %.2f ", pd.kp);
printf("kd %.2f ", pd.kd);
printf("ki %.2f ", pd.ki);
printf("maxi %.2f ", pd.max_int);
printf("maxo %.2f ", pd.max_output);
printf("off %.2f ", pd.offset);
printf("scale %.2f ", pd.scale);
printf("\n");
}
break;
case VOCAB_PIDS: {
Pid *p = new Pid[jnts];
ok = pid->getPids(p);
Bottle& b = response.addList();
int i;
for (i = 0; i < jnts; i++)
{
Bottle& c = b.addList();
c.addDouble(p[i].kp);
c.addDouble(p[i].kd);
c.addDouble(p[i].ki);
c.addDouble(p[i].max_int);
c.addDouble(p[i].max_output);
c.addDouble(p[i].offset);
c.addDouble(p[i].scale);
}
printf("%s\n", b.toString().c_str());
delete[] p;
}
break;
case VOCAB_LIMITS: {
double min, max;
int j = p.get(2).asInt();
lim->getLimits(j, &min, &max);
printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
printf("limits: (%.2f %.2f)\n", min, max);
}
break;
case VOCAB_ERRS: {
pid->getErrors(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUTS: {
iopenloop->getOutputs(tmp);
printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUT: {
int j = p.get(2).asInt();
double v;
iopenloop->getOutput(j, &v);
printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str());
printf("%.2f ", v);
printf("\n");
}
break;
case VOCAB_REFERENCE: {
double ref_pos;
int j = p.get(2).asInt();
pid->getReference(j,&ref_pos);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str());
printf ("%.2f ", ref_pos);
printf (")\n");
}
break;
case VOCAB_REFERENCES: {
pid->getReferences(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEEDS: {
pos->getRefSpeeds(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEED: {
double ref_speed;
int j = p.get(2).asInt();
pos->getRefSpeed(j,&ref_speed);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf ("%.2f ", ref_speed);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATION: {
double ref_acc;
int j = p.get(2).asInt();
pos->getRefAcceleration(j,&ref_acc);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf ("%.2f ", ref_acc);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATIONS: {
pos->getRefAccelerations(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_AMP_CURRENTS: {
amp->getCurrents(tmp);
printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
}
break;
case VOCAB_SET:
switch(p.get(1).asVocab()) {
case VOCAB_POSITION_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
pos->positionMove(j, ref);
}
break;
case VOCAB_VELOCITY_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
vel->velocityMove(j, ref);
}
break;
case VOCAB_REF_SPEED: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
pos->setRefSpeed(j, ref);
}
break;
case VOCAB_REF_ACCELERATION: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
pos->setRefAcceleration(j, ref);
}
break;
case VOCAB_POSITION_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
pos->positionMove(tmp);
}
break;
case VOCAB_VELOCITY_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
vel->velocityMove(tmp);
}
break;
case VOCAB_REF_SPEEDS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
pos->setRefSpeeds(tmp);
}
break;
case VOCAB_REF_ACCELERATIONS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
pos->setRefAccelerations(tmp);
}
break;
case VOCAB_STOP: {
int j = p.get(2).asInt();
printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j);
pos->stop(j);
}
break;
case VOCAB_STOPS: {
printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
pos->stop();
}
break;
case VOCAB_ENCODER: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
enc->setEncoder(j, ref);
}
break;
case VOCAB_ENCODERS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
enc->setEncoders(tmp);
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
Bottle *l = p.get(3).asList();
if (l==0)
{
printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
}
else
{
int elems=l->size();
if (elems>=3)
{
pd.kp = l->get(0).asDouble();
pd.kd = l->get(1).asDouble();
pd.ki = l->get(2).asDouble();
if (elems>=7)
{
pd.max_int = l->get(3).asDouble();
pd.max_output = l->get(4).asDouble();
pd.offset = l->get(5).asDouble();
pd.scale = l->get(6).asDouble();
}
printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
pid->setPid(j, pd);
}
else
{
printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
}
}
}
break;
case VOCAB_DISABLE: {
int j = p.get(2).asInt();
printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
pid->disablePid(j);
amp->disableAmp(j);
}
break;
case VOCAB_ENABLE: {
int j = p.get(2).asInt();
printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
amp->enableAmp(j);
pid->enablePid(j);
}
break;
case VOCAB_LIMITS: {
int j = p.get(2).asInt();
printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
Bottle *l = p.get(3).asList();
lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
}
break;
case VOCAB_OUTPUT: {
int j=p.get(2).asInt();
double v=p.get(3).asDouble();
iopenloop->setRefOutput(j,v);
printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v);
}
break;
}
break;
} /* switch get(0) */
} /* while () */
ApplicationCleanQuit:
dd.close();
delete[] tmp;
Network::fini();
return 0;
}
int main()
{
Network::init();
srand(time(NULL));
Property options;
options.put("robot", "icub"); // typically from the command line.
options.put("device", "remote_controlboard");
Value& robotname = options.find("robot");
string s("/");
s += robotname.asString();
s += "/right_arm/babbling";
options.put("local", s.c_str());
s.clear();
s += "/";
s += robotname.asString();
s += "/right_arm";
options.put("remote", s.c_str());
PolyDriver dd(options);
if (!dd.isValid()) {
cout << "Device not available. Here are the known devices:\n"<< endl;
cout << Drivers::factory().toString().c_str() << endl;;
Network::fini();
return 0;
}
IPositionControl *pos;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
if (!ok) {
cout << "Device not able to acquire views" << endl;
Network::fini();
dd.close();
return 0;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
// limits
vector<pair<int, int> > vLimitJoints;
for (int i = 0; i < jnts; i++) {
double min, max;
lim->getLimits(i, &min, &max);
vLimitJoints.push_back(pair<int, int> ((int)min, (int)max));
}
// get value of the arm
// we move : 4 5 6 7 8 9 10 11 12 13 14 15
// we don't move: 0 1 2 3
vector<bool> mask;
mask.resize(jnts);
mask[0] = false;
mask[1] = false;
mask[2] = false;
mask[3] = false;
mask[4] = false;
mask[5] = false;
mask[6] = false;
mask[7] = false;
mask[8] = false;
mask[9] = false;
mask[10] = false;
mask[11] = true;
mask[12] = true;
mask[13] = true;
mask[14] = true;
mask[15] = true;
Vector tmp;
tmp.resize(jnts,0.0);
for (int i = 4; i < jnts; i++)
{
pos->positionMove(i, 0.0);
}
bool initDone = false;
while (!initDone)
{
initDone = true;
for (int i = 4; i < jnts; i++)
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
initDone &= jntMotionDone;
}
}
while(true)
{
cout << "Moving to new posture..." << endl;
for (int i = 4; i < jnts; i++)
{
if (mask[i])
{
double newValue = yarp::os::Random::uniform(vLimitJoints[i].first, vLimitJoints[i].second);
tmp[i] = newValue;
pos->positionMove(i, tmp[i]);
}
}
cout << "Waiting for posture to be reached... ("<<tmp.toString(3,3)<<" ) ..." ;
bool motionDone = false;
while (!motionDone)
{
motionDone = true;
for (int i = 4; i < jnts; i++)
{
if (mask[i])
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
motionDone &= jntMotionDone;
}
}
}
Time::delay(15.0);
cout << "ok" << endl;
}
dd.close();
return 0;
}
bool SpringyFingersModel::calibrate(const Property &options)
{
if (configured)
{
IControlMode2 *imod; driver.view(imod);
IControlLimits *ilim; driver.view(ilim);
IEncoders *ienc; driver.view(ienc);
IPositionControl *ipos; driver.view(ipos);
int nAxes; ienc->getAxes(&nAxes);
Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
printMessage(1,"steering the hand to a suitable starting configuration\n");
for (int j=7; j<nAxes; j++)
{
imod->setControlMode(j,VOCAB_CM_POSITION);
ilim->getLimits(j,&qmin[j],&qmax[j]);
ipos->getRefAcceleration(j,&acc[j]);
ipos->getRefSpeed(j,&vel[j]);
ipos->setRefAcceleration(j,1e9);
ipos->setRefSpeed(j,60.0);
ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition
}
printMessage(1,"proceeding with the calibration\n");
Property &opt=const_cast<Property&>(options);
string tag=opt.check("finger",Value("all")).asString().c_str();
if (tag=="thumb")
{
calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
}
else if (tag=="index")
{
calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
}
else if (tag=="middle")
{
calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
}
else if (tag=="ring")
{
calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
}
else if (tag=="little")
{
calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
}
else if ((tag=="all") || (tag=="all_serial"))
{
calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
}
else if (tag=="all_parallel")
{
CalibThread thr[5];
thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]);
thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]);
thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]);
thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]);
thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]);
thr[0].start(); thr[1].start(); thr[2].start();
thr[3].start(); thr[4].start();
bool done=false;
while (!done)
{
done=true;
for (int i=0; i<5; i++)
{
done&=thr[i].isDone();
if (thr[i].isDone() && thr[i].isRunning())
thr[i].stop();
}
Time::delay(0.1);
}
}
else
{
printMessage(1,"unknown finger request %s\n",tag.c_str());
return false;
}
for (int j=7; j<nAxes; j++)
{
ipos->setRefAcceleration(j,acc[j]);
ipos->setRefSpeed(j,vel[j]);
}
return true;
}
else
return false;
}
int main(int argc, char *argv[])
{
// just list the devices if no argument given
if (argc <= 2) {
printf("You can call %s like this:\n", argv[0]);
printf(" %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
printf("For example:\n");
printf(" %s --robot icub --part any --remote /controlboard\n", argv[0]);
printf("Here are devices listed for your system:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
// get command line options
Property options;
options.fromCommand(argc, argv);
if (!options.check("robot") || !options.check("part")) {
printf("Missing either --robot or --part options\n");
return 0;
}
Network yarp;
Time::turboBoost();
char name[1024];
Value& v = options.find("robot");
Value& part = options.find("part");
Value *val;
if (!options.check("device", val)) {
options.put("device", "remote_controlboard");
}
if (!options.check("local", val)) {
sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
options.put("local", name);
}
if (!options.check("remote", val)) {
sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str());
options.put("remote", name);
}
fprintf(stderr, "%s", options.toString().c_str());
// create a device
PolyDriver dd(options);
if (!dd.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 1;
}
IPositionControl *pos;
IVelocityControl *vel;
IEncoders *enc;
IPidControl *pid;
IAmplifierControl *amp;
IControlLimits *lim;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(enc);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(lim);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 1;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
double *tmp = new double[jnts];
assert (tmp != NULL);
printf("Device active...\n");
while (dd.isValid()) {
char s[1024];
printf("-> ");
char c = 0;
int i = 0;
while (c != '\n') {
c = (char)fgetc(stdin);
s[i++] = c;
}
s[i-1] = s[i] = 0;
Bottle p;
p.fromString(s);
printf("Bottle: %s\n", p.toString().c_str());
switch(p.get(0).asVocab()) {
case VOCAB_HELP:
printf("\n\n");
printf("Available commands:\n\n");
printf("type [get] and one of the following:\n");
printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
printf("\n");
printf("type [set] and one of the following:\n");
printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());
printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
printf("\n");
break;
case VOCAB_QUIT:
goto ApplicationCleanQuit;
break;
case VOCAB_GET:
switch(p.get(1).asVocab()) {
case VOCAB_AXES: {
int nj = 0;
enc->getAxes(&nj);
printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
}
break;
case VOCAB_ENCODERS: {
enc->getEncoders(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
pid->getPid(j, &pd);
printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
printf("kp %.2f ", pd.kp);
printf("kd %.2f ", pd.kd);
printf("ki %.2f ", pd.ki);
printf("maxi %.2f ", pd.max_int);
printf("maxo %.2f ", pd.max_output);
printf("off %.2f ", pd.offset);
printf("scale %.2f ", pd.scale);
printf("\n");
}
break;
case VOCAB_LIMITS: {
double min, max;
int j = p.get(2).asInt();
lim->getLimits(j, &min, &max);
printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
printf("limits: (%.2f %.2f)\n", min, max);
}
break;
case VOCAB_ERRS: {
pid->getErrorLimits(tmp);
printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_OUTPUTS: {
pid->getErrors(tmp);
printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REFERENCES: {
pid->getReferences(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_SPEEDS: {
pos->getRefSpeeds(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_REF_ACCELERATIONS: {
pos->getRefAccelerations(tmp);
printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
case VOCAB_AMP_CURRENTS: {
amp->getCurrents(tmp);
printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
for(i = 0; i < jnts; i++)
printf ("%.2f ", tmp[i]);
printf (")\n");
}
break;
}
break;
case VOCAB_SET:
switch(p.get(1).asVocab()) {
case VOCAB_POSITION_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
pos->positionMove(j, ref);
}
break;
case VOCAB_VELOCITY_MOVE: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
vel->velocityMove(j, ref);
}
break;
case VOCAB_REF_SPEED: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
pos->setRefSpeed(j, ref);
}
break;
case VOCAB_REF_ACCELERATION: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
pos->setRefAcceleration(j, ref);
}
break;
case VOCAB_POSITION_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
pos->positionMove(tmp);
}
break;
case VOCAB_VELOCITY_MOVES: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
vel->velocityMove(tmp);
}
break;
case VOCAB_REF_SPEEDS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
pos->setRefSpeeds(tmp);
}
break;
case VOCAB_REF_ACCELERATIONS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
pos->setRefAccelerations(tmp);
}
break;
case VOCAB_STOP: {
int j = p.get(2).asInt();
printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str());
pos->stop(j);
}
break;
case VOCAB_STOPS: {
printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str());
pos->stop();
}
break;
case VOCAB_ENCODER: {
int j = p.get(2).asInt();
double ref = p.get(3).asDouble();
printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
enc->setEncoder(j, ref);
}
break;
case VOCAB_ENCODERS: {
Bottle *l = p.get(2).asList();
for (i = 0; i < jnts; i++) {
tmp[i] = l->get(i).asDouble();
}
printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
enc->setEncoders(tmp);
}
break;
case VOCAB_PID: {
Pid pd;
int j = p.get(2).asInt();
Bottle *l = p.get(3).asList();
pd.kp = l->get(0).asDouble();
pd.kd = l->get(1).asDouble();
pd.ki = l->get(2).asDouble();
pd.max_int = l->get(3).asDouble();
pd.max_output = l->get(4).asDouble();
pd.offset = l->get(5).asDouble();
pd.scale = l->get(6).asDouble();
printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
pid->setPid(j, pd);
}
break;
case VOCAB_DISABLE: {
int j = p.get(2).asInt();
printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
pid->disablePid(j);
amp->disableAmp(j);
}
break;
case VOCAB_ENABLE: {
int j = p.get(2).asInt();
printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
amp->enableAmp(j);
pid->enablePid(j);
}
break;
case VOCAB_LIMITS: {
int j = p.get(2).asInt();
printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
Bottle *l = p.get(3).asList();
lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
}
break;
}
break;
} /* switch get(0) */
} /* while () */
ApplicationCleanQuit:
dd.close();
delete[] tmp;
return 0;
}
int main(int argc, char *argv[])
{
Network yarp;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
fprintf(stderr, "--part name (right_arm or left_arm)\n");
fprintf(stderr, "--durationInS duration (in seconds)\n");
fprintf(stderr, "--sampleInMS sample time (in milliseconds)\n");
fprintf(stderr, "--refTimeInMS Reference time of minimum jerk trajectory generator (in milliseconds)\n");
return 1;
}
if( !params.check("sampleInMS") ||
!params.check("durationInS") ||
!params.check("refTimeInMS") )
{
fprintf(stderr, "Necessary params not passed\n");
return EXIT_FAILURE;
}
std::string robotName=params.find("robot").asString().c_str();
std::string partName=params.find("part").asString().c_str();
double durationInS = params.find("durationInS").asDouble();
double samplesInMS = params.find("sampleInMS").asDouble();
double refTimeInMS = params.find("refTimeInMS").asDouble();
std::cout << "robotName : " << robotName << std::endl;
std::cout << "partName : " << partName << std::endl;
std::cout << "durationInS : " << durationInS << std::endl;
std::cout << "samplesInMS : " << samplesInMS << std::endl;
std::cout << "refTimeInMS : " << refTimeInMS << std::endl;
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/"+partName;
std::string localPorts="/randomShoulderMovements/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionDirect *pos;
IEncoders *encs;
IControlLimits * lims;
IControlMode2 * ictrl;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
ok = ok && robotDevice.view(lims);
ok = ok && robotDevice.view(ictrl);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encodersInDeg(nj);
Vector desPosInDeg(3);
Vector commandInDeg(3);
Vector minShoulderInDeg(3);
Vector maxShoulderInDeg(3);
Vector lowerBoundConstraint;
Vector upperBoundConstraint;
Matrix constraintMatrix;
for(int i = 0; i < 3; i++ )
{
lims->getLimits(i,&(minShoulderInDeg[i]),&(maxShoulderInDeg[i]));
}
bool encodersRead = encs->getEncoders(encodersInDeg.data());
while( !encodersRead )
{
encodersRead = encs->getEncoders(encodersInDeg.data());
}
getShoulderConstraint(lowerBoundConstraint,upperBoundConstraint,constraintMatrix);
// Create the minimum jerk filter
iCub::ctrl::minJerkTrajGen filter(3,samplesInMS,refTimeInMS);
filter.init(encodersInDeg.subVector(0,2));
std::cout << "Filter initial value " << encodersInDeg.subVector(0,2).toString() << std::endl;
std::vector<int> indexToControl;
indexToControl.push_back(0);
indexToControl.push_back(1);
indexToControl.push_back(2);
// Set control modes
ictrl->setControlMode(0,VOCAB_CM_POSITION_DIRECT);
ictrl->setControlMode(1,VOCAB_CM_POSITION_DIRECT);
ictrl->setControlMode(2,VOCAB_CM_POSITION_DIRECT);
double samplesInS = samplesInMS/1000.0;
int samples = (int)(durationInS/samplesInS);
int samplesForNewPosition = (int)(refTimeInMS/samplesInMS);
for(int i=0; i < samples; i++ )
{
if( i % samplesForNewPosition == 0 )
{
// Generate a new desired shoulder position
generateRandomShoulderPosition(desPosInDeg,minShoulderInDeg,maxShoulderInDeg,
lowerBoundConstraint,upperBoundConstraint,constraintMatrix);
std::cout << "New position generated" << std::endl;
std::cout << "Position generated " << desPosInDeg.toString() << std::endl;
std::cout << "Remaining time " << (samples-i)*(samplesInS) << std::endl;
}
filter.computeNextValues(desPosInDeg);
commandInDeg = filter.getPos();
std::cout << "Position set " << commandInDeg.toString() << std::endl;
pos->setPositions(3,indexToControl.data(),commandInDeg.data());
yarp::os::Time::delay(samplesInS);
}
robotDevice.close();
return 0;
}
bool configure(ResourceFinder &rf)
{
Time::turboBoost();
string name=rf.check("name",Value("yarpMinJerk")).asString().c_str();
string robot=rf.check("robot",Value("icubSim")).asString().c_str();
string part=rf.check("part",Value("left_arm")).asString().c_str();
joint=rf.check("joint",Value(3)).asInt();
compliant=rf.check("compliant");
Property option;
option.put("device","remote_controlboard");
option.put("remote",("/"+robot+"/"+part).c_str());
option.put("local",("/"+name+"/"+part).c_str());
if (!drv.open(option))
return false;
drv.view(imod);
drv.view(iint);
drv.view(ienc);
drv.view(ivel);
imod->setControlMode(joint,VOCAB_CM_VELOCITY);
if (compliant)
iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT);
IControlLimits *ilim; drv.view(ilim);
double min_joint,max_joint;
ilim->getLimits(joint,&min_joint,&max_joint);
double enc;
while (!ienc->getEncoder(joint,&enc))
Time::delay(0.1);
Controller_P.Plant_IC=enc;
Controller_P.Plant_Max=max_joint;
Controller_P.Plant_Min=min_joint;
Controller_P.AutoCompensator_ThresHystMax=0.5;
Controller_P.AutoCompensator_ThresHystMin=0.1;
yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint);
// Pack model data into RTM
Controller_M->ModelData.defaultParam = &Controller_P;
Controller_M->ModelData.blockIO = &Controller_B;
Controller_M->ModelData.dwork = &Controller_DW;
// Initialize model
Controller_initialize(Controller_M, &Controller_U_reference,
&Controller_U_compensator_state,
&Controller_U_plant_output,
&Controller_Y_controller_output,
&Controller_Y_controller_reference,
&Controller_Y_plant_reference,
&Controller_Y_error_statistics,
&Controller_Y_enable_compensation);
Controller_U_reference=enc;
Controller_U_compensator_state=CompensatorState::Off;
Controller_U_plant_output=enc;
dataIn.open(("/"+name+"/data:i").c_str());
dataOut.open(("/"+name+"/data:o").c_str());
rpc.open(("/"+name+"/rpc").c_str());
attach(rpc);
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
string robot=rf.find("robot").asString().c_str();
string hand=rf.find("hand").asString().c_str();
string modelType=rf.find("modelType").asString().c_str();
fingerName=rf.find("finger").asString().c_str();
if (fingerName=="thumb")
joint=10;
else if (fingerName=="index")
joint=12;
else if (fingerName=="middle")
joint=14;
else if (fingerName=="ring")
joint=15;
else if (fingerName=="little")
joint=15;
else
{
fprintf(stdout,"unknown finger!\n");
return false;
}
Property driverOpt("(device remote_controlboard)");
driverOpt.put("remote",("/"+robot+"/"+hand+"_arm").c_str());
driverOpt.put("local",("/"+name).c_str());
if (!driver.open(driverOpt))
return false;
driver.view(ipos);
driver.view(ienc);
IControlLimits *ilim;
driver.view(ilim);
ilim->getLimits(joint,&min,&max);
double margin=0.1*(max-min);
min=min+margin;
max=max-margin;
val=&min;
Property genOpt;
genOpt.put("name",(name+"/"+modelType).c_str());
genOpt.put("robot",robot.c_str());
genOpt.put("type",hand.c_str());
genOpt.put("verbose",1);
string general(genOpt.toString().c_str());
string thumb( "(thumb (name thumb))");
string index( "(index (name index))");
string middle("(middle (name middle))");
string ring( "(ring (name ring))");
string little("(little (name little))");
Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str());
fprintf(stdout,"configuring options: %s\n",options.toString().c_str());
if (modelType=="springy")
model=new SpringyFingersModel;
else if (modelType=="tactile")
model=new TactileFingersModel;
else
{
fprintf(stdout,"unknown model type!\n");
return false;
}
if (model->fromProperty(options))
return true;
else
{
delete model;
return false;
}
}