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