void PlaneRegistration::estimate_during_cutting(){ fk_solver_->JntToCart( jnt_pos_, tip_frame_ ); Eigen::Vector3d startPoint_temp( startPoint ); // reset start point every 5 seconds to capture the most recent motion if ( ros::Time::now().toSec() - last_reset_start_point_time > 5 ){ startPoint_temp = curr_point_history.col(0); last_reset_start_point_time = ros::Time::now().toSec(); } if ( ptsloc == 1 ){ Eigen::Vector3d curr_point( tip_frame_.p[0], tip_frame_.p[1], tip_frame_.p[2] ); trajectory_unit_vector = estimate_trj_unit_vector( startPoint_temp, curr_point, trajectory_unit_vector_history ); trajectory_unit_vector_last = trajectory_unit_vector; } if ( ptsloc == 0 ){ trajectory_unit_vector = trajectory_unit_vector_last; } trajectory_unit_vector_msg.x = trajectory_unit_vector(0); trajectory_unit_vector_msg.y = trajectory_unit_vector(1); trajectory_unit_vector_msg.z = trajectory_unit_vector(2); pub_traj_unit_vector_.publish( trajectory_unit_vector_msg ); }
void PlaneRegistration::local_probing(){ fk_solver_->JntToCart( jnt_pos_, tip_frame_ ); Eigen::Vector3d start_point( startPoint(0), startPoint(1), startPoint(2) ); Eigen::Vector3d curr_point( tip_frame_.p[0], tip_frame_.p[1], tip_frame_.p[2] ); // cutter moving in the first straight line, estimate the first vector if ( ptsloc == 1 ){ plane_vec_1 = estimate_trj_unit_vector( start_point, curr_point, unit_vector_history_1 ); ptsloclast = 1; } // resetting start point when moving from direction 1 to direction 2 if ( ptsloc == -1 ){ reset_start_point = 1; } // cutter moving in another direction, estimate the second vector if ( ptsloc == 2 ){ plane_vec_2 = estimate_trj_unit_vector( start_point, curr_point, unit_vector_history_2 ); ptsloclast = 2; } // finished collecting two vectors on plane, calculate the normal if ( ptsloc == 3 ){ Eigen::Vector3d normal; normal = plane_vec_1.cross( plane_vec_2 ); // make sure the reported normal makes an angle < 90 degrees with the cutter z axis Eigen::Vector3d tipZ( tip_frame_.M.UnitZ()[0], tip_frame_.M.UnitZ()[1], tip_frame_.M.UnitZ()[2] ); if ( normal.dot( tipZ ) < 0 ){ normal = -normal; } Eigen::Vector3d unit_normal = normal / normal.norm(); std::cout << "est. Normal: "<< std::endl; std::cout << unit_normal <<std::endl; local_probing_est.x = unit_normal(0); local_probing_est.y = unit_normal(1); local_probing_est.z = unit_normal(2); pub_local_probing_normal_est_.publish( local_probing_est ); } }
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; }