bool goHome()
{
printMessage(0,"Going home...\n");
yarp::sig::Vector poss(7,0.0);
yarp::sig::Vector vels(7,0.0);
printMessage(1,"Configuring arm...\n");
poss[0]=-30.0; vels[0]=10.0; poss[1]=30.0; vels[1]=10.0;
poss[2]= 00.0; vels[2]=10.0; poss[3]=45.0; vels[3]=10.0;
poss[4]= 00.0; vels[4]=10.0; poss[5]=00.0; vels[5]=10.0;
poss[6]= 00.0; vels[6]=10.0;
for (int i=0; i<7; i++)
{
iposs->setRefSpeed(i,vels[i]);
iposs->positionMove(i,poss[i]);
}
printMessage(1,"Configuring hand...\n");
poss.resize(9,0.0);
vels.resize(9,0.0);
poss[0]=40.0; vels[0]=60.0; poss[1]=10.0; vels[1]=60.0;
poss[2]=60.0; vels[2]=60.0; poss[3]=70.0; vels[3]=60.0;
poss[4]=00.0; vels[4]=60.0; poss[5]=00.0; vels[5]=60.0;
poss[6]=70.0; vels[6]=60.0; poss[7]=100.0; vels[7]=60.0;
poss[8]=240.0; vels[8]=120.0;
for (int i=7; i<nEncs; i++)
{
iposs->setRefAcceleration(i,1e9);
iposs->setRefSpeed(i,vels[i-7]);
iposs->positionMove(i,poss[i-7]);
}
return true;
}
void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
IPositionControl *ipos = currentPart->pos;
IPidControl *ipid = currentPart->pid;
IPositionDirect *iDir = currentPart->iDir;
GtkWidget **sliderVel = currentPart->sliderVelArray;
double val = gtk_range_get_value(range);
double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]);
IControlMode *iCtrl = currentPart->ctrlmode2;
int mode;
iCtrl->getControlMode(*joint, &mode);
if (!POS_UPDATE[*joint])
{
if( ( mode == VOCAB_CM_POSITION) || (mode == VOCAB_CM_MIXED) )
{
ipos->setRefSpeed(*joint, valVel);
ipos->positionMove(*joint, val);
}
else if( ( mode == VOCAB_CM_IMPEDANCE_POS))
{
fprintf(stderr, " using old 'impedance_position' mode, this control mode is deprecated!");
ipos->setRefSpeed(*joint, valVel);
ipos->positionMove(*joint, val);
}
else if ( mode == VOCAB_CM_POSITION_DIRECT)
{
if (position_direct_enabled)
{
iDir->setPosition(*joint, val);
}
else
{
fprintf(stderr, "You cannot send direct position commands without using --direct option!");
}
}
else
{
fprintf(stderr, "Joint not in position nor positionDirect so cannot send references");
}
}
return;
}
virtual bool threadInit()
{
// open a client interface to connect to the gaze server
// we suppose that:
// 1 - the iCub simulator is running;
// 2 - the gaze server iKinGazeCtrl is running and
// launched with the following options: "--from configSim.ini"
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local","/gaze_client");
if (!clientGaze.open(optGaze))
return false;
// open the view
clientGaze.view(igaze);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the tracking mode, the neck limits and so on.
igaze->storeContext(&startup_context_id);
// set trajectory time:
igaze->setNeckTrajTime(0.8);
igaze->setEyesTrajTime(0.4);
// put the gaze in tracking mode, so that
// when the torso moves, the gaze controller
// will compensate for it
igaze->setTrackingMode(true);
// print out some info about the controller
Bottle info;
igaze->getInfo(info);
fprintf(stdout,"info = %s\n",info.toString().c_str());
Property optTorso("(device remote_controlboard)");
optTorso.put("remote","/icubSim/torso");
optTorso.put("local","/torso_client");
if (!clientTorso.open(optTorso))
return false;
// open the view
clientTorso.view(ienc);
clientTorso.view(ipos);
ipos->setRefSpeed(0,10.0);
fp.resize(3);
state=STATE_TRACK;
t=t0=t1=t2=t3=t4=Time::now();
return true;
}
void partMover::home_click(GtkButton *button, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
//fprintf(stderr, "Home group was not empty \n");
bool ok = true;
Bottle xtmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double positionZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", positionZero);
xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
//fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double velocityZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", velocityZero);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
else
{
ipos->setRefSpeed(*joint, velocityZero);
ipos->positionMove(*joint, positionZero);
}
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
void _send(const ActionItem *x)
{
if (!connected)
{
cerr<<"Error: not connected to control board skipping"<<endl;
return;
}
int size=x->getCmd().size();
int offset=x->getOffset();
double time=x->getTime();
int nJoints=0;
enc->getAxes(&nJoints);
if ((offset+size)>nJoints)
{
cerr<<"Error: detected possible overflow, skipping"<<endl;
cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
return;
}
Vector disp(size);
if (time==0)
{
return;
}
for (size_t i=0; i<disp.length(); i++)
{
double q;
if (!enc->getEncoder(offset+i,&q))
{
cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
return;
}
disp[i]=x->getCmd()[i]-q;
if (disp[i]<0.0)
disp[i]=-disp[i];
}
for (size_t i=0; i<disp.length(); i++)
{
pos->setRefSpeed(offset+i,disp[i]/time);
pos->positionMove(offset+i,x->getCmd()[i]);
}
cout << "Script port: " << const_cast<Vector &>(x->getCmd()).toString() << endl;
}
void partMover::home_all(GtkButton *button, partMover* currentPart)
{
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
bool ok = true;
Bottle xtmp, ytmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
ytmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
ok = ok && (ytmp.size() == NUMBER_OF_JOINTS+1);
if(ok)
{
for (int joint = 0; joint < NUMBER_OF_JOINTS; joint++)
{
double positionZero = xtmp.get(joint+1).asDouble();
//fprintf(stderr, "%f ", positionZero);
double velocityZero = ytmp.get(joint+1).asDouble();
//fprintf(stderr, "%f ", velocityZero);
ipos->setRefSpeed(joint, velocityZero);
ipos->positionMove(joint, positionZero);
}
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
void partMover::sliderVel_release(GtkRange *range, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
GtkWidget **sliderAry = currentPart->sliderArray;
double val = gtk_range_get_value(range);
double posit = gtk_range_get_value((GtkRange *) sliderAry[*joint]);
ipos->setRefSpeed(*joint, val);
ipos->positionMove(*joint, posit);
return;
}
bool updateModule()
{
if (calibrate)
{
Property options;
options.put("finger",fingerName.c_str());
model->calibrate(options);
calibrate=false;
ipos->setRefAcceleration(joint,1e9);
if ((fingerName=="ring")||(fingerName=="little"))
ipos->setRefSpeed(joint,60.0);
else
ipos->setRefSpeed(joint,30.0);
ipos->positionMove(joint,*val);
}
else
{
if (Node *finger=model->getNode(fingerName))
{
Value data; finger->getSensorsData(data);
Value out; finger->getOutput(out);
fprintf(stdout,"%s sensors data = %s; output = %s\n",
finger->getName().c_str(),data.toString().c_str(),out.toString().c_str());
}
double fb; ienc->getEncoder(joint,&fb);
if (fabs(*val-fb)<5.0)
{
val==&min?val=&max:val=&min;
ipos->positionMove(joint,*val);
}
}
return true;
}
void ReachManager::InitPositionControl(string partName)
{
Property options;
options.put("device", "remote_controlboard");
options.put("local", ("/reach_manager/position_control/" + partName).c_str()); //local port names
string remotePortName = "/" + (string)parameters["robot"]->asString() + "/" + partName + "_arm";
options.put("remote", remotePortName.c_str()); //where we connect to
// create a device
polydrivers[partName] = new PolyDriver(options);
if (!polydrivers[partName]->isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return;
}
IPositionControl *pos;
IEncoders *encs;
if (!(polydrivers[partName]->view(pos) && polydrivers[partName]->view(encs))) {
printf("Problems acquiring interfaces\n");
return;
}
pos->getAxes(&nbJoints[partName]);
Vector encoders;
Vector tmp;
encoders.resize(nbJoints[partName]);
tmp.resize(nbJoints[partName]);
for (int i = 0; i < nbJoints[partName]; i++)
{
tmp[i] = 5.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nbJoints[partName]; i++)
{
tmp[i] = 3.0;
pos->setRefSpeed(i, tmp[i]);
}
}
void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
IPositionControl *ipos = currentPart->pos;
IPidControl *ipid = currentPart->pid;
GtkWidget **sliderVel = currentPart->sliderVelArray;
double val = gtk_range_get_value(range);
double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]);
if (!POS_UPDATE[*joint])
{
ipos->setRefSpeed(*joint, valVel);
ipos->positionMove(*joint, val);
//ipid->setReference(*joint, val);
}
return;
}
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;
}
virtual bool threadInit() {
/* Read in parameters from resource finder */
loadParams();
/* Start up gaze control client interface */
Property option("(device gazecontrollerclient)");
option.put("remote","/iKinGazeCtrl");
option.put("local","/client/gaze");
clientGazeCtrl.open(option);
gaze=NULL;
if (clientGazeCtrl.isValid()) {
clientGazeCtrl.view(gaze);
} else {
printf("could not initialize gaze control interface, failing...\n");
return false;
}
//set gaze control interface params
gaze->setNeckTrajTime(neckTT);
gaze->setEyesTrajTime(eyeTT);
gaze->bindNeckPitch(-30,30);
gaze->bindNeckYaw(-25,25);
gaze->bindNeckRoll(-10,10);
/* Start up arm cartesian control client interface */
Property optiona("(device cartesiancontrollerclient)");
if (armInUse) {
string tmpcrname = "/" + robotname + "/cartesianController/right_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/right_arm");
}
else {
string tmpcrname = "/" + robotname + "/cartesianController/left_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/left_arm");
}
clientArmCart.open(optiona);
carm = NULL;
if (clientArmCart.isValid())
{
clientArmCart.view(carm);
} else {
printf("could not initialize arm cartesian control interface, failing...\n");
return false;
}
// set trajectory time
carm->setTrajTime(trajTime); //slow for safety
// get the torso dofs
Vector newDof, curDof;
carm->getDOF(curDof);
newDof=curDof;
//enable torso pitch and yaw, also wrist movements
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
newDof[7]=1;
newDof[8]=1;
newDof[9]=1;
// impose some restriction on the torso pitch
double tpmin, tpmax;
carm->getLimits(0,&tpmin,&tpmax);
carm->setLimits(0,tpmin,maxPitch);
// send the request for dofs reconfiguration
carm->setDOF(newDof,curDof);
string lname, rname;
Property doption;
doption.put("device", "remote_controlboard");
lname = "/"+name+"/torso"; rname = "/"+robotname+"/torso";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotTorso.open(doption);
if (!robotTorso.isValid()) {
printf("could not initialize torso control interface, failing...\n");
return false;
}
robotTorso.view(tPos); robotTorso.view(tEnc);
lname = "/"+name+"/head"; rname = "/"+robotname+"/head";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotHead.open(doption);
if (!robotHead.isValid()) {
printf("could not initialize head control interface, failing...\n");
return false;
}
robotHead.view(hPos); robotHead.view(hEnc);
lname = "/"+name+"/"+armname+"_arm"; rname = "/"+robotname+"/"+armname+"_arm";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotArm.open(doption);
if (!robotArm.isValid()) {
printf("could not initialize arm control interface, failing...\n");
return false;
}
robotArm.view(aPos); robotArm.view(aEnc);
for (int i=0; i<7; i++) {
aPos->setRefSpeed(i, 10.0);
}
/* Create and open ports */
cportL=new ClickPort(bfL);
string cplName="/"+name+"/clk:l";
cportL->open(cplName.c_str());
cportL->useCallback();
cportR=new ClickPort(bfR);
string cprName="/"+name+"/clk:r";
cportR->open(cprName.c_str());
cportR->useCallback();
susPort=new Port;
string suspName="/"+name+"/sus:o";
susPort->open(suspName.c_str());
return true;
}
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;
}
bool tune(const int i)
{
PidData &pid=pids[i];
Property pGeneral;
pGeneral.put("joint",i);
string sGeneral="(general ";
sGeneral+=pGeneral.toString().c_str();
sGeneral+=')';
Bottle bGeneral,bPlantEstimation,bStictionEstimation;
bGeneral.fromString(sGeneral.c_str());
bPlantEstimation.fromString("(plant_estimation (Ts 0.01) (Q 1.0) (R 1.0) (P0 100000.0) (tau 1.0) (K 1.0) (max_pwm 800.0))");
bStictionEstimation.fromString("(stiction_estimation (Ts 0.01) (T 2.0) (vel_thres 5.0) (e_thres 1.0) (gamma (10.0 10.0)) (stiction (0.0 0.0)))");
Bottle bConf=bGeneral;
bConf.append(bPlantEstimation);
bConf.append(bStictionEstimation);
Property pOptions(bConf.toString().c_str());
OnlineCompensatorDesign designer;
if (!designer.configure(*driver,pOptions))
{
yError("designer configuration failed!");
return false;
}
idlingCoupledJoints(i,true);
Property pPlantEstimation;
pPlantEstimation.put("max_time",20.0);
pPlantEstimation.put("switch_timeout",2.0);
designer.startPlantEstimation(pPlantEstimation);
yInfo("Estimating plant for joint %d: max duration = %g seconds",
i,pPlantEstimation.find("max_time").asDouble());
double t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
Property pResults;
designer.getResults(pResults);
double tau=pResults.find("tau_mean").asDouble();
double K=pResults.find("K_mean").asDouble();
yInfo("plant = %g/s * 1/(1+s*%g)",K,tau);
Property pControllerRequirements,pController;
pControllerRequirements.put("tau",tau);
pControllerRequirements.put("K",K);
pControllerRequirements.put("f_c",0.75);
if (i!=15)
{
pControllerRequirements.put("T_dr",1.0);
pControllerRequirements.put("type","PI");
}
else
pControllerRequirements.put("type","P");
designer.tuneController(pControllerRequirements,pController);
yInfo("tuning results: %s",pController.toString().c_str());
double Kp=pController.find("Kp").asDouble();
double Ki=pController.find("Ki").asDouble();
pid.scale=4.0;
int scale=(int)pid.scale; int shift=1<<scale;
double fwKp=floor(Kp*pid.encs_ratio*shift);
double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0);
pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp;
pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi;
pid.Kd=0.0;
yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale);
Property pStictionEstimation;
pStictionEstimation.put("max_time",60.0);
pStictionEstimation.put("Kp",Kp);
pStictionEstimation.put("Ki",0.0);
pStictionEstimation.put("Kd",0.0);
designer.startStictionEstimation(pStictionEstimation);
yInfo("Estimating stiction for joint %d: max duration = %g seconds",
i,pStictionEstimation.find("max_time").asDouble());
t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
designer.getResults(pResults);
pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble());
pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble());
yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down);
IControlMode2 *imod;
IPositionControl *ipos;
IEncoders *ienc;
driver->view(imod);
driver->view(ipos);
driver->view(ienc);
imod->setControlMode(i,VOCAB_CM_POSITION);
ipos->setRefSpeed(i,50.0);
ipos->positionMove(i,0.0);
yInfo("Driving the joint back to rest... ");
t0=Time::now();
while (Time::now()-t0<5.0)
{
double enc;
ienc->getEncoder(i,&enc);
if (fabs(enc)<1.0)
break;
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
Time::delay(0.2);
}
yInfo("done!");
idlingCoupledJoints(i,false);
return true;
}
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");
return -1;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/left_arm";
std::string localPorts="/test/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;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 4.0;
pos->setRefSpeed(i, tmp[i]);
}
//pos->setRefSpeeds(tmp.data()))
//fisrst zero all joints
//
command=0;
//now set the shoulder to some value
command[0]=-26.0;
command[1]=20.0;
command[2]=0.0;
command[3]=49.0;
command[4]=0.0;
pos->positionMove(command.data());
bool done=false;
while(!done) {
Time::delay(0.5);
pos->checkMotionDone(&done);
}
printf("\niCub @ HOME. Press any key...");
mygetch();
int times=0;
while(true)
{
times++;
if (times%2)
{
printf("\n\nSet pos1: ");
//command[0]=-50;
command[1]=64.0;
//command[2]=-10;
//command[3]=50;
//command[4]=0;
}
else
{
printf("\n\nSet pos2: ");
//command[0]=-20;
command[1]=20.0;
//command[2]=-10;
//command[3]=30;
//command[4]=0;
}
pos->positionMove(command.data());
printf("waiting");
bool done3=false;
while(!done3) {
Time::delay(1.0);
pos->checkMotionDone(&done3);
printf(".");
}
printf("ok!\n");
mygetch();
}
robotDevice.close();
return 0;
}
int main(int argc, char *argv[])
{
Network yarp;
int maxSpeed;
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");
return -1;
}
if (!params.check("repetitions"))
{
fprintf(stderr, "Please specify number of repetitions\n");
fprintf(stderr, "--repetitions num (e.g. 10)\n");
return -1;
}
if (!params.check("speed"))
{
fprintf(stderr, "Speed not specified using default\n");
fprintf(stderr, "--speed num (e.g. 2)\n");
maxSpeed = 10.0;
}
else
{
maxSpeed = params.find("speed").asInt();
}
// sanity check on argument value
if(maxSpeed <0 || maxSpeed>50)
{
maxSpeed = 10;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/head";
int numTimes = params.find("repetitions").asInt();
std::string localPorts="/headMovement_koroibot/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;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 10.0;
pos->setRefSpeed(i, tmp[i]);
}
//first read all encoders
printf("waiting for encoders");
while(!encs->getEncoders(encoders.data()))
{
Time::delay(0.1);
printf(".");
}
printf("\n;");
int ctr =0;
bool done = false;
while(ctr<numTimes)
{
printf("Starting headMovement\n");
command=encoders;
if(ctr%2 == 0)
command[2]=HEAD_YAW_MAX;
else
command[2]=HEAD_YAW_MIN;
done = false;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
ctr++;
}
command[2] = 0;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
robotDevice.close();
return 0;
}
virtual bool threadInit(){
if(!handleParams()){
return false;
}
armPlan = new Port;
armPred = new Port;
armLocJ = new BufferedPort<Vector>;
headLoc = new BufferedPort<Vector>;
armPlan->open("/babbleTrack/plan:o");
armPred->open("/babbleTrack/pred:i");
armLocJ->open("/babbleTrack/arm:o");
headLoc->open("/babbleTrack/head:o");
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
igaze = NULL;
Property options;
options.put("device","gazecontrollerclient");
options.put("remote","/iKinGazeCtrl");
options.put("local","/client/gaze");
clientGazeCtrl = new PolyDriver;
clientGazeCtrl->open(options);
options.clear();
string localPorts = "/babbleTrack/cmd";
string remotePorts = "/" + robotName + "/" + arm + "_arm";
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
robotDevice = new PolyDriver;
robotDevice->open(options);
if(clientGazeCtrl->isValid()){
clientGazeCtrl->view(igaze);
}
else{
return false;
}
if (!robotDevice->isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return false;
}
bool ok;
ok = robotDevice->view(pos);
ok = ok && robotDevice->view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return false;
}
pos->getAxes(&nj);
command = new Vector;
tmp = new Vector;
command->resize(nj);
tmp->resize(nj);
for (int i = 0; i < nj; i++) {
(*tmp)[i] = 25.0;
}
pos->setRefAccelerations(tmp->data());
for (int i = 0; i < nj; i++) {
(*tmp)[i] = 20.0;
pos->setRefSpeed(i, (*tmp)[i]);
}
*command = 0;
//set the arm joints to "middle" values
(*command)[0] = -45;
(*command)[1] = 45;
(*command)[2] = 0;
(*command)[3] = 45;
//flex hand
(*command)[4] = 60;
(*command)[7] = 20;
(*command)[10] = 15;
(*command)[11] = 15;
(*command)[12] = 15;
(*command)[13] = 15;
(*command)[14] = 15;
(*command)[15] = 15;
pos->positionMove(command->data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
bool fwCvOn = 0;
fwCvOn = Network::connect("/babbleTrack/plan:o","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/babbleTrack/pred:i");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
return false;
}
return true;
}
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[]){
Network yarp;
Port armPlan;
Port armPred;
BufferedPort<Vector> objAngles;
BufferedPort<Vector> armOut;
armPlan.open("/learnedReach/plan");
armPred.open("/learnedReach/pred");
objAngles.open("/learnedReach/loc:i");
armOut.open("/learnedReach/arm:o");
bool fwCvOn = 0;
fwCvOn = Network::connect("/learnedReach/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/learnedReach/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
return -1;
}
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name\n");
fprintf(stderr, "e.g. --robot icub\n");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("arm")){
remotePorts += params.find("arm").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/learnedReach/cmd";
if(!params.check("map")){
fprintf(stderr, "Please specify learned visuomotor map file\n");
fprintf(stderr, "e.g. --map map.dat\n");
return -1;
}
string fName = params.find("map").asString().c_str();
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
PolyDriver robotDevice(options);
if (!robotDevice.isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IEncoders *enc;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return 0;
}
int nj = 0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector commandCart;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
commandCart.resize(3);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 20.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
//not really yaw and pitch
int azMin = -80; int azMax = 0;
int elMin = -60; int elMax = 0;
int verMin = 0; int verMax = 20;
int Y; int P; int V;
int mmapSize; int usedJoints;
//read in first lines to get map dimensions
string line;
string buf;
ifstream mapFile(fName.c_str());
if(mapFile.is_open()){
getline(mapFile,line);
stringstream ss(line);
ss >> buf;
Y = atoi(buf.c_str());
ss >> buf;
P = atoi(buf.c_str());
ss >> buf;
V = atoi(buf.c_str());
ss.clear();
getline(mapFile,line);
ss.str(line);
ss >> buf;
mmapSize = atoi(buf.c_str());
ss >> buf;
usedJoints = atoi(buf.c_str());
}
void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
const double min, const double max)
{
printMessage(1,"calibrating finger %s ...\n",finger.getName().c_str());
double margin=0.1*(max-min);
double _min=min+margin;
double _max=max-margin;
double tol_min=5.0;
double tol_max=5.0;
double *val=&_min;
double *tol=&tol_min;
double timeout=2.0*(_max-_min)/finger.getCalibVel();
mutex.lock();
IControlMode2 *imod; driver.view(imod);
IEncoders *ienc; driver.view(ienc);
IPositionControl *ipos; driver.view(ipos);
mutex.unlock();
// workaround
if ((finger.getName()=="ring") || (finger.getName()=="little"))
{
_min=30.0;
_max=180.0;
tol_min=20.0;
tol_max=50.0;
}
Property reset("(reset)");
Property feed("(feed)");
Property train("(train)");
finger.calibrate(reset);
mutex.lock();
imod->setControlMode(joint,VOCAB_CM_POSITION);
ipos->setRefSpeed(joint,finger.getCalibVel());
mutex.unlock();
for (int i=0; i<5; i++)
{
mutex.lock();
ipos->positionMove(joint,*val);
mutex.unlock();
bool done=false;
double fbOld=1e9;
double t0=Time::now();
while (!done)
{
Time::delay(0.01);
mutex.lock();
double fb; ienc->getEncoder(joint,&fb);
mutex.unlock();
if (fabs(fb-fbOld)>0.5)
{
printMessage(2,"feeding finger %s\n",finger.getName().c_str());
finger.calibrate(feed);
}
done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
fbOld=fb;
}
if (val==&_min)
{
val=&_max;
tol=&tol_max;
}
else
{
val=&_min;
tol=&tol_min;
}
}
printMessage(1,"training finger %s ...\n",finger.getName().c_str());
finger.calibrate(train);
printMessage(1,"finger %s trained!\n",finger.getName().c_str());
}
int main(int argc, char *argv[])
{
Network yarp;
// IMPORTANT
// TO BE ABLE TO FIND KASPAR Devices
#ifdef USE_KASPAR_MOD
yarp::dev::DriverCollection dev;
#endif
printf("SDLJSLKdjfklSJ\n");
Property config;
//config.fromCommand(argc, argv);
//config.put("device", "KasparServos");
config.fromConfigFile("D:/roboskin/coding/roboskin/src/modules/kasparServos/KasparRoboSkin.cfg");
//Property options;
//options.put("device", "fsrTouchSensor");
//options.put("device", "remote_controlboard");
PolyDriver dd;
dd.open(config);
/*CAUTION::::: device here should be remote_controlboard, rather than dynamixelAX12FtdiDriver*/
//http://wikiri.upc.es/index.php/YARP_devices
//http://eris.liralab.it/wiki/YARP_Motor_Control
//http://eris.liralab.it/yarpdoc/d1/d88/classyarp_1_1dev_1_1RemoteControlBoard.html
//PolyDriver dd("(device remote_controlboard) (local /client) (remote /controlboard)");
if (!dd.isValid())
{
printf("Device not available.\n");
// Network::fini();
return 1;
}
IPositionControl *pos;
ITorqueControl *tor;
IEncoders *enc;
//yarp::os::Thread *th;
if(!dd.view(pos))
{
printf("Problem of open pos device\n");
return 0;
}
pos->setRefSpeed(9, 10);
std::cout << pos->positionMove(9, 150) << std::endl;
/*
// Test for SSC32 servos
pos->setRefSpeed(6, 10);
std::cout << pos->positionMove(6, 0) << std::endl;
//Time::delay(1);
//pos->setRefSpeed(3, 0.1);
std::cout << pos->positionMove(6, 0) << std::endl;
//Time::delay(1);
//pos->setRefSpeed(4, 0.1);
pos->setRefSpeed(5, 30);
pos->setRefSpeed(4, 80);
pos->setRefSpeed(3, 80);
std::cout << pos->positionMove(5, 0) << std::endl;
std::cout << pos->positionMove(4, 0) << std::endl;
std::cout << pos->positionMove(3, -40) << std::endl;
Time::delay(1);
std::cout << pos->positionMove(3, 40) << std::endl;
*/
///*
if(!dd.view(tor))
{
printf("Problem of open tor device\n");
return 0;
}
tor->setRefTorque(9, 1023);
Time::delay(2);
tor->setRefTorque(9, 0);
Time::delay(2);
tor->setRefTorque(9, 1023);
Time::delay(2);
//tor->setRefTorque(9, 1023);
/*
if(!dd.view(enc))
{
printf("Problem of open enc device\n");
return 0;
}
int jnts = 5;
double t = 0;
double p = 0;
pos->getAxes(&jnts);
std::cout << jnts << std::endl;
pos->setRefSpeed(4, 23);
std::cout << pos->positionMove(4, 90) << std::endl;
pos->setRefSpeed(6, 20);
std::cout << pos->relativeMove(6, 10) << std::endl;
enc->getEncoder(4, &p);
std::cout << "Position for joint 4 is --> " << p << std::endl;
// printf("axes: %d\n", jnts);
Time::delay(1);
pos->setRefSpeed(2, 23);
//yarp::os::Searchable c;
// std::cout << pos->positionMove(2, 15) << std::endl;
//Time::delay(1);
pos->setRefSpeed(4, 9);
//yarp::os::Searchable c;
std::cout << pos->positionMove(4, 100) << std::endl;
yarp::os::Time::delay(1);
//sleep(1);
//tor->getTorque(4, &t);
//std::cout << t << std::endl;
std::cout << pos->relativeMove(4, 20) << std::endl;
//sleep(1);
//pos->relativeMove(4, 40);
//yarp::os::Time::delay(1);
//sleep(1);
//sleep(1);
for (int ii = 0; ii < 10; ii++)
{
//tor->getTorque(4, &t);
// enc->getEncoderSpeed(4, &t);
// std::cout << "Torque for join t 4 is --> " << t << std::endl;
//std::cout << pos->positionMove(4, 90 + ii*5) << std::endl;
// sleep(1);
yarp::os::Time::delay(0.11);
enc->getEncoder(4, &p);
std::cout << "Position for joint 4 is --> " << p << std::endl;
enc->getEncoder(1, &p);
std::cout << "Position for joint 1 is --> " << p << std::endl;
enc->getEncoder(2, &p);
std::cout << "Position for joint 2 is --> " << p << std::endl;
enc->getEncoder(3, &p);
std::cout << "Position for joint 3 is --> " << p << std::endl;
}
yarp::os::Time::delay(1);
// std::cout << pos->positionMove(4, 90) << std::endl;
std::cout << pos->relativeMove(4, -20) << std::endl;
// std::cout << pos->positionMove(2, 30) << std::endl;
// std::cout << pos->positionMove(2, 50) << std::endl;
Time::delay(1);
*/
dd.close();
return 0;
}
int main(int argc, char *argv[]){
Network yarp;
//Port<Bottle> armPlan;
//Port<Bottle> armPred;
Port armPlan;
Port armPred;
armPlan.open("/randArm/plan");
armPred.open("/randArm/pred");
bool fwCvOn = 0;
fwCvOn = Network::connect("/randArm/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/randArm/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o");
return 1;
}
const gsl_rng_type *T;
gsl_rng *r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name");
fprintf(stderr, "e.g. --robot icub");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("side")){
remotePorts += params.find("side").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/randArm/cmd";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str());
options.put("remote", remotePorts.c_str());
PolyDriver robotDevice(options);
if (!robotDevice.isValid()){
printf("Device not available. Here are known devices: \n");
printf("%s", Drivers::factory().toString().c_str());
Network::fini();
return 1;
}
IPositionControl *pos;
IEncoders *enc;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(enc);
if (!ok){
printf("Problems acquiring interfaces\n");
return 0;
}
int nj = 0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector commandCart;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
commandCart.resize(nj);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 5.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
while (true){
tmp = command;
command[0] += 15*(2*gsl_rng_uniform(r)-1);
command[1] += 15*(2*gsl_rng_uniform(r)-1);
command[2] += 15*(2*gsl_rng_uniform(r)-1);
command[3] += 15*(2*gsl_rng_uniform(r)-1);
printf("%.1lf %.1lf %.1lf %.1lf\n", command[0], command[1], command[2], command[3]);
//above 0 doesn't seem to be safe for joint 0
if (command[0] > 0 || command[0] < -90){
command[0] = tmp[0];
}
if (command[1] > 160 || command[1] < -0){
command[1] = tmp[1];
}
if (command[2] > 100 || command[2] < -35){
command[2] = tmp[2];
}
if (command[3] > 100 || command[3] < 10){
command[3] = tmp[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add(command[i]);
}
armPlan.write(plan);
armPred.read(pred);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command.data());
done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
}
else{
printf("Self collision detected!\n");
}
}
robotDevice.close();
gsl_rng_free(r);
return 0;
}
