// 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; }
double ContBaseState::distance(const ContBaseState& start, const ContBaseState& end){ double dX = end.x() - start.x(); double dY = end.y() - start.y(); double dZ = end.z() - start.z(); return pow((pow(dX,2) + pow(dY,2) + pow(dZ,2)),.5); }