38 #include <boost/math/constants/constants.hpp> 73 return "Fix Start State Bounds";
79 std::vector<std::size_t>& added_path_index)
const 84 robot_state::RobotState start_state = planning_scene->getCurrentState();
85 robot_state::robotStateMsgToRobotState(planning_scene->getTransforms(), req.start_state, start_state);
87 const std::vector<const robot_model::JointModel*>& jmodels =
88 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
89 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
90 planning_scene->getRobotModel()->getJointModels();
92 bool change_req =
false;
93 for (std::size_t i = 0; i < jmodels.size(); ++i)
101 const robot_model::JointModel* jm = jmodels[i];
102 if (jm->getType() == robot_model::JointModel::REVOLUTE)
104 if (static_cast<const robot_model::RevoluteJointModel*>(jm)->isContinuous())
106 double initial = start_state.getJointPositions(jm)[0];
107 start_state.enforceBounds(jm);
108 double after = start_state.getJointPositions(jm)[0];
109 if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
115 if (jm->getType() == robot_model::JointModel::PLANAR)
117 const double* p = start_state.getJointPositions(jm);
118 double copy[3] = { p[0], p[1], p[2] };
119 if (static_cast<const robot_model::PlanarJointModel*>(jm)->normalizeRotation(copy))
121 start_state.setJointPositions(jm, copy);
127 if (jm->getType() == robot_model::JointModel::FLOATING)
129 const double* p = start_state.getJointPositions(jm);
130 double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] };
131 if (static_cast<const robot_model::FloatingJointModel*>(jm)->normalizeRotation(copy))
133 start_state.setJointPositions(jm, copy);
140 robot_state::RobotStatePtr prefix_state;
141 for (std::size_t i = 0; i < jmodels.size(); ++i)
143 if (!start_state.satisfiesBounds(jmodels[i]))
145 if (start_state.satisfiesBounds(jmodels[i],
bounds_dist_))
148 prefix_state.reset(
new robot_state::RobotState(start_state));
149 start_state.enforceBounds(jmodels[i]);
151 ROS_INFO(
"Starting state is just outside bounds (joint '%s'). Assuming within bounds.",
152 jmodels[i]->
getName().c_str());
156 std::stringstream joint_values;
157 std::stringstream joint_bounds_low;
158 std::stringstream joint_bounds_hi;
159 const double* p = start_state.getJointPositions(jmodels[i]);
160 for (std::size_t k = 0; k < jmodels[i]->getVariableCount(); ++k)
161 joint_values << p[k] <<
" ";
162 const robot_model::JointModel::Bounds& b = jmodels[i]->getVariableBounds();
163 for (std::size_t k = 0; k < b.size(); ++k)
165 joint_bounds_low << b[k].min_position_ <<
" ";
166 joint_bounds_hi << b[k].max_position_ <<
" ";
169 <<
"' from the starting state is outside bounds by a significant margin: [ " 170 << joint_values.str() <<
"] should be in the range [ " << joint_bounds_low.str()
171 <<
"], [ " << joint_bounds_hi.str() <<
"] but the error above the ~" 172 << BOUNDS_PARAM_NAME <<
" parameter (currently set to " <<
bounds_dist_ <<
")");
182 robot_state::robotStateToRobotStateMsg(start_state, req2.start_state,
false);
183 solved = planner(planning_scene, req2, res);
186 solved = planner(planning_scene, req, res);
195 res.
trajectory_->addPrefixWayPoint(prefix_state, 0.0);
197 for (std::size_t i = 0; i < added_path_index.size(); ++i)
198 added_path_index[i]++;
199 added_path_index.push_back(0);
robot_trajectory::RobotTrajectoryPtr trajectory_
std::string getName(void *handle)
virtual std::string getDescription() const
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::FixStartStateBounds, planning_request_adapter::PlanningRequestAdapter)
virtual bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const
static const std::string BOUNDS_PARAM_NAME
#define ROS_WARN_STREAM(args)
moveit_msgs::MotionPlanRequest MotionPlanRequest
boost::function< bool(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res)> PlannerFn
#define ROS_INFO_STREAM(args)
static const std::string DT_PARAM_NAME
bool getParam(const std::string &key, std::string &s) const