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; }