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