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