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