int main(){
//myrobot³õʼ»¯
const double l1=300,l2 =500;
Vector2d WF_vx(1,0),WF_vy(0,1);
Point wf_origin("wf_origin",0,0);
Joint jt1(0,0,0,-180,180,0),jt2(l1,0,0,-180,180,0);
Frame WF("wf",WF_vx,WF_vy,wf_origin);
Robot myRobot(l1,l2,jt1,jt2,WF);
//Point tf1_origin("tf1_origin",400,200),tf2_origin("tf2_origin",100,300),tf3_origin("tf3_origin",200,400);
//Vector2d TF1_vx(0,1),TF1_vy(-1,0),TF2_vx(-1,0),TF2_vy(0,-1),TF3_vx(0,-1),TF3_vy(1,0);
//Frame TF1("tf1",TF1_vx,TF1_vy,tf1_origin),TF2("tf2",TF2_vx,TF2_vy,tf2_origin),TF3("tf3",TF3_vx,TF3_vy,tf3_origin);
//myRobot.TaskFrameCreate(TF1);
//myRobot.TaskFrameCreate(TF2);
//myRobot.TaskFrameCreate(TF3);
//Point P1(1,2),P2(1,2),P3(1,2),P4(1,2),P5(1,2);
//myRobot.RobotShow();
//myRobot.PTPMove(WF,P2);
//myRobot.RobotShow();
//myRobot.PTPMove(TF1,P3);
//myRobot.RobotShow();
//myRobot.PTPMove(TF2,P4);
//myRobot.RobotShow();
//myRobot.PTPMove(TF3,P5);
//myRobot.RobotShow();
return 0;
}
int main(int argc, char * argv[] )
{
//-- Extract data from arguments
std::string config_file;
int run_time;
float step_time;
if ( argc == 3 )
{
config_file = argv[1];
run_time = atoi (argv[2] );
step_time = 0.25;
}
else if ( argc == 4)
{
config_file = argv[1];
run_time = atoi (argv[2] );
step_time = atof(argv[3]);
}
else
{
std::cout << "Usage: evaluate-gaits-sim (config file) (run time(ms)) [simulation step(ms)=2ms]" << std::endl;
exit(-1);
}
//-- Load configuration on a ConfigParser
hormodular::ConfigParser configParser;
if ( configParser.parse(config_file) != 0)
{
std::cerr << "[Evaluate] Error: error parsing xml config file!" << std::endl;
return false;
}
//-- Create robot:
hormodular::ModularRobot myRobot(configParser);
myRobot.setTimeStep(step_time);
myRobot.setProperty("viewer", "enabled");
std::cout << "Evaluate-Gaits (simulated version)" << std::endl
<< "------------------------------------" << std::endl;
//-- Reset robot:
myRobot.reset();
//-- Testing timing:
struct timeval starttime, endtime;
gettimeofday( &starttime, NULL);
//-- Run controller
myRobot.run(run_time);
//-- Get actual elapsed time:
gettimeofday(&endtime, NULL);
int sec_diff = endtime.tv_sec-starttime.tv_sec;
int usec_diff = endtime.tv_usec-starttime.tv_usec;
if (usec_diff < 0)
{
usec_diff += 1000000;
sec_diff -= 1;
}
//-- Report distance travelled:
std::cout << "Distance travelled: " << myRobot.getTravelledDistance() << std::endl;
std::cout << "Robot time elapsed: " << (int) run_time << std::endl;
std::cout << "Real time elapsed: " << sec_diff << "s " << usec_diff << "us " << std::endl;
std::cout << std::endl;
std::cout << "Finished!" << std::endl;
return 0;
}
