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);
}
void handHandler(const bool b)
{
if (b)
{
if (s1==idle)
s1=triggered;
else if (s1==triggered)
{
if (++c1*getPeriod()>0.5)
{
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
s1=running;
}
}
else
ivel->velocityMove(joints.size(),joints.getFirst(),vels.data());
}
else
{
if (s1==triggered)
vels=-1.0*vels;
if (c1!=0)
ivel->stop(joints.size(),joints.getFirst());
s1=idle;
c1=0;
}
}
/*
* 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 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;
}
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;
}
bool close()
{
iarm->stopControl();
iarm->restoreContext(startup_context);
drvCart.close();
ivel->stop(joints.size(),joints.getFirst());
for (size_t i=0; i<modes.size(); i++)
modes[i]=VOCAB_CM_POSITION;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
drvHand.close();
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
simPort.write(cmd,reply);
simPort.close();
}
if (gaze)
{
igaze->stopControl();
drvGaze.close();
}
igeo->stopFeedback();
igeo->setTransformation(eye(4,4));
drvGeomagic.close();
forceFbPort.close();
return true;
}
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;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("teleop-icub")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str();
double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble();
part=rf.check("part",Value("right_arm")).asString().c_str();
simulator=rf.check("simulator",Value("off")).asString()=="on";
gaze=rf.check("gaze",Value("off")).asString()=="on";
minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble());
maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble());
bool torso=rf.check("torso",Value("on")).asString()=="on";
Property optGeo("(device hapticdeviceclient)");
optGeo.put("remote",("/"+geomagic).c_str());
optGeo.put("local",("/"+name+"/geomagic").c_str());
if (!drvGeomagic.open(optGeo))
return false;
drvGeomagic.view(igeo);
if (simulator)
{
simPort.open(("/"+name+"/simulator:rpc").c_str());
if (!Network::connect(simPort.getName().c_str(),"/icubSim/world"))
{
yError("iCub simulator is not running!");
drvGeomagic.close();
simPort.close();
return false;
}
}
if (gaze)
{
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
{
drvGeomagic.close();
simPort.close();
return false;
}
drvGaze.view(igaze);
}
Property optCart("(device cartesiancontrollerclient)");
optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
optCart.put("local",("/"+name+"/cartesianController/"+part).c_str());
if (!drvCart.open(optCart))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
return false;
}
drvCart.view(iarm);
Property optHand("(device remote_controlboard)");
optHand.put("remote",("/"+robot+"/"+part).c_str());
optHand.put("local",("/"+name+"/"+part).c_str());
if (!drvHand.open(optHand))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
drvCart.close();
return false;
}
drvHand.view(imod);
drvHand.view(ipos);
drvHand.view(ivel);
iarm->storeContext(&startup_context);
iarm->restoreContext(0);
Vector dof(10,1.0);
if (!torso)
dof[0]=dof[1]=dof[2]=0.0;
else
dof[1]=0.0;
iarm->setDOF(dof,dof);
iarm->setTrajTime(Tp2p);
Vector accs,poss;
for (int i=0; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_POSITION);
accs.push_back(1e9);
vels.push_back(100.0);
poss.push_back(0.0);
}
poss[0]=20.0;
poss[1]=70.0;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
ipos->positionMove(joints.size(),joints.getFirst(),poss.data());
joints.clear();
modes.clear();
vels.clear();
for (int i=2; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_VELOCITY);
vels.push_back(40.0);
}
vels[vels.length()-1]=100.0;
s0=s1=idle;
c0=c1=0;
onlyXYZ=true;
stateStr[idle]="idle";
stateStr[triggered]="triggered";
stateStr[running]="running";
Matrix T=zeros(4,4);
T(0,1)=1.0;
T(1,2)=1.0;
T(2,0)=1.0;
T(3,3)=1.0;
igeo->setTransformation(SE3inv(T));
igeo->setCartesianForceMode();
igeo->getMaxFeedback(maxFeedback);
Tsim=zeros(4,4);
Tsim(0,1)=-1.0;
Tsim(1,2)=1.0; Tsim(1,3)=0.5976;
Tsim(2,0)=-1.0; Tsim(2,3)=-0.026;
Tsim(3,3)=1.0;
pos0.resize(3,0.0);
rpy0.resize(3,0.0);
x0.resize(3,0.0);
o0.resize(4,0.0);
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("ssph");
// radius
cmd.addDouble(0.02);
// position
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
// color
cmd.addInt(1);
cmd.addInt(0);
cmd.addInt(0);
// collision
cmd.addString("FALSE");
simPort.write(cmd,reply);
}
forceFbPort.open(("/"+name+"/force-feedback:i").c_str());
feedback.resize(3,0.0);
return true;
}
bool exploreTorso(Vector target)
{
Vector torsoInitialJoints;
Vector torsoActualJoints;
Vector torsoVelocityCommand;
Vector torsoAccCommand;
Vector error,integral,derivative,preError;
int jointsNumber=0;
int i;
double time= 0.0;
itorsoVelocity->getAxes(&jointsNumber);
torsoAccCommand.resize(jointsNumber);
torsoVelocityCommand.resize(jointsNumber);
torsoInitialJoints.resize(jointsNumber);
torsoActualJoints.resize(jointsNumber);
error.resize(jointsNumber);
integral.resize(jointsNumber);
derivative.resize(jointsNumber);
preError.resize(jointsNumber);
preError[0] = 0.0;
preError[1] = 0.0;
preError[2] = 0.0;
integral[0] = 0.0;
integral[1] = 0.0;
integral[2] = 0.0;
derivative[0] = 0.0;
derivative[1] = 0.0;
derivative[2] = 0.0;
for(i =0; i< jointsNumber;i++);
torsoAccCommand[i] = torsoAcceleration[i];
itorsoVelocity->setRefAccelerations(torsoAccCommand.data());
if (!iTorsoEncoder->getEncoders(torsoInitialJoints.data())){
cout<<"Error in reading encoders."<<endl;
return false;
}
VectorOf<int> modes(3);
modes[0]=modes[1]=modes[2]=VOCAB_CM_VELOCITY;
itorsoMode->setControlModes(modes.getFirst());
error = target-torsoInitialJoints;
integral = integral + (error * DT);
derivative = (error - preError) / DT;
preError = error;
torsoVelocityCommand = kp * error + KI * integral + KD * derivative;
time = Time::now();
while (norm(error)>0.2){
if(Time::now()-time>maxTorsoTrajTime){
cout<<"Max time reached."<<endl;
itorsoVelocity->stop();
return true;
}
itorsoVelocity->velocityMove(torsoVelocityCommand.data());
if (!iTorsoEncoder->getEncoders(torsoActualJoints.data())){
cout<<"Error in reading encoders."<<endl;
itorsoVelocity->stop();
return false;
}
error = target-torsoActualJoints;
integral = integral + (error * DT);
derivative = (error - preError) / DT;
preError = error;
torsoVelocityCommand = kp * error + KI * integral + KD * derivative;
Time::delay(DT);
}
itorsoVelocity->stop();
return true;
}
