// returns <num_steps> number of interpolated points, with the start and end
// included in this count. That's why we subtract 1 from the input number.
vector<ContBaseState> ContBaseState::interpolate(const ContBaseState& start,
const ContBaseState& end,
int num_steps){
vector<ContBaseState> interp_steps;
if (num_steps < 2){
interp_steps.push_back(start);
interp_steps.push_back(end);
return interp_steps;
}
num_steps--;
double dX = end.x() - start.x();
double dY = end.y() - start.y();
double dZ = end.z() - start.z();
double dTheta = angles::shortest_angular_distance(start.theta(), end.theta());
double step_mult = 1/static_cast<double>(num_steps);
for (int i=0; i <= num_steps; i++){
ContBaseState state(start.x() + i*dX*step_mult,
start.y() + i*dY*step_mult,
start.z() + i*dZ*step_mult,
start.theta() + i*dTheta*step_mult);
interp_steps.push_back(state);
}
return interp_steps;
}
// takes in an ompl state and returns a proper robot state that represents the
// same state.
bool OMPLPR2Planner::convertFullState(ompl::base::State* state, RobotState& robot_state,
ContBaseState& base){
ContObjectState obj_state;
// fix the l_arm angles
vector<double> init_l_arm(7,0);
init_l_arm[0] = (0.038946287971107774);
init_l_arm[1] = (1.2146697069025374);
init_l_arm[2] = (1.3963556492780154);
init_l_arm[3] = -1.1972269899800325;
init_l_arm[4] = (-4.616317135720829);
init_l_arm[5] = -0.9887266887318599;
init_l_arm[6] = 1.1755681069775656;
LeftContArmState l_arm(init_l_arm);
RightContArmState r_arm;
const ompl::base::CompoundState* s = dynamic_cast<const ompl::base::CompoundState*> (state);
obj_state.x((*(s->as<VectorState>(0)))[0]);
obj_state.y((*(s->as<VectorState>(0)))[1]);
obj_state.z((*(s->as<VectorState>(0)))[2]);
obj_state.roll((*(s->as<VectorState>(0)))[3]);
obj_state.pitch((*(s->as<VectorState>(0)))[4]);
obj_state.yaw((*(s->as<VectorState>(0)))[5]);
r_arm.setUpperArmRoll((*(s->as<VectorState>(0)))[6]);
l_arm.setUpperArmRoll((*(s->as<VectorState>(0)))[7]);
base.z((*(s->as<VectorState>(0)))[8]);
base.x(s->as<SE2State>(1)->getX());
base.y(s->as<SE2State>(1)->getY());
base.theta(s->as<SE2State>(1)->getYaw());
RobotState seed_state(base, r_arm, l_arm);
RobotPosePtr final_state;
if (!RobotState::computeRobotPose(obj_state, seed_state, final_state))
return false;
robot_state = *final_state;
return true;
}
