shared_ptr<arma::mat> log_q_matrix(shared_ptr<arma::mat> z, const ExampleIds& example_ids) { shared_ptr<arma::mat> log_q( new arma::mat(z->n_rows, z->n_cols) ); arma::uword ind = 0; for(auto j : example_ids) { for(arma::uword i=0; i<z->n_rows; ++i) { // TODO inline? (*log_q)(i, ind) = compute_log_q((*z)(i, ind), i, j); } ++ind; } return log_q; }
void MotionPlannerPerrin::publish(){ // // q vector is filled like this: // //q[0]=ID //q[1]=end of trajectory //q[2]=time start //q[3]=time end //--- 17 values per frame: //q[4:15]=q values (12 values) //q[16]=comX //q[17]=comY //q[18]=waistOrient in radian //q[19]=zmpX //q[20]=zmpY // //the next starting points for frames i=1:T //-> q[4 + i*17 + k], k=0:16; //obtain footsteps from planner std::vector<fastreplanning::footStepInterface> fsi; planner->getInterfaceSteps(fsi); getAbsoluteFromRelativeFootSteps(fsi); Logger log_steps("playback_steps.dat"); Logger log_q("playback_q.dat"); if(fsi.size()>0){ ROS_INFO("NUMBER OF FOOTSTEPS: %d", fsi.size()); for(uint i=0;i<fsi.size();i++){ log_steps(fsi.at(i).data); double x = fsi.at(i).data.at(5); double y = fsi.at(i).data.at(6); double yaw = fsi.at(i).data.at(7); char foot = fsi.at(i).data.at(3); if(foot == 'R'){ ros::RightFootMarker f(x,y,yaw); if(i==0) f.reset(); //clear all previous footsteps in rviz f.publish(); }else{ ros::LeftFootMarker f(x,y,yaw); if(i==0) f.reset(); //clear all previous footsteps in rviz f.publish(); } ROS_INFO("[%d] at x=%f, y=%f, theta=%f", i,x,y,yaw); } } std::vector<double> q = planner->getArticulatedValues(); //void generateTrajectory(vector<vector<double> > & trajTimedRadQ, StepFeatures & stepF, int from = 0, int size = -1); log_q(q, "\n"); ROS_INFO("Articulated values: %d", q.size()); uint i = 1; if(q.size()>0 && fsi.size()>i){ //play footstep transition and update starting position tv->init(q); tv->setCoMOffset(cur_com_x, cur_com_y, cur_com_t); ros::Rate r(500); while(tv->next()){ ros::spinOnce(); r.sleep(); } DEBUG( if(fsi.at(i).data.at(1)>100000.0){ HALT("wrong y error spotted") } ) double x = fsi.at(i).data.at(5); double y = fsi.at(i).data.at(6); double t = fsi.at(i).data.at(7); char foot = fsi.at(i).data.at(3); std::vector<double> lastFoot = vecD(x,y,t,foot); std::vector<double> newCoM = tv->getFinalCoM(); cur_sf_x = lastFoot.at(0); cur_sf_y = lastFoot.at(1); cur_sf_yaw = lastFoot.at(2); cur_sf_foot = foot; cur_com_x = newCoM.at(0); cur_com_y = newCoM.at(1); cur_com_t = newCoM.at(2); planner->updateLocalizationProtected( lastFoot, newCoM ); uint curIndex; planner->getCurrentStepIndex( curIndex ); ROS_INFO("------------------ CURRENT STEP INDEX %d", curIndex); if(curIndex >= fsi.size()-1){ //std::vector<double> lastFoot = vecD(0,0,0,'L'); //std::vector<double> newCoM = vecD(0,0,0); //ROS_INFO("restarting trajectory"); //planner->updateLocalizationProtected( lastFoot, newCoM ); } }