ContObjectState RobotState::getObjectStateRelMap(ContBaseState base) const {
std::vector<double> angles;
SBPLArmModelPtr arm_model;
KDL::Frame offset;
bool left_arm_dominant = (m_planning_mode == PlanningModes::LEFT_ARM ||
m_planning_mode == PlanningModes::LEFT_ARM_MOBILE);
if (left_arm_dominant){
m_left_arm.getAngles(&angles);
arm_model = m_left_arm.getArmModel();
offset = m_left_arm.getObjectOffset().Inverse();
} else {
m_right_arm.getAngles(&angles);
arm_model = m_right_arm.getArmModel();
offset = m_right_arm.getObjectOffset().Inverse();
}
// 10 is the link number for the wrist
KDL::Frame to_wrist;
arm_model->computeFK(angles, base.body_pose(), 10, &to_wrist);
KDL::Frame f = to_wrist * offset;
double wr,wp,wy;
f.M.GetRPY(wr,wp,wy);
return ContObjectState(f.p.x(), f.p.y(), f.p.z(), wr, wp, wy);
}
// 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;
}
bool OMPLPR2Planner::planPathCallback(SearchRequestParams& search_request, int trial_id){
if (m_planner_id == PRM_P)
ROS_INFO("running PRM planner!");
if (m_planner_id == RRT)
ROS_INFO("running RRT planner!");
if (m_planner_id == RRTSTAR)
ROS_INFO("running RRTStar planner!");
planner->clear();
planner->getProblemDefinition()->clearSolutionPaths();
planner->as<ompl::geometric::PRM>()->clearQuery();
search_request.left_arm_start.getAngles(&m_collision_checker->l_arm_init);
FullState ompl_start(fullBodySpace);
FullState ompl_goal(fullBodySpace);
if (!createStartGoal(ompl_start, ompl_goal, search_request))
return false;
pdef->clearGoal();
pdef->clearStartStates();
pdef->setStartAndGoalStates(ompl_start,ompl_goal);
ompl::base::GoalState* temp_goal = new ompl::base::GoalState(planner->getSpaceInformation());
temp_goal->setState(ompl_goal);
ompl::base::GoalPtr temp_goal2(temp_goal);
// something about different planner types here
//if(planner_id_==2 || planner_id_==3){
pdef->setGoal(temp_goal2);
//}
double t0 = ros::Time::now().toSec();
planner->solve(60.0);
double t1 = ros::Time::now().toSec();
double planning_time = t1-t0;
ompl::base::PathPtr path = planner->getProblemDefinition()->getSolutionPath();
RRTData data;
if (path){
ROS_INFO("OMPL found a solution!");
data.planned = true;
ompl::geometric::PathGeometric geo_path = static_cast<ompl::geometric::PathGeometric&>(*path);
double t2 = ros::Time::now().toSec();
bool b1 = pathSimplifier->reduceVertices(geo_path);
bool b2 = pathSimplifier->collapseCloseVertices(geo_path);
bool b3 = pathSimplifier->shortcutPath(geo_path);
//ROS_ERROR("shortcut:%d\n",b3);
double t3 = ros::Time::now().toSec();
double reduction_time = t3-t2;
data.plan_time = planning_time;
data.shortcut_time = reduction_time;
vector<RobotState> robot_states;
vector<ContBaseState> base_states;
geo_path.interpolate();
for(unsigned int i=0; i<geo_path.getStateCount(); i++){
ompl::base::State* state = geo_path.getState(i);
RobotState robot_state;
ContBaseState base;
if (!convertFullState(state, robot_state, base)){
ROS_ERROR("ik failed on path reconstruction!");
}
vector<double> l_arm, r_arm;
robot_states.push_back(robot_state);
base_states.push_back(base);
robot_state.right_arm().getAngles(&r_arm);
robot_state.left_arm().getAngles(&l_arm);
BodyPose bp = base.body_pose();
Visualizer::pviz->visualizeRobot(r_arm, l_arm, bp, 150, "robot", 0);
usleep(10000);
}
data.robot_state = robot_states;
data.base = base_states;
data.path_length = geo_path.getStateCount();
m_stats_writer.write(trial_id, data);
} else {
data.planned = false;
ROS_ERROR("failed to plan");
return false;
}
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);
}
// 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;
}
