/*
* Disable PID
*/
void partMover::dis_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;
IControlMode2 *ictrl = currentPart->ctrlmode2;
#if 0
ictrl->setControlMode(*joint,VOCAB_CM_IDLE);
#else
ictrl->setControlMode(*joint,VOCAB_CM_FORCE_IDLE);
#endif
return;
}
void idlingCoupledJoints(const int i, const bool sw)
{
IControlMode2 *imod;
driver->view(imod);
PidData &pid=pids[i];
for (size_t j=0; j<pid.idling_joints.size(); j++)
imod->setControlMode(pid.idling_joints[j],
sw?VOCAB_CM_IDLE:VOCAB_CM_POSITION);
}
bool close()
{
// Terminate model
Controller_terminate(Controller_M);
imod->setControlMode(joint,VOCAB_CM_POSITION);
if (compliant)
iint->setInteractionMode(joint,VOCAB_IM_STIFF);
rpc.close();
dataOut.close();
dataIn.close();
drv.close();
return true;
}
void partMover::run_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;
IControlMode2 *ictrl = currentPart->ctrlmode2;
GtkWidget **sliderAry = currentPart->sliderArray;
double posJoint;
while (!iiencs->getEncoder(*joint, &posJoint))
Time::delay(0.001);
ictrl->setControlMode(*joint,VOCAB_CM_POSITION);
gtk_range_set_value ((GtkRange *) (sliderAry[*joint]), posJoint);
return;
}
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());
}
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;
}
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");
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;
}
