TEST_F(TestTransitionWrapper, wrapper) { { rclcpp_lifecycle::Transition t(12, "no_states_set"); EXPECT_EQ(12, t.id()); EXPECT_STREQ("no_states_set", t.label().c_str()); } { std::string transition_name = "no_states_set"; rclcpp_lifecycle::Transition t(12, transition_name); transition_name = "not_no_states_set"; EXPECT_EQ(12, t.id()); EXPECT_STREQ("no_states_set", t.label().c_str()); } { rclcpp_lifecycle::State start_state(1, "start_state"); rclcpp_lifecycle::State goal_state(2, "goal_state"); rclcpp_lifecycle::Transition t( 12, "from_start_to_goal", std::move(start_state), std::move(goal_state)); EXPECT_EQ(12, t.id()); EXPECT_FALSE(t.label().empty()); EXPECT_STREQ("from_start_to_goal", t.label().c_str()); } }
void OpenBagMotionPlanner::PlanBagOpeningMovement () { tf::Transform point0Transf, point1Transf, arrowTransf; tfServer->get("world", "point0_control", point0Transf); tfServer->get("world", "point1_control", point1Transf); tfServer->get("world", "arrow_control", arrowTransf); moveit_msgs::RobotTrajectory trajectory; std::vector<geometry_msgs::Pose> waypoints; robot_state::RobotState start_state(*groupPtr->getCurrentState()); groupPtr->setStartState(start_state); waypoints = CalcOpeningTrajectory(point0Transf, point1Transf, arrowTransf); double fraction = groupPtr->computeCartesianPath(waypoints, 0.01, 0.0, trajectory); // ros::Time begin = ros::Time::now(); // while(fraction != 1 && (ros::Time::now() - begin).toSec() < 5.0) // { // fraction = groupPtr->computeCartesianPath(waypoints, 0.01, 0.0, trajectory); // } plan.trajectory_ = trajectory; }
OpenBagMotionPlanner::OpenBagMotionPlanner(boost::shared_ptr<TFServer> tfServer_) //, ros::NodeHandle& nh_) { groupPtr = boost::shared_ptr< moveit::planning_interface::MoveGroup > (new moveit::planning_interface::MoveGroup (GROUPNAME)); robot_state::RobotState start_state(*groupPtr->getCurrentState()); joint_model_group = start_state.getJointModelGroup(groupPtr->getName()); groupPtr->setStartState(start_state); groupPtr->allowReplanning(false); groupPtr->setPlannerId("RRTConnectkConfigDefault"); tfServer = tfServer_; }
void MotionPlanningFrame::computeLoadQueryButtonClicked() { if (planning_scene_storage_) { QList<QTreeWidgetItem *> sel = ui_->planning_scene_tree->selectedItems(); if (!sel.empty()) { QTreeWidgetItem *s = sel.front(); if (s->type() == ITEM_TYPE_QUERY) { std::string scene = s->parent()->text(0).toStdString(); std::string query_name = s->text(0).toStdString(); moveit_warehouse::MotionPlanRequestWithMetadata mp; bool got_q = false; try { got_q = planning_scene_storage_->getPlanningQuery(mp, scene, query_name); } catch (std::runtime_error &ex) { ROS_ERROR("%s", ex.what()); } if (got_q) { robot_state::RobotStatePtr start_state(new robot_state::RobotState(*planning_display_->getQueryStartState())); robot_state::robotStateMsgToRobotState(planning_display_->getPlanningSceneRO()->getTransforms(), mp->start_state, *start_state); planning_display_->setQueryStartState(*start_state); robot_state::RobotStatePtr goal_state(new robot_state::RobotState(*planning_display_->getQueryGoalState())); for (std::size_t i = 0 ; i < mp->goal_constraints.size() ; ++i) if (mp->goal_constraints[i].joint_constraints.size() > 0) { std::map<std::string, double> vals; for (std::size_t j = 0 ; j < mp->goal_constraints[i].joint_constraints.size() ; ++j) vals[mp->goal_constraints[i].joint_constraints[j].joint_name] = mp->goal_constraints[i].joint_constraints[j].position; goal_state->setVariablePositions(vals); break; } planning_display_->setQueryGoalState(*goal_state); } else ROS_ERROR("Failed to load planning query '%s'. Has the message format changed since the query was saved?", query_name.c_str()); } } } }
void EpicNavigationNodeOMPL::initAlg() { // Only initialize the algorithm if all necessary variables have been defined. if (!goal_assigned || !start_assigned || occupancy_grid == nullptr) { return; } // Setup the OMPL state space. ompl::base::RealVectorBounds ompl_bounds(2); ompl_bounds.setLow(0, 0.0); ompl_bounds.setHigh(0, height); ompl_bounds.setLow(1, 0.0); ompl_bounds.setHigh(1, width); ompl::base::StateSpacePtr ompl_space(new ompl::base::RealVectorStateSpace(2)); ompl_space->as<ompl::base::RealVectorStateSpace>()->setBounds(ompl_bounds); // The state validity checker holds the map representation so we can check if a cell is valid or not. ompl::base::SpaceInformationPtr ompl_space_info(new ompl::base::SpaceInformation(ompl_space)); ompl_space_info->setStateValidityChecker(ompl::base::StateValidityCheckerPtr( new EpicNavigationNodeOMPLStateValidityChecker(ompl_space_info, occupancy_grid))); ompl_space_info->setup(); // We relinquish control of the occupancy_grid. It will be managed by the EpicNavigatioNodeOMPLStateValidityChecker object. occupancy_grid = nullptr; // Create the problem definition. ompl::base::ScopedState<> start_state(ompl_space); start_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0] = start_location.second; start_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[1] = start_location.first; ompl::base::ScopedState<> goal_state(ompl_space); goal_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0] = goal_location.second; goal_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[1] = goal_location.first; ompl::base::ProblemDefinitionPtr ompl_problem_def(new ompl::base::ProblemDefinition(ompl_space_info)); ompl_problem_def->setOptimizationObjective(omplGetPathLengthObjective(ompl_space_info)); if (algorithm == EPIC_ALGORITHM_RRT_CONNECT) { ompl_planner = ompl::base::PlannerPtr(new ompl::geometric::RRTConnect(ompl_space_info)); } else { } init_alg = true; }
void OpenBagMotionPlanner::Viewpoint() { double deg_to_rad = M_PI / 180; std::vector<double> angles; angles.resize(6); angles[0] = -65.56 * deg_to_rad; angles[1] = -101.73 * deg_to_rad; angles[2] = 78.13 * deg_to_rad; angles[3] = 48.27 * deg_to_rad; angles[4] = 103.46 * deg_to_rad; angles[5] = -87.73 * deg_to_rad; robot_state::RobotState start_state(*groupPtr->getCurrentState()); groupPtr->setStartState(start_state); groupPtr->setJointValueTarget(angles); bool success = groupPtr->plan(plan); ROS_INFO("Survey %s",success?"SUCCESS":"FAILED"); Execution(); }
void simulator_ctt::execute_functioncall( const statet &state, edget &edge) { const program_formulat::formula_goto_programt::instructiont &instruction=*state.data().threads.front().PC; const irep_idt &function_id=instruction.code.function; // find in program formula program_formulat::function_mapt::const_iterator f_it=program_formula.function_map.find(function_id); assert(f_it!=program_formula.function_map.end()); const program_formulat::functiont &f=f_it->second; // produce a start state statet start_state(state); statet::threadt &thread=start_state.data_w().threads.back(); // adjust PC thread.program=&(f.body); thread.PC=thread.start_pc(); // assign args assert(instruction.code.function_args.size()==f.args.size()); for(unsigned i=0; i<f.args.size(); i++) { formulat formula=instruction.code.function_args[i]; formula=instantiate(state, 0, formula); start_state.data_w().set_var(f.args[i], 0, formula); } std::cout << "Adding edge for " << function_id << std::endl; // add edge edget &f_edge=new_edge(function_id, start_state); f_edge.calls.push_back(conft(edge, state)); }
//give a network,and generate its corresponding DN:dynamical-network or SN:state-network std::unordered_multimap<int,std::pair<int,int>> genDN(int *network,int size) { bool set[2] = {false,true}; //it is useless Sequence start_state(size,2); Sequence end_state(size,2); std::unordered_multimap<int,std::pair<int,int>> stateNetwork; int stateSum = pow(2,size); for(int i = 0;i < size;i++) start_state[i] = true; for(int stateNum = 0;stateNum < stateSum;stateNum++){ //calculate the end state nextEndState(network,start_state,end_state,size); //store it to a map:start_state->(end_state,b) stateNetwork.insert(std::unordered_multimap<int,std::pair<int,int>>::value_type ((int)end_state,std::make_pair((int)start_state,0))); //for every starting state,get the next state start_state.nextSequence(); } return stateNetwork; }
bool CHOMPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { moveit_msgs::MotionPlanDetailedResponse res_msg; if (chomp_interface_->solve(planning_scene_, request_, chomp_interface_->getParams(), res_msg)) { res.trajectory_.resize(1); res.trajectory_[0] = robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); moveit::core::RobotState start_state(robot_model_); robot_state::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); res.description_.push_back("plan"); res.processing_time_ = res_msg.processing_time; res.error_code_ = res_msg.error_code; return true; } else { res.error_code_ = res_msg.error_code; return false; } }
SM& set_equal (SM& sm, const expression<Expr>& expr) { typename state_machine_traits<SM>::state start, end; start = start_state(sm); bool final_is_set(false); typename expression_arg<SM>::connect_const_iterator citr, cend; typename expression_arg<SM>::to_const_iterator titr, tend; typename expression_arg<SM>::from_const_iterator fitr, fend; expression_arg<SM> arg; expr.apply(sm,arg); citr = arg.connection_begin(); cend = arg.connection_end(); for(;citr != cend; ++citr) { add_transition(sm,start,end,citr->tok,citr->wt); } titr = arg.to_begin(); tend = arg.to_end(); for(;titr != tend; ++titr) { if (!final_is_set) { end = add_state(sm,true); final_is_set=true; } add_transition(sm,start,titr->to,titr->tok,titr->wt); } fitr = arg.from_begin(); fend = arg.from_end(); for(;fitr != fend; ++fitr) { if (!final_is_set) { end = add_state(sm,true); final_is_set=true; } add_transition(sm,fitr->from,end,fitr->tok,fitr->wt); } return sm; }
int main(int argc, char **argv) { ros::init(argc, argv, "lesson_move_group"); // start a background "spinner", so our node can process ROS messages // - this lets us know when the move is completed ros::AsyncSpinner spinner(1); spinner.start(); moveit::planning_interface::MoveGroup group("manipulator"); //group.setPlannerId("ompl_planning/plan_kinematic_path"); //metto il robot in home //group.setNamedTarget("home"); //group.setNamedTarget("straight_up"); group.move(); //--------------------------------- std::vector< double > Angles=group.getCurrentRPY(); Eigen::AngleAxisd rollAngle(Angles[0], Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd yawAngle(Angles[1], Eigen::Vector3d::UnitY()); Eigen::AngleAxisd pitchAngle(Angles[2], Eigen::Vector3d::UnitX()); tf::Quaternion qt; qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]); //abbasso il robot in posizion operativa robot_state::RobotState start_state(*group.getCurrentState()); moveit::core::RobotStatePtr kinematic_state; moveit_msgs::RobotTrajectory trajectory; std::vector<geometry_msgs::Pose> waypoints; geometry_msgs::Pose target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); //target_pose3.position.x += 0.2; target_pose3.position.z -= 0.5; waypoints.push_back(target_pose3); //waypoints.push_back(poseNew); //double fraction = group.computeCartesianPath(waypoints,CART_STEP_SIZE_,CART_JUMP_THRESH_,trajectory_,AVOID_COLLISIONS_); double fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); robot_trajectory::RobotTrajectory rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); // check to see if plan successful move_group_interface::MoveGroup::Plan plan; plan.trajectory_=trajectory; group.execute(plan); //group.move(plan) ; sleep(1.5); Angles=group.getCurrentRPY(); waypoints.clear(); target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); target_pose3.position.y += 0.5; waypoints.push_back(target_pose3); fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); //rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); plan.trajectory_=trajectory; //group.execute(plan); sleep(2.0); Angles=group.getCurrentRPY(); qt=tf::createQuaternionFromRPY ( Angles[0], Angles[1], Angles[2]); waypoints.clear(); target_pose3 = group.getCurrentPose().pose; waypoints.push_back(target_pose3); tf::Transform point_pos( tf::Transform(tf::createQuaternionFromRPY(Angles[0],Angles[1],Angles[2]-0.5),tf::Vector3(target_pose3.position.x,target_pose3.position.y,target_pose3.position.z))); tf::poseTFToMsg (point_pos,target_pose3); waypoints.push_back(target_pose3); fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); kinematic_state = moveit::core::RobotStatePtr(group.getCurrentState()); //rt(kinematic_state->getRobotModel(), "manipulator"); rt.setRobotTrajectoryMsg(*kinematic_state, trajectory); plan.trajectory_=trajectory; group.execute(plan); sleep(2.0); //ruoto il robot rispetto alla z della base group.setNamedTarget("home"); group.move(); }
//give a network,and generate its B:basin,W:the overlap of all trajectory void genBW(int *network,int size) { bool set[2] = {false,true}; //it is useless Sequence start_state(size,2); Sequence end_state(size,2); std::unordered_multimap<int,std::pair<int,int>> stateNetwork; int stateSum = pow(2,size); double *TF = new double [stateSum]; for(int i = 0;i < size;i++) start_state[i] = true; for(int stateNum = 0;stateNum < stateSum;stateNum++){ //calculate the end state nextEndState(network,start_state,end_state,size); //store it to a map:start_state->(end_state,b) stateNetwork.insert(std::unordered_multimap<int,std::pair<int,int>>::value_type ((int)end_state,std::make_pair((int)start_state,0))); //for every starting state,get the next state start_state.nextSequence(); } auto bw = findMaxBW(stateNetwork,TF); //statistics the checkpoint double *weight1 = new double[size]; double *weight2 = new double[size]; double *weight = new double[size]; int *count1 = new int[size]; int *count2 = new int[size]; int *count = new int[size]; for(int i = 0; i < size; ++i){ count1[i] = 0; count2[i] = 0; count[i] = 0; } int state; start_state.reset(); for(int i = 0; i < stateSum;i++){ state = (int)start_state; for(int j = 0; j < size; j++) std::cout<<start_state[j]<<" "; std::cout<<TF[state]<<std::endl; if(TF[state] >= 1){ for(int j = 0; j < size; j++){ if(start_state[j] == 1){ weight1[j] += TF[state]; count1[j] += 1; } else{ weight2[j] -= TF[state]; count2[j] += 1; } weight[j] += TF[state]; count[j] += 1; } } start_state.nextSequence(); } std::cout<<"B = "<<bw.first<<" W = "<<bw.second<<std::endl; for(int j = 0; j < size; j++) std::cout<<(j+1)<<" "<<count1[j]<<" "<<count2[j]<<" "<< weight1[j]<<" "<<weight2[j]<<" "<<(weight1[j]+weight2[j])/weight[j]<<std::endl; delete weight1; delete weight2; delete weight; delete count1; delete count2; delete count; delete TF; }
/// extract the stack language from the CFG static void extract( cfg_cfg_type &cfg, nfa_nfa_type &nfa, bool partition, bool label_states ) throw() { char name_buff[1024] = {'\0'}; std::map<variable_context, nfa_state_type> state_map; std::map<cfg_variable_type, context_set> contexts; cfg_variable_type L, R; cfg_symbol_string_type syms; cfg_generator_type cfg_vars(cfg.search((~L) --->* ~syms)); // if we're not partitioning then the pre-emptively set the start // state to refer to the start variable of the grammar. if(!partition) { nfa_state_type start_state(nfa.get_start_state()); cfg_variable_type start_var(cfg.get_start_variable()); variable_context start_state_context; start_state_context.var = start_var; state_map[start_state_context] = start_state; if(label_states) { nfa.set_name(start_state, cfg.get_name(start_var)); } nfa.add_accept_state(start_state); } // go make all states for(; cfg_vars.match_next(); ) { for(unsigned i(0); i < syms.length(); ++i) { if(!syms.at(i).is_variable()) { continue; } R = syms.at(i); variable_context ctx_var; ctx_var.var = R; if(partition) { ctx_var.context = L; } nfa_state_type state; // create the state if(0U == state_map.count(ctx_var)) { io::verbose("Added state for %s to %s\n", cfg.get_name(L), cfg.get_name(R)); state = nfa.add_state(); state_map[ctx_var] = state; if(label_states) { if(partition) { sprintf(name_buff, "%s:%s", cfg.get_name(L), cfg.get_name(R)); nfa.set_name(state, name_buff); } else { nfa.set_name(state, cfg.get_name(R)); } } // we've already created this state } else { state = state_map[ctx_var]; } contexts[L].insert(ctx_var); } } // go add all transitions for(cfg_vars.rewind(); cfg_vars.match_next(); ) { for(unsigned i(0); i < syms.length(); ++i) { if(!syms.at(i).is_variable()) { continue; } R = syms.at(i); variable_context ctx_var; ctx_var.var = R; if(partition) { ctx_var.context = L; } context_set &related(contexts[R]); typename context_set::iterator it(related.begin()); for(; it != related.end(); ++it) { io::verbose("Added transition from %s to %s\n", cfg.get_name(R), cfg.get_name(it->var)); nfa_state_type from_state(state_map[ctx_var]); nfa_state_type to_state(state_map[*it]); nfa.add_transition( from_state, label_states ? nfa.epsilon() : nfa.get_symbol(cfg.get_name(it->var)), to_state ); } } // there are transitions from L to R, but there are no transitions // from L to R to something else. context_set &related(contexts[L]); if(!partition && 0U != related.size()) { variable_context ctx_var; ctx_var.var = L; nfa_state_type from_state(state_map[ctx_var]); typename context_set::iterator it(related.begin()); if(0U != nfa.num_transitions(from_state)) { continue; } for(; it != related.end(); ++it) { io::verbose("Added transition from %s to %s\n", cfg.get_name(R), cfg.get_name(it->var)); nfa_state_type to_state(state_map[*it]); nfa.add_transition( from_state, label_states ? nfa.epsilon() : nfa.get_symbol(cfg.get_name(it->var)), to_state ); } } } io::verbose("Adding in start state and transitions.\n"); // if we are partitioning, then we need to use the start state // to signal that parsing is being started if(partition) { nfa_state_type start_state(nfa.get_start_state()); cfg_variable_type start_var(cfg.get_start_variable()); nfa.set_start_state(start_state); nfa.add_accept_state(start_state); if(label_states) { sprintf(name_buff, ":%s", cfg.get_name(start_var)); nfa.set_name(start_state, name_buff); } std::set<variable_context> &related(contexts[start_var]); typename std::set<variable_context>::iterator it(related.begin()); for(; it != related.end(); ++it) { nfa_state_type to_state(state_map[*it]); nfa.add_transition( start_state, label_states ? nfa.epsilon() : nfa.get_symbol(cfg.get_name(it->var)), to_state ); } } }
lr_symbol* lr_parser::parse() { /* set up direct reference to tables to drive the parser */ production_tab = production_table(); action_tab = action_table(); reduce_tab = reduce_table(); /* initialize the action encapsulation object */ init_actions(); /* do user initialization */ user_init(); /* get the first token */ cur_token = scan(); /* push dummy symbol with start state to get us underway */ stack.remove_all_elements(); lr_symbol dummy_sym(0, start_state()); stack.push(&dummy_sym); /* continue until accept or fatal error */ while (true) { /* Check current token for freshness. */ assert(-1 == cur_token->parse_state); /* current state is always on the top of the stack */ /* look up action out of the current state with the current input */ int act = get_action(stack.peek()->parse_state, cur_token->sym); /* decode the action -- > 0 encodes shift */ if (act > 0) { act = act - 1; DEBUG_LOG("Shift and goto " << act); /* shift to the encoded state by pushing it on the stack */ cur_token->parse_state = act; stack.push(cur_token); /* advance to the next Symbol */ cur_token = scan(); } /* if its less than zero, then it encodes a reduce action */ else if (act < 0) { act = (-act) - 1; DEBUG_LOG("Reduce by rule " << act); /* perform the action for the reduce */ lr_symbol* lhs_sym = do_action(act); /* check for accept indication */ if (lhs_sym == 0) { return stack.peek(); } /* look up information about the production */ lhs_sym->sym = production_tab[act].lhs_sym; short handle_size = production_tab[act].rhs_size; /* pop the handle off the stack */ stack.npop(handle_size); /* look up the state to go to from the one popped back to */ act = get_reduce(stack.peek()->parse_state, lhs_sym->sym); /* shift to that state */ lhs_sym->parse_state = act; stack.push(lhs_sym); DEBUG_LOG(" and goto " << act); } /* finally if the entry is zero, we have an error */ else if (act == 0) { DEBUG_LOG("Error"); /* call user syntax error reporting routine */ syntax_error(cur_token); /* try to error recover */ switch (error_recovery()) { case ERS_FAIL: /* if that fails give up with a fatal syntax error */ unrecovered_syntax_error(cur_token); return 0; case ERS_SUCCESS: break; case ERS_ACCEPT: return stack.peek(); default: assert(0); } } } }
std::pair <std::vector<StateOfCar>, Seed> AStarSeed::findPathToTarget(const cv::Mat& map, const State& start, const State& goal, const int distance_transform, const int debug_current_state, int& status) { // USE : for guaranteed termination of planner int no_of_iterations = 0; int max_iterations = 10000; node_handle.getParam("local_planner/max_iterations", max_iterations); fusion_map = map; map_max_rows = map.rows; map_max_cols = map.cols; if (distance_transform == 1) { distanceTransform(); } if (debug_current_state) { image = fusion_map.clone(); } StateOfCar start_state(start), target_state(goal); std::map<StateOfCar, open_map_element> open_map; std::map<StateOfCar, StateOfCar, comparatorMapState> came_from; SS::PriorityQueue<StateOfCar> open_set; open_set.push(start_state); if (start_state.isCloseTo(target_state)) { status = 1; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if (isOnTheObstacle(start_state)) { std::cout << "Bot is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if (isOnTheObstacle(target_state)) { std::cout << "Target is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } while (!open_set.empty() && ros::ok()) { if (no_of_iterations > max_iterations) { status = open_set.size(); return std::make_pair(std::vector<StateOfCar>(), Seed()); } StateOfCar current_state = open_set.top(); if (open_map.find(current_state) != open_map.end() && open_map[current_state].membership == CLOSED) { open_set.pop(); } current_state = open_set.top(); if (debug_current_state) { std::cout << "current x : " << current_state.x() << " current y : " << current_state.y() << std::endl; plotPointInMap(current_state); cv::imshow("[PLANNER] Map", image); cvWaitKey(0); } // TODO : use closeTo instead of onTarget if (current_state.isCloseTo(target_state)) { status = 2; return reconstructPath(current_state, came_from); } open_set.pop(); open_map[current_state].membership = UNASSIGNED; open_map[current_state].cost = -current_state.gCost(); open_map[current_state].membership = CLOSED; std::vector<StateOfCar> neighbors_of_current_state = neighborNodesWithSeeds(current_state); for (unsigned int iterator = 0; iterator < neighbors_of_current_state.size(); iterator++) { StateOfCar neighbor = neighbors_of_current_state[iterator]; double tentative_gcost_along_followed_path = neighbor.gCost() + current_state.gCost(); double admissible = neighbor.distanceTo(target_state); double consistent = admissible; if (!((open_map.find(neighbor) != open_map.end()) && (open_map[neighbor].membership == OPEN))) { came_from[neighbor] = current_state; neighbor.hCost(consistent); neighbor.gCost(tentative_gcost_along_followed_path); neighbor.updateTotalCost(); open_set.push(neighbor); open_map[neighbor].membership = OPEN; open_map[neighbor].cost = neighbor.gCost(); } } no_of_iterations++; } status = 0; return std::make_pair(std::vector<StateOfCar>(), Seed()); }
/** Solves the motion planning problem */ std::vector<std::vector<double> > PathPlanner::solve(const std::vector<double> &start_state_vec, double timeout) { std::vector<double> ss_vec; ompl::base::ScopedState<> start_state(space_); for (size_t i =0; i < dim_; i++) { ss_vec.push_back(start_state_vec[i]); start_state[i] = ss_vec[i]; } std::vector<std::vector<double> > solution_vector; boost::shared_ptr<MotionValidator> mv = boost::static_pointer_cast<MotionValidator>(si_->getMotionValidator()); /** Add the start state to the problem definition */ if (verbose_) { cout << "Adding start state: "; for (size_t i = 0; i < dim_; i++) { cout << start_state[i] << ", "; } cout << endl; } /**if (!isValid(start_state.get())) { cout << "Path planner: ERROR: Start state not valid!" << endl; return solution_vector; }*/ problem_definition_->addStartState(start_state); ompl::base::GoalPtr gp(new ManipulatorGoalRegion(si_, robot_, goal_states_, ee_goal_position_, ee_goal_threshold_, false)); boost::dynamic_pointer_cast<ompl::base::GoalRegion>(gp)->setThreshold(ee_goal_threshold_); problem_definition_->setGoal(gp); if (check_linear_path_) { bool collides = false; std::vector<double> goal_state_vec = boost::dynamic_pointer_cast<ManipulatorGoalRegion>(gp)->sampleGoalVec(); std::vector<std::vector<double> > linear_path(genLinearPath(ss_vec, goal_state_vec)); for (size_t i = 1; i < linear_path.size(); i++) { if (!(mv->checkMotion(linear_path[i - 1], linear_path[i], continuous_collision_))) { collides = true; break; } } if (!collides) { clear(); if (verbose_) { cout << "Linear path is a valid solution. Returning linear path of length " << linear_path.size() << endl; } return augmentPath_(linear_path); } } /** Solve the planning problem with a maximum of *timeout* seconds per attempt */ bool solved = false; boost::timer t; bool approximate_solution = true; /**while (!solved || approximate_solution) { solved = planner_->solve(4.0); //solved = planner_->solve(10.0); if (t.elapsed() > timeout) { cout << "Timeout reached." << endl; return solution_vector; } for (auto &solution : problem_definition_->getSolutions()) { cout << "diff " << solution.difference_ << endl; approximate_solution = false; } }**/ solved = solve_(timeout); if (!solved) { return solution_vector; } boost::shared_ptr<ompl::geometric::PathGeometric> solution_path = boost::static_pointer_cast<ompl::geometric::PathGeometric>(problem_definition_->getSolutionPath()); solution_vector.push_back(ss_vec); /** We found a solution, so get the solution path */ if (verbose_) { cout << "Solution found in " << t.elapsed() << " seconds." << endl; cout << "Solution path has length " << solution_path->getStates().size() << endl; } std::vector<double> vals; const bool cont_check = true; for (size_t i=1; i<solution_path->getStates().size(); i++) { vals.clear(); for (unsigned int j = 0; j < dim_; j++) { //vals.push_back(solution_path->getState(i)->as<ompl::base::RealVectorStateSpace::StateType>()->values[j]); vals.push_back(solution_path->getState(i)->as<shared::RealVectorStateSpace::StateType>()->values[j]); } solution_vector.push_back(vals); } clear(); return augmentPath_(solution_vector); }
bool SSLPathPlanner::doPlanForRobot (const int& id/*, bool& is_stop*/) { // is_send_plan = true; tmp_robot_id_queried = id; State* tmp_start = manifold->allocState (); tmp_start->as<RealVectorStateManifold::StateType> ()->values[0] = team_state_[id].pose.x; tmp_start->as<RealVectorStateManifold::StateType> ()->values[1] = team_state_[id].pose.y; PathGeometric direct_path (planner_setup->getSpaceInformation ()); direct_path.states.push_back (manifold->allocState ()); manifold->copyState (direct_path.states[0], tmp_start); State* tmp_goal = manifold->allocState (); tmp_goal->as<RealVectorStateManifold::StateType> ()->values[0] = pose_control.pose[id].pose.x; tmp_goal->as<RealVectorStateManifold::StateType> ()->values[1] = pose_control.pose[id].pose.y; // std::cout<<pose_control.pose[id].pose.x<<"\t"<<pose_control.pose[id].pose.y<<std::endl; direct_path.states.push_back (manifold->allocState ()); manifold->copyState (direct_path.states[1], tmp_goal); Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y); Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y); //too close to the target point, no need for planning, just take it as a next_target_pose if (sqrt (getSquaredDistance (p_start, p_goal)) < VERY_CRITICAL_DIST) { next_target_poses[id].x = pose_control.pose[id].pose.x; next_target_poses[id].y = pose_control.pose[id].pose.y; next_target_poses[id].theta = pose_control.pose[id].pose.theta; return true; } // manifold->printState (direct_path.states[0], std::cout); // manifold->printState (direct_path.states[1], std::cout); //direct path is available, no need for planning // if (direct_path.check ()) if (!doesIntersectObstacles (id, p_start, p_goal)) { // std::cout << "direct path is AVAILABLE for robot " << id << std::endl; if (solution_data_for_robots[id].size () > direct_path.states.size ()) { for (uint32_t i = direct_path.states.size (); i < solution_data_for_robots[id].size (); i++) manifold->freeState (solution_data_for_robots[id][i]); solution_data_for_robots[id].resize (direct_path.states.size ()); } else if (solution_data_for_robots[id].size () < direct_path.states.size ()) { for (uint32_t i = solution_data_for_robots[id].size (); i < direct_path.states.size (); i++) solution_data_for_robots[id].push_back (manifold->allocState ()); } for (uint32_t i = 0; i < direct_path.states.size (); i++) manifold->copyState (solution_data_for_robots[id][i], direct_path.states[i]); manifold->freeState (tmp_start); manifold->freeState (tmp_goal); next_target_poses[id].x = pose_control.pose[id].pose.x; next_target_poses[id].y = pose_control.pose[id].pose.y; next_target_poses[id].theta = pose_control.pose[id].pose.theta; // std::cout<<next_target_poses[id].x<<"\t"<<next_target_poses[id].y<<std::endl; return true; } manifold->freeState (tmp_start); manifold->freeState (tmp_goal); // std::cout << "direct path is NOT AVAILABLE for robot " << id << ", doing planning" << std::endl; //direct path is not available, DO PLAN if the earlier plan is invalidated! //earlier plan is still valid if (checkPlanForRobot (id)) return true; else if (is_plan_done[id]) return false; //earlier plan is not valid anymore, replan planner_setup->clear (); planner_setup->clearStartStates (); ScopedState<RealVectorStateManifold> start_state (manifold); ScopedState<RealVectorStateManifold> goal_state (manifold); start_state->values[0] = team_state_[id].pose.x; start_state->values[1] = team_state_[id].pose.y; goal_state->values[0] = pose_control.pose[id].pose.x; goal_state->values[1] = pose_control.pose[id].pose.y; planner_setup->setStartAndGoalStates (start_state, goal_state); planner_setup->getProblemDefinition ()->fixInvalidInputStates (ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, 100); bool solved = planner_setup->solve (0.100);//100msec if (solved) { planner_setup->simplifySolution (); PathGeometric* path; path = &planner_setup->getSolutionPath (); // PathSimplifier p (planner_setup->getSpaceInformation ()); // p.reduceVertices (*path, 1000); if (solution_data_for_robots[id].size () < path->states.size ()) { for (unsigned int i = solution_data_for_robots[id].size (); i < path->states.size (); i++) solution_data_for_robots[id].push_back (manifold->allocState ()); } else if (solution_data_for_robots[id].size () > path->states.size ()) { for (unsigned int i = path->states.size (); i < solution_data_for_robots[id].size (); i++) manifold->freeState (solution_data_for_robots[id][i]); //drop last elements that are already being freed solution_data_for_robots[id].resize (path->states.size ()); } for (unsigned int i = 0; i < path->states.size (); i++) manifold->copyState (solution_data_for_robots[id][i], path->states[i]); //leader-follower approach based segment enlargement //pick next location Point_2 curr_point (path->states[0]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[0]->as< RealVectorStateManifold::StateType> ()->values[1]); for (uint32_t i = 1; i < path->states.size (); i++) { Point_2 next_point (path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[i]->as< RealVectorStateManifold::StateType> ()->values[1]); if (!doesIntersectObstacles (id, curr_point, next_point)) { next_target_poses[id].x = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0]; next_target_poses[id].y = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[1]; next_target_poses[id].theta = pose_control.pose[id].pose.theta; } else break; } // next_target_poses[id].x = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[0]; // next_target_poses[id].y = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[1]; // planner_data_for_robots[id].clear (); // planner_data_for_robots[id] = planner_setup->getPlannerData (); } return solved; }
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params, moveit_msgs::MotionPlanDetailedResponse& res) const { if (!planning_scene) { ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized."); res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; return false; } if (req.start_state.joint_state.position.empty()) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty"); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; return false; } if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data())) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits"); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; return false; } ros::WallTime start_time = ros::WallTime::now(); ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name); jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name, trajectory.getTrajectoryPoint(0)); if (req.goal_constraints.empty()) { ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!"); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; } if (req.goal_constraints[0].joint_constraints.empty()) { ROS_ERROR_STREAM("Only joint-space goals are supported"); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; return false; } int goal_index = trajectory.getNumPoints() - 1; trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0); sensor_msgs::JointState js; for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints) { js.name.push_back(joint_constraint.joint_name); js.position.push_back(joint_constraint.position); ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position " << joint_constraint.position); } jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index)); const moveit::core::JointModelGroup* model_group = planning_scene->getRobotModel()->getJointModelGroup(req.group_name); // fix the goal to move the shortest angular distance for wrap-around joints: for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++) { const moveit::core::JointModel* model = model_group->getActiveJointModels()[i]; const moveit::core::RevoluteJointModel* revolute_joint = dynamic_cast<const moveit::core::RevoluteJointModel*>(model); if (revolute_joint != nullptr) { if (revolute_joint->isContinuous()) { double start = (trajectory)(0, i); double end = (trajectory)(goal_index, i); ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end)); (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end); } } } const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames(); const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index); moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState(); goal_robot_state.setVariablePositions( active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size())); if (not goal_robot_state.satisfiesBounds()) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits"); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; return false; } // fill in an initial trajectory based on user choice from the chomp_config.yaml file if (params.trajectory_initialization_method_.compare("quintic-spline") == 0) trajectory.fillInMinJerk(); else if (params.trajectory_initialization_method_.compare("linear") == 0) trajectory.fillInLinearInterpolation(); else if (params.trajectory_initialization_method_.compare("cubic") == 0) trajectory.fillInCubicInterpolation(); else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0) { if (!(trajectory.fillInFromTrajectory(res))) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, " "trajectory must contain at least start and goal state"); return false; } } else ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file"); ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ", (params.trajectory_initialization_method_).c_str()); // optimize! moveit::core::RobotState start_state(planning_scene->getCurrentState()); moveit::core::robotStateMsgToRobotState(req.start_state, start_state); start_state.update(); ros::WallTime create_time = ros::WallTime::now(); int replan_count = 0; bool replan_flag = false; double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10; int org_max_iterations = 200; // storing the initial chomp parameters values org_learning_rate = params.learning_rate_; org_ridge_factor = params.ridge_factor_; org_planning_time_limit = params.planning_time_limit_; org_max_iterations = params.max_iterations_; std::unique_ptr<ChompOptimizer> optimizer; // create a non_const_params variable which stores the non constant version of the const params variable ChompParameters params_nonconst = params; // while loop for replanning (recovery behaviour) if collision free optimized solution not found while (true) { if (replan_flag) { // increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5 // additional secs in hope to find a solution; increase maximum iterations params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002, params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50); } // initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in // case of a recovery behaviour optimizer.reset(new ChompOptimizer(&trajectory, planning_scene, req.group_name, ¶ms_nonconst, start_state)); if (!optimizer->isInitialized()) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer"); res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec()); bool optimization_result = optimizer->optimize(); // replan with updated parameters if no solution is found if (params_nonconst.enable_failure_recovery_) { ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, " "planning_time_limit, max_iterations), attempt: # %d ", (replan_count + 1)); ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ", params_nonconst.learning_rate_, params_nonconst.ridge_factor_, params_nonconst.planning_time_limit_, params_nonconst.max_iterations_); if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_) { replan_count++; replan_flag = true; } else { break; } } else break; } // end of while loop // resetting the CHOMP Parameters to the original values after a successful plan params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations); ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec()); create_time = ros::WallTime::now(); // assume that the trajectory is now optimized, fill in the output structure: ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints()); res.trajectory.resize(1); res.trajectory[0].joint_trajectory.joint_names = active_joint_names; res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; // @TODO this is probably a hack // fill in the entire trajectory res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints()); for (int i = 0; i < trajectory.getNumPoints(); i++) { res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints()); for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++) { res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j); } // Setting invalid timestamps. // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0); } ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec()); ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec()); res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); // report planning failure if path has collisions if (not optimizer->isCollisionFree()) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid."); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } // check that final state is within goal tolerances kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel()); robot_state::RobotState last_state(start_state); last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names, res.trajectory[0].joint_trajectory.points.back().positions); bool constraints_are_ok = true; for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints) { constraints_are_ok = constraints_are_ok and jc.configure(constraint); constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied; if (not constraints_are_ok) { ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name); res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; return false; } } return true; }
bool RobotMoveit::executeCarteGoalWithConstraint(RobotMoveit::WhichArm arm, geometry_msgs::Pose target) { moveit::planning_interface::MoveGroup* group; if(!selectArmGroup(arm,&group)) { return false; } std::string arm_name; switch(arm) { case RobotMoveit::LEFT: arm_name = "left"; break; case RobotMoveit::RIGHT: arm_name = "right"; break; default: ROS_ERROR("Received faulty arm argument!"); return false; } moveit_msgs::OrientationConstraint ocm; ocm.link_name = arm_name+"_gripper";//"_lower_forearm"; ocm.header.frame_id = "base";//"torso"; // ocm.orientation.x = -0.6768 ; // ocm.orientation.y = -0.2047 ; // ocm.orientation.z = -0.2047; ocm.orientation.w = 1.0; ocm.absolute_x_axis_tolerance = 0.1; ocm.absolute_y_axis_tolerance = 0.1; ocm.absolute_z_axis_tolerance = 0.1; ocm.weight = 1.0; moveit_msgs::Constraints test_constraints; test_constraints.orientation_constraints.push_back(ocm); group->setPathConstraints(test_constraints); // will only work if the current state already satisfies the path constraints robot_state::RobotState start_state(*group->getCurrentState()); geometry_msgs::Pose start_pose2; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55; start_pose2.position.y = -0.05; start_pose2.position.z = 0.8; // const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group->getName()); // start_state.setFromIK(joint_model_group, start_pose2); group->setStartState(start_state); group->setPoseTarget(target); bool success = group->plan(my_plan); ROS_INFO("Planning Cartesian goal %s",success?"SUCCEEDED":"FAILED"); // TODO: Publish to RVIZ // ROS_INFO("Visualizing plan 1 (again)"); // display_trajectory.trajectory_start = my_plan.start_state_; // display_trajectory.trajectory.push_back(my_plan.trajectory_); // display_publisher.publish(display_trajectory); // /* Sleep to give Rviz time to visualize the plan. */ // sleep(5.0); if(success) { group->move(); group->clearPathConstraints(); return true; } group->clearPathConstraints(); return false; }
int main(int argc, char **argv) { ros::init(argc, argv, "move_group_interface_tutorial"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start(); /* This sleep is ONLY to allow Rviz to come up */ sleep(20.0); // BEGIN_TUTORIAL // // Setup // ^^^^^ // // The :move_group_interface:`MoveGroup` class can be easily // setup using just the name // of the group you would like to control and plan for. moveit::planning_interface::MoveGroup group("right_arm"); // We will use the :planning_scene_interface:`PlanningSceneInterface` // class to deal directly with the world. moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // (Optional) Create a publisher for visualizing plans in Rviz. ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; // Getting Basic Information // ^^^^^^^^^^^^^^^^^^^^^^^^^ // // We can print the name of the reference frame for this robot. ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str()); // We can also print the name of the end-effector link for this group. ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str()); // Planning to a Pose goal // ^^^^^^^^^^^^^^^^^^^^^^^ // We can plan a motion for this group to a desired pose for the // end-effector. geometry_msgs::Pose target_pose1; target_pose1.orientation.w = 1.0; target_pose1.position.x = 0.28; target_pose1.position.y = -0.7; target_pose1.position.z = 1.0; group.setPoseTarget(target_pose1); // Now, we call the planner to compute the plan // and visualize it. // Note that we are just planning, not asking move_group // to actually move the robot. moveit::planning_interface::MoveGroup::Plan my_plan; bool success = group.plan(my_plan); ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); // Visualizing plans // ^^^^^^^^^^^^^^^^^ // Now that we have a plan we can visualize it in Rviz. This is not // necessary because the group.plan() call we made above did this // automatically. But explicitly publishing plans is useful in cases that we // want to visualize a previously created plan. if (1) { ROS_INFO("Visualizing plan 1 (again)"); display_trajectory.trajectory_start = my_plan.start_state_; display_trajectory.trajectory.push_back(my_plan.trajectory_); display_publisher.publish(display_trajectory); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); } // Moving to a pose goal // ^^^^^^^^^^^^^^^^^^^^^ // // Moving to a pose goal is similar to the step above // except we now use the move() function. Note that // the pose goal we had set earlier is still active // and so the robot will try to move to that goal. We will // not use that function in this tutorial since it is // a blocking function and requires a controller to be active // and report success on execution of a trajectory. /* Uncomment below line when working with a real robot*/ /* group.move() */ // Planning to a joint-space goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Let's set a joint space goal and move towards it. This will replace the // pose target we set above. // // First get the current set of joint values for the group. std::vector<double> group_variable_values; group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values); // Now, let's modify one of the joints, plan to the new joint // space goal and visualize the plan. group_variable_values[0] = -1.0; group.setJointValueTarget(group_variable_values); success = group.plan(my_plan); ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(5.0); // Planning with Path Constraints // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Path constraints can easily be specified for a link on the robot. // Let's specify a path constraint and a pose goal for our group. // First define the path constraint. moveit_msgs::OrientationConstraint ocm; ocm.link_name = "r_wrist_roll_link"; ocm.header.frame_id = "base_link"; ocm.orientation.w = 1.0; ocm.absolute_x_axis_tolerance = 0.1; ocm.absolute_y_axis_tolerance = 0.1; ocm.absolute_z_axis_tolerance = 0.1; ocm.weight = 1.0; // Now, set it as the path constraint for the group. moveit_msgs::Constraints test_constraints; test_constraints.orientation_constraints.push_back(ocm); group.setPathConstraints(test_constraints); // We will reuse the old goal that we had and plan to it. // Note that this will only work if the current state already // satisfies the path constraints. So, we need to set the start // state to a new pose. robot_state::RobotState start_state(*group.getCurrentState()); geometry_msgs::Pose start_pose2; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55; start_pose2.position.y = -0.05; start_pose2.position.z = 0.8; const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group.getName()); start_state.setFromIK(joint_model_group, start_pose2); group.setStartState(start_state); // Now we will plan to the earlier pose target from the new // start state that we have just created. group.setPoseTarget(target_pose1); success = group.plan(my_plan); ROS_INFO("Visualizing plan 3 (constraints) %s",success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(10.0); // When done with the path constraint be sure to clear it. group.clearPathConstraints(); // Cartesian Paths // ^^^^^^^^^^^^^^^ // You can plan a cartesian path directly by specifying a list of waypoints // for the end-effector to go through. Note that we are starting // from the new start state above. The initial pose (start state) does not // need to be added to the waypoint list. std::vector<geometry_msgs::Pose> waypoints; geometry_msgs::Pose target_pose3 = start_pose2; target_pose3.position.x += 0.2; target_pose3.position.z += 0.2; waypoints.push_back(target_pose3); // up and out target_pose3.position.y -= 0.2; waypoints.push_back(target_pose3); // left target_pose3.position.z -= 0.2; target_pose3.position.y += 0.2; target_pose3.position.x -= 0.2; waypoints.push_back(target_pose3); // down and right (back to start) // We want the cartesian path to be interpolated at a resolution of 1 cm // which is why we will specify 0.01 as the max step in cartesian // translation. We will specify the jump threshold as 0.0, effectively // disabling it. moveit_msgs::RobotTrajectory trajectory; double fraction = group.computeCartesianPath(waypoints, 0.01, // eef_step 0.0, // jump_threshold trajectory); ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)", fraction * 100.0); /* Sleep to give Rviz time to visualize the plan. */ sleep(15.0); // Adding/Removing Objects and Attaching/Detaching Objects // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // First, we will define the collision object message. moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = group.getPlanningFrame(); /* The id of the object is used to identify it. */ collision_object.id = "box1"; /* Define a box to add to the world. */ shape_msgs::SolidPrimitive primitive; primitive.type = primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] = 0.4; primitive.dimensions[1] = 0.1; primitive.dimensions[2] = 0.4; /* A pose for the box (specified relative to frame_id) */ geometry_msgs::Pose box_pose; box_pose.orientation.w = 1.0; box_pose.position.x = 0.6; box_pose.position.y = -0.4; box_pose.position.z = 1.2; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation = collision_object.ADD; std::vector<moveit_msgs::CollisionObject> collision_objects; collision_objects.push_back(collision_object); // Now, let's add the collision object into the world ROS_INFO("Add an object into the world"); planning_scene_interface.addCollisionObjects(collision_objects); /* Sleep so we have time to see the object in RViz */ sleep(2.0); // Planning with collision detection can be slow. Lets set the planning time // to be sure the planner has enough time to plan around the box. 10 seconds // should be plenty. group.setPlanningTime(10.0); // Now when we plan a trajectory it will avoid the obstacle group.setStartState(*group.getCurrentState()); group.setPoseTarget(target_pose1); success = group.plan(my_plan); ROS_INFO("Visualizing plan 5 (pose goal move around box) %s", success?"":"FAILED"); /* Sleep to give Rviz time to visualize the plan. */ sleep(10.0); // Now, let's attach the collision object to the robot. ROS_INFO("Attach the object to the robot"); group.attachObject(collision_object.id); /* Sleep to give Rviz time to show the object attached (different color). */ sleep(4.0); // Now, let's detach the collision object from the robot. ROS_INFO("Detach the object from the robot"); group.detachObject(collision_object.id); /* Sleep to give Rviz time to show the object detached. */ sleep(4.0); // Now, let's remove the collision object from the world. ROS_INFO("Remove the object from the world"); std::vector<std::string> object_ids; object_ids.push_back(collision_object.id); planning_scene_interface.removeCollisionObjects(object_ids); /* Sleep to give Rviz time to show the object is no longer there. */ sleep(4.0); // Dual-arm pose goals // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // First define a new group for addressing the two arms. Then define // two separate pose goals, one for each end-effector. Note that // we are reusing the goal for the right arm above moveit::planning_interface::MoveGroup two_arms_group("arms"); two_arms_group.setPoseTarget(target_pose1, "r_wrist_roll_link"); geometry_msgs::Pose target_pose2; target_pose2.orientation.w = 1.0; target_pose2.position.x = 0.7; target_pose2.position.y = 0.15; target_pose2.position.z = 1.0; two_arms_group.setPoseTarget(target_pose2, "l_wrist_roll_link"); // Now, we can plan and visualize moveit::planning_interface::MoveGroup::Plan two_arms_plan; two_arms_group.plan(two_arms_plan); sleep(4.0); // END_TUTORIAL ros::shutdown(); return 0; }
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::GetMotionPlan::Request &req, const chomp::ChompParameters& params, moveit_msgs::GetMotionPlan::Response &res) const { ros::WallTime start_time = ros::WallTime::now(); ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.motion_plan_request.group_name); jointStateToArray(planning_scene->getRobotModel(), req.motion_plan_request.start_state.joint_state, req.motion_plan_request.group_name, trajectory.getTrajectoryPoint(0)); int goal_index = trajectory.getNumPoints()- 1; trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0); sensor_msgs::JointState js; for(unsigned int i = 0; i < req.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) { js.name.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name); js.position.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].position); ROS_INFO_STREAM("Setting joint " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name << " to position " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].position); } jointStateToArray(planning_scene->getRobotModel(), js, req.motion_plan_request.group_name, trajectory.getTrajectoryPoint(goal_index)); const planning_models::RobotModel::JointModelGroup* model_group = planning_scene->getRobotModel()->getJointModelGroup(req.motion_plan_request.group_name); // fix the goal to move the shortest angular distance for wrap-around joints: for (size_t i = 0; i < model_group->getJointModels().size(); i++) { const planning_models::RobotModel::JointModel* model = model_group->getJointModels()[i]; const planning_models::RobotModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::RobotModel::RevoluteJointModel*>(model); if (revolute_joint != NULL) { if(revolute_joint->isContinuous()) { double start = (trajectory)(0, i); double end = (trajectory)(goal_index, i); ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end)); (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end); } } } // fill in an initial quintic spline trajectory trajectory.fillInMinJerk(); // optimize! planning_models::RobotState *start_state(planning_scene->getCurrentState()); planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state); ros::WallTime create_time = ros::WallTime::now(); ChompOptimizer optimizer(&trajectory, planning_scene, req.motion_plan_request.group_name, ¶ms, start_state); if(!optimizer.isInitialized()) { ROS_WARN_STREAM("Could not initialize optimizer"); res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec()); ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec()); optimizer.optimize(); ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec()); create_time = ros::WallTime::now(); // assume that the trajectory is now optimized, fill in the output structure: ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints()); // fill in joint names: res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints()); for (size_t i = 0; i < model_group->getJointModels().size(); i++) { res.trajectory.joint_trajectory.joint_names[i] = model_group->getJointModels()[i]->getName(); } res.trajectory.joint_trajectory.header = req.motion_plan_request.start_state.joint_state.header; // @TODO this is probably a hack // fill in the entire trajectory res.trajectory.joint_trajectory.points.resize(trajectory.getNumPoints()); for (int i=0; i < trajectory.getNumPoints(); i++) { res.trajectory.joint_trajectory.points[i].positions.resize(trajectory.getNumJoints()); for (size_t j=0; j < res.trajectory.joint_trajectory.points[i].positions.size(); j++) { res.trajectory.joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j); if(i == trajectory.getNumPoints()-1) { ROS_INFO_STREAM("Joint " << j << " " << res.trajectory.joint_trajectory.points[i].positions[j]); } } // Setting invalid timestamps. // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. res.trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0); } ROS_INFO("Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec()); ROS_INFO("Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.joint_trajectory.points[goal_index].time_from_start.toSec()); res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec()); return true; }
//calculate the delta-B & delta-D for a given network double* calBD(int *network,int size) { auto stateNetwork = genDN(network,size); int stateSum = pow(2,size); Sequence start_state(size,2); double *TF_B = new double [stateSum]; double *TF_D = new double [stateSum]; //initialization for (int i = 0; i < stateSum; ++i) { TF_B[i] = 0; TF_D[i] = 0; } calDeltaBRec(stateNetwork,76,TF_B); //!!!!! calDeltaDRec(stateNetwork,76,TF_D); //statistics the checkpoint double *weight_b1 = new double[size]; double *weight_b2 = new double[size]; double *weight_b = new double[size]; double *weight_d1 = new double[size]; double *weight_d2 = new double[size]; double *weight_d = new double[size]; //initialization for (int i = 0; i < size; ++i) { weight_b1[i] = 0; weight_b2[i] = 0; weight_b[i] = 0; weight_d1[i] = 0; weight_d2[i] = 0; weight_d[i] = 0; } int state; //calculate the b start_state.reset(); for(int i = 0; i < stateSum;i++){ state = (int)start_state; if(TF_B[state] >= 1){ for(int j = 0; j < size; j++){ if(start_state[j] == 1) weight_b1[j] += TF_B[state]; else weight_b2[j] -= TF_B[state]; weight_b[j] += TF_B[state]; } } start_state.nextSequence(); } //calculate the d start_state.reset(); for(int i = 0; i < stateSum;i++){ state = (int)start_state; if(TF_D[state] >= 1){ for(int j = 0; j < size; j++){ if(start_state[j] == 1) weight_d1[j] += TF_D[state]; else weight_d2[j] -= TF_D[state]; weight_d[j] += TF_D[state]; } } start_state.nextSequence(); } // for(int j = 0; j < size; j++) // std::cout<<(j+1)<<" "<<(weight_b1[j]+weight_b2[j])/weight_b[j]<< // " "<<(weight_d1[j]+weight_d2[j])/weight_d[j]<<std::endl; double *bd = new double [size]; for(int j = 0; j < size; ++j){ bd[j] = ((weight_b1[j]+weight_b2[j])/weight_b[j]+ (weight_d1[j]+weight_d2[j])/weight_d[j])/2; } // for(int i = 0; i < size; ++i){ // std::cout<<" "<<bd[i]<<" "<<std::endl; // } delete weight_b1; delete weight_b2; delete weight_b; delete weight_d1; delete weight_d2; delete weight_d; delete TF_B; delete TF_D; return bd; }
int main(int argc, char **argv){ ros::init (argc, argv, "circ_trajectory_viz_execution"); ros::NodeHandle n; ros::AsyncSpinner spinner(1); spinner.start(); ros::Publisher rvizMarkerPub; rvizMarkerPub = n.advertise < visualization_msgs::Marker > ("visualization_marker", 1000); moveit::planning_interface::MoveGroup group("right_arm"); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // (Optional) Create a publisher for visualizing plans in Rviz. //ros::Publisher display_publisher = n.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); //moveit_msgs::DisplayTrajectory display_trajectory; // Getting Basic Information // ^^^^^^^^^^^^^^^^^^^^^^^^^ // // We can print the name of the reference frame for this robot. ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str()); // We can also print the name of the end-effector link for this group. ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str()); // Move the robot to a known starting position robot_state::RobotState start_state(*group.getCurrentState()); geometry_msgs::Pose start_pose2; start_pose2.orientation.x = 1.0; start_pose2.orientation.y = 0.0; start_pose2.orientation.z = 0.0; start_pose2.orientation.w = 1.0; start_pose2.position.x = 0.55;//0.55; start_pose2.position.y = -0.55;//-0.05; start_pose2.position.z = 0.8;//0.8; const robot_state::JointModelGroup *joint_model_group = start_state.getJointModelGroup(group.getName()); start_state.setFromIK(joint_model_group, start_pose2); group.setStartState(start_state); // Now we will plan to the earlier pose target from the new // start state that we have just created. group.setPoseTarget(start_pose2); moveit::planning_interface::MoveGroup::Plan my_plan; bool success = group.plan(my_plan); ROS_INFO("Moving to start position %s",success?"":"FAILED"); group.move(); /* Sleep to give Rviz time to visualize the plan. */ sleep(10.0); // When done with the path constraint be sure to clear it. group.clearPathConstraints(); ROS_INFO("Opening Bag"); rosbag::Bag bag; bag.open("/home/stevenjj/catkin_ws/src/hw3/circular_forward_trajectory.bag", rosbag::bagmode::Read); std::vector<std::string> topics; topics.push_back(std::string("visualization_marker")); //Specify topic to read rosbag::View view(bag, rosbag::TopicQuery(topics)); // Define Reference quaternion to be the x-axis. //tf::Quaternion main_axis(1,0,0, 1); tf::Vector3 r_gripper_position(start_pose2.position.x, start_pose2.position.y, start_pose2.position.z); tf::Quaternion main_axis(start_pose2.orientation.x, start_pose2.orientation.y, start_pose2.orientation.z, start_pose2.orientation.w); main_axis = main_axis.normalize(); // Define the position and orientation of the first marker tf::Vector3 first_marker_vector_offset; tf::Quaternion first_marker_axis; // Define Quaternion Rotation Offset tf::Quaternion axis_rotation; int marker_index = 0; // Count total number of markers // Identify the first marker and use its position and orientation to identify the first rotation int total_markers = 0; foreach(rosbag::MessageInstance const m, view){ if (total_markers == 0){ visualization_msgs::Marker::ConstPtr first_marker = m.instantiate<visualization_msgs::Marker>(); first_marker_vector_offset.setX(first_marker->pose.position.x); first_marker_vector_offset.setY(first_marker->pose.position.y); first_marker_vector_offset.setZ(first_marker->pose.position.z); tf::Quaternion store_axis (first_marker->pose.orientation.x, first_marker->pose.orientation.y, first_marker->pose.orientation.z, first_marker->pose.orientation.w); first_marker_axis = store_axis; } total_markers++; // count total number of markers in the rosbag } // Fancy Quaternion math. This took forever. p' = p_a^-1 * p2 gives the rotation needed to go from p_a to p2. axis_rotation = first_marker_axis.inverse() * main_axis; // Find axis of rotation tf::Transform transform_to_main_axis(tf::Quaternion(0,0,0,1), r_gripper_position - first_marker_vector_offset); // Create transform tf::Transform rotate_to_main_axis(axis_rotation, tf::Vector3(0,0,0)); // Create transform foreach(rosbag::MessageInstance const m, view){ //std::cout << m.getTopic() << std::endl; ROS_INFO("Inside bag!"); //geometry_msgs::Pose::ConstPtr p = m.instantiate<geometry_msgs::Pose>(); visualization_msgs::Marker::ConstPtr p = m.instantiate<visualization_msgs::Marker>(); //std::cout << m.getDataType() << std::endl; // Identify topic type //std::cout << p->pose.position.x << std::endl; pub_recorded_marker(rvizMarkerPub, p, marker_index, total_markers, transform_to_main_axis, rotate_to_main_axis); marker_index++; //sleep(1.0); std::cout << total_markers << std::endl; }
bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, const moveit_msgs::GetMotionPlan::Request &req, moveit_msgs::GetMotionPlan::Response &res, const PlanningParameters& params) const { res.trajectory.joint_trajectory.points.clear(); (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = PlanningStatistics(); planning_models::RobotState *start_state(planning_scene->getCurrentState()); planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state); ros::WallTime wt = ros::WallTime::now(); boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene)); if(!env_chain->setupForMotionPlan(planning_scene, req, res, params)) { //std::cerr << "Env chain setup failing" << std::endl; return false; } //std::cerr << "Creation with params " << params.use_bfs_ << " took " << (ros::WallTime::now()-wt).toSec() << std::endl; boost::this_thread::interruption_point(); //DummyEnvironment* dummy_env = new DummyEnvironment(); boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true)); planner->set_initialsolution_eps(100.0); planner->set_search_mode(true); planner->force_planning_from_scratch(); planner->set_start(env_chain->getPlanningData().start_hash_entry_->stateID); planner->set_goal(env_chain->getPlanningData().goal_hash_entry_->stateID); //std::cerr << "Creation took " << (ros::WallTime::now()-wt) << std::endl; std::vector<int> solution_state_ids; int solution_cost; wt = ros::WallTime::now(); //CALLGRIND_START_INSTRUMENTATION; bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost); //CALLGRIND_STOP_INSTRUMENTATION; double el = (ros::WallTime::now()-wt).toSec(); std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl; std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_ << " average time " << (env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0)) << " hz " << 1.0/(env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0)) << std::endl; std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz " << 1.0/(env_chain->getPlanningStatistics().total_coll_check_time_.toSec()/(env_chain->getPlanningStatistics().coll_checks_*1.0)) << std::endl; std::cerr << "Path length is " << solution_state_ids.size() << std::endl; if(!b_ret) { res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED; return false; } if(solution_state_ids.size() == 0) { std::cerr << "Success but no path" << std::endl; res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } if(!env_chain->populateTrajectoryFromStateIDSequence(solution_state_ids, res.trajectory.joint_trajectory)) { std::cerr << "Success but path bad" << std::endl; res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } ros::WallTime pre_short = ros::WallTime::now(); //std::cerr << "Num traj points before " << res.trajectory.joint_trajectory.points.size() << std::endl; trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory; env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory); //std::cerr << "Num traj points after " << res.trajectory.joint_trajectory.points.size() << std::endl; //std::cerr << "Time " << (ros::WallTime::now()-pre_short).toSec() << std::endl; //env_chain->getPlaneBFSMarker(mark, env_chain->getGoalPose().translation().z()); res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; PlanningStatistics stats = env_chain->getPlanningStatistics(); stats.total_planning_time_ = ros::WallDuration(el); (const_cast<SBPLInterface*>(this))->last_planning_statistics_ = stats; return true; }