Vector reactCtrlThread::solveIK(int &_exit_code)
{
slv=new reactIpOpt(*arm->asChain(),tol,verbosity);
// Next step will be provided iteratively.
// The equation is x(t_next) = x_t + (x_d - x_t) * (t_next - t_now/T-t_now)
// s.t. t_next = t_now + dT
double dT=getRate()/1000.0;
double t_t=yarp::os::Time::now();
int exit_code=-1;
// if (t_t>=t_d)
// {
// return Vector(3,0.0);
// }
// First test: the next point will be given w.r.t. the current one
// x_n = x_t + (x_d-x_t) * (dT/(t_d-t_t));
// Second test: the next point will be agnostic of the current
// configuration
// x_n = x_0 + (x_d-x_0) * ((t_t+dT-t_0)/(t_d-t_0));
// Third solution: use the particleThread
// If the particle reached the target, let's stop it
if (norm(x_n-x_0) > norm(x_d-x_0)) //if the particle is farther than the final target, we reset the particle - it will stay with the target
{
prtclThrd->resetParticle(x_d);
}
x_n=prtclThrd->getParticle(); //to get next target
if(visualizeParticleInSim){
Vector x_n_sim(3,0.0);
convertPosFromRootToSimFoR(x_n,x_n_sim);
moveSphere(2,x_n_sim); //sphere created as second (particle) will keep the index 2
}
AvoidanceHandlerAbstract *avhdl;
avhdl = new AvoidanceHandlerTactile(*arm->asChain(),collisionPoints,verbosity);
Matrix vLimAdapted=avhdl->getVLIM(vLimNominal);
printf("calling ipopt with the following joint velocity limits (deg): \n %s \n",vLimAdapted.toString(3,3).c_str());
//printf("calling ipopt with the following joint velocity limits (rad): \n %s \n",(vLimAdapted*CTRL_DEG2RAD).toString(3,3).c_str());
// Remember: at this stage everything is kept in degrees because the robot is controlled in degrees.
// At the ipopt level it comes handy to translate everything in radians because iKin works in radians.
// So, q_dot_0 is in degrees, but I have to convert it in radians before sending it to ipopt
Vector res=slv->solve(x_n,q_dot_0*CTRL_DEG2RAD,dT,vLimAdapted*CTRL_DEG2RAD,ipoptBoundSmoothnessOn,&exit_code)*CTRL_RAD2DEG;
// printMessage(0,"t_d: %g\tt_t: %g\n",t_d-t_0, t_t-t_0);
printMessage(0,"x_n: %s\tx_d: %s\tdT %g\n",x_n.toString(3,3).c_str(),x_d.toString(3,3).c_str(),dT);
printMessage(0,"x_0: %s\tx_t: %s\n", x_0.toString(3,3).c_str(),x_t.toString(3,3).c_str());
printMessage(0,"norm(x_n-x_t): %g\tnorm(x_d-x_n): %g\tnorm(x_d-x_t): %g\n",
norm(x_n-x_t), norm(x_d-x_n), norm(x_d-x_t));
printMessage(0,"Result: %s\n",res.toString(3,3).c_str());
_exit_code=exit_code;
q_dot_0=res; //result at this step will be prepared as q_dot_0 for the next iteration of the solver
delete slv;
return res;
}
int main(int argc, char *argv[])
{
yarp::os::Network yarp;
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("react-control");
rf.setDefaultConfigFile("reactController-sim.ini");
rf.configure(argc,argv);
int verbosity = rf.check("verbosity",Value(0)).asInt();
double sim_time=rf.check("sim-time",Value(10.0)).asDouble();
double motor_tau=rf.check("motor-tau",Value(0.0)).asDouble(); //motor transfer function
string avoidance_type=rf.check("avoidance-type",Value("tactile")).asString(); //none | visuo | tactile
bool visuo_scaling_by_sNorm=rf.check("visuo-scaling-snorm",Value("off")).asString()=="on"?true:false; // on | off
string target_type=rf.check("target-type",Value("moving-circular")).asString(); // moving-circular | static
string obstacle_type=rf.check("obstacle-type",Value("falling")).asString(); //falling | static
yInfo("Starting with the following parameters: \n verbosity: %d \n sim-time: %f \n motor-tau: %f \n avoidance-type: %s \n target-type: %s \n obstacle-type: %s \n",verbosity,sim_time,motor_tau,avoidance_type.c_str(),target_type.c_str(),obstacle_type.c_str());
if (!yarp.checkNetwork())
{
yError("No Network!!!");
return -1;
}
iCubArm arm("left");
iKinChain &chain=*arm.asChain();
chain.releaseLink(0); //releasing torso links that are blocked by default
chain.releaseLink(1);
chain.releaseLink(2);
Vector q0(chain.getDOF(),0.0);
q0[3]=-25.0; q0[4]=20.0; q0[6]=50.0; //setting shoulder position
chain.setAng(CTRL_DEG2RAD*q0);
Matrix lim(chain.getDOF(),2); //joint position limits, in degrees
Matrix v_lim(chain.getDOF(),2); //joint velocity limits, in degrees/s
for (size_t r=0; r<chain.getDOF(); r++)
{
lim(r,0)=CTRL_RAD2DEG*chain(r).getMin();
lim(r,1)=CTRL_RAD2DEG*chain(r).getMax();
v_lim(r,0)=-50.0;
v_lim(r,1)=+50.0;
}
v_lim(1,0)=v_lim(1,1)=0.0; // disable torso roll
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",1e-6);
app->Options()->SetStringValue("mu_strategy","adaptive");
app->Options()->SetIntegerValue("max_iter",10000);
app->Options()->SetNumericValue("max_cpu_time",0.05);
app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
app->Options()->SetStringValue("hessian_approximation","limited-memory");
app->Options()->SetStringValue("derivative_test","none");
app->Options()->SetIntegerValue("print_level",0);
app->Initialize();
Ipopt::SmartPtr<ControllerNLP> nlp=new ControllerNLP(chain);
AvoidanceHandlerAbstract *avhdl;
if (avoidance_type=="none")
avhdl=new AvoidanceHandlerAbstract(arm);
else if (avoidance_type=="visuo")
avhdl=new AvoidanceHandlerVisuo(arm,visuo_scaling_by_sNorm);
else if (avoidance_type=="tactile")
avhdl=new AvoidanceHandlerTactile(arm);
else if (avoidance_type=="visuo-tactile")
avhdl=new AvoidanceHandlerVisuoTactile(arm,visuo_scaling_by_sNorm);
else
{
yError()<<"unrecognized avoidance type! exiting ...";
return 1;
}
yInfo()<<"Avoidance-Handler="<<avhdl->getType();
yInfo()<<"Avoidance Parameters="<<avhdl->getParameters().toString();
double dt=0.01;
double T=1.0;
nlp->set_dt(dt);
Motor motor(q0,lim,motor_tau,dt);
Vector v(chain.getDOF(),0.0);
Vector xee=chain.EndEffPosition();
//actual target
Vector xc(3); //center of target
xc[0]=-0.35;
xc[1]=0.0;
xc[2]=0.1;
double rt=.1; //target will be moving along circular trajectory with this radius
//if (target_type == "moving-circular") double rt=0.1;
if (target_type == "static")
rt=0.0; //static target will be "moving" along a trajectory with 0 radius
minJerkTrajGen target(xee,dt,T); //target for end-effector
Vector xo(3); //obstacle position
Vector vo(3,0.0); //obstacle velocity
Obstacle obstacle(xo,0.07,vo,dt);
if (obstacle_type == "falling"){
xo[0]=-0.3;
xo[1]=0.0;
xo[2]=0.4;
vo[2]=-0.1;
obstacle.setPosition(xo);
obstacle.setVelocity(vo);
}
else if(obstacle_type == "static"){
xo[0]=-0.35;
xo[1]=-0.05;
//xo[2]=0.04;
xo[2]=0.02;
obstacle.setPosition(xo);
obstacle.setRadius(0.04);
}
ofstream fout_param; //log parameters that stay constant during the simulation, but are important for analysis - e.g. joint limits
ofstream fout; //to log data every iteration
fout_param.open("param.log");
fout.open("data.log");
fout_param<<chain.getDOF()<<" ";
for (size_t i=0; i<chain.getDOF(); i++)
{
fout_param<<CTRL_RAD2DEG*chain(i).getMin()<<" ";
fout_param<<CTRL_RAD2DEG*chain(i).getMax()<<" ";
}
for (size_t i=0; i<chain.getDOF(); i++)
{
fout_param<<v_lim(i,0)<<" ";
fout_param<<v_lim(i,1)<<" ";
}
std::signal(SIGINT,signal_handler);
for (double t=0.0; t<sim_time; t+=dt)
{
//printf("\n**************************************\n main loop:t: %f s \n",t);
Vector xd=xc; //target moving along circular trajectory
xd[1]+=rt*cos(2.0*M_PI*0.3*t);
xd[2]+=rt*sin(2.0*M_PI*0.3*t);
target.computeNextValues(xd);
Vector xr=target.getPos(); //target for end-effector - from minJerkTrajGen
xo=obstacle.move();
avhdl->updateCtrlPoints();
Matrix VLIM=avhdl->getVLIM(obstacle,v_lim);
nlp->set_xr(xr);
nlp->set_v_lim(VLIM); //VLIM in deg/s; set_v_lim converts to radians/s
nlp->set_v0(v);
nlp->init();
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
v=nlp->get_result(); //updating the joint velocities with the output from optimizer
xee=chain.EndEffPosition(CTRL_DEG2RAD*motor.move(v));
if (verbosity>0){ //this is probably not the right way to do it
yInfo()<<" t [s] = "<<t;
yInfo()<<" v [deg/s] = ("<<v.toString(3,3)<<")";
yInfo()<<" |xr-xee| [m] = "<<norm(xr-xee);
yInfo()<<"";
}
ostringstream strVLIM;
for (size_t i=0; i<VLIM.rows(); i++)
strVLIM<<VLIM.getRow(i).toString(3,3)<<" ";
ostringstream strCtrlPoints;
deque<Vector> ctrlPoints=avhdl->getCtrlPointsPosition();
for (size_t i=0; i<ctrlPoints.size(); i++)
strCtrlPoints<<ctrlPoints[i].toString(3,3)<<" ";
fout.setf(std::ios::fixed, std::ios::floatfield);
fout<<setprecision(3);
fout<<t<<" "<<
xd.toString(3,3)<<" "<<
obstacle.toString()<<" "<<
xr.toString(3,3)<<" "<<
v.toString(3,3)<<" "<<
(CTRL_RAD2DEG*chain.getAng()).toString(3,3)<<" "<<
strVLIM.str()<<
strCtrlPoints.str()<<
endl;
//in columns on the output for 10 DOF case: 1:time, 2:4 target, 5:8 obstacle, 9:11 end-eff target, 12:21 joint velocities, 22:31 joint pos, 32:51 joint vel limits set by avoidance handler (joint1_min, joint1_max, joint2_min, ...., joint10_min, joint10_max) , 52:end - current control points' (x,y,z) pos in Root FoR for each control point
if (gSignalStatus==SIGINT)
{
yWarning("SIGINT detected: exiting ...");
break;
}
}
fout.close();
delete avhdl;
return 0;
}
int main(int argc, char *argv[])
{
ResourceFinder rf;
rf.configure(argc,argv);
double sim_time=rf.check("sim-time",Value(10.0)).asDouble();
double motor_tau=rf.check("motor-tau",Value(0.0)).asDouble();
string avoidance_type=rf.check("avoidance-type",Value("tactile")).asString();
iCubArm arm("left");
iKinChain &chain=*arm.asChain();
chain.releaseLink(0);
chain.releaseLink(1);
chain.releaseLink(2);
Vector q0(chain.getDOF(),0.0);
q0[3]=-25.0; q0[4]=20.0; q0[6]=50.0;
chain.setAng(CTRL_DEG2RAD*q0);
Matrix lim(chain.getDOF(),2);
Matrix v_lim(chain.getDOF(),2);
for (size_t r=0; r<chain.getDOF(); r++)
{
lim(r,0)=CTRL_RAD2DEG*chain(r).getMin();
lim(r,1)=CTRL_RAD2DEG*chain(r).getMax();
v_lim(r,0)=-50.0;
v_lim(r,1)=+50.0;
}
v_lim(1,0)=v_lim(1,1)=0.0; // disable torso roll
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",1e-6);
app->Options()->SetStringValue("mu_strategy","adaptive");
app->Options()->SetIntegerValue("max_iter",10000);
app->Options()->SetNumericValue("max_cpu_time",0.05);
app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
app->Options()->SetStringValue("hessian_approximation","limited-memory");
app->Options()->SetStringValue("derivative_test","none");
app->Options()->SetIntegerValue("print_level",0);
app->Initialize();
Ipopt::SmartPtr<ControllerNLP> nlp=new ControllerNLP(chain);
AvoidanceHandlerAbstract *avhdl;
if (avoidance_type=="none")
avhdl=new AvoidanceHandlerAbstract(arm);
else if (avoidance_type=="visuo")
avhdl=new AvoidanceHandlerVisuo(arm);
else if (avoidance_type=="tactile")
avhdl=new AvoidanceHandlerTactile(arm);
else if (avoidance_type=="visuo-tactile")
avhdl=new AvoidanceHandlerVisuoTactile(arm);
else
{
yError()<<"unrecognized avoidance type! exiting ...";
return 1;
}
yInfo()<<"Avoidance-Handler="<<avhdl->getType();
yInfo()<<"Avoidance Parameters="<<avhdl->getParameters().toString();
double dt=0.01;
double T=1.0;
nlp->set_dt(dt);
Motor motor(q0,lim,motor_tau,dt);
Vector v(chain.getDOF(),0.0);
Vector xee=chain.EndEffPosition();
minJerkTrajGen target(xee,dt,T);
Vector xc(3);
xc[0]=-0.35;
xc[1]=0.0;
xc[2]=0.1;
double rt=0.1;
Vector xo(3);
xo[0]=-0.3;
xo[1]=0.0;
xo[2]=0.4;
Vector vo(3,0.0);
vo[2]=-0.1;
Obstacle obstacle(xo,0.07,vo,dt);
ofstream fout;
fout.open("data.log");
std::signal(SIGINT,signal_handler);
for (double t=0.0; t<sim_time; t+=dt)
{
Vector xd=xc;
xd[1]+=rt*cos(2.0*M_PI*0.3*t);
xd[2]+=rt*sin(2.0*M_PI*0.3*t);
target.computeNextValues(xd);
Vector xr=target.getPos();
xo=obstacle.move();
avhdl->updateCtrlPoints();
Matrix VLIM=avhdl->getVLIM(obstacle,v_lim);
nlp->set_xr(xr);
nlp->set_v_lim(VLIM);
nlp->set_v0(v);
nlp->init();
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
v=nlp->get_result();
xee=chain.EndEffPosition(CTRL_DEG2RAD*motor.move(v));
yInfo()<<" t [s] = "<<t;
yInfo()<<" v [deg/s] = ("<<v.toString(3,3)<<")";
yInfo()<<" |xr-xee| [m] = "<<norm(xr-xee);
yInfo()<<"";
ostringstream strCtrlPoints;
deque<Vector> ctrlPoints=avhdl->getCtrlPointsPosition();
for (size_t i=0; i<ctrlPoints.size(); i++)
strCtrlPoints<<ctrlPoints[i].toString(3,3)<<" ";
fout<<t<<" "<<
xr.toString(3,3)<<" "<<
obstacle.toString()<<" "<<
v.toString(3,3)<<" "<<
(CTRL_RAD2DEG*chain.getAng()).toString(3,3)<<" "<<
strCtrlPoints.str()<<
endl;
if (gSignalStatus==SIGINT)
{
yWarning("SIGINT detected: exiting ...");
break;
}
}
fout.close();
delete avhdl;
return 0;
}
