38 #include <boost/math/constants/constants.hpp>
77 return "Fix Start State Bounds";
82 std::vector<std::size_t>& added_path_index)
const override
90 const std::vector<const moveit::core::JointModel*>& jmodels =
91 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
92 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
95 bool change_req =
false;
111 if (fabs(initial - after) > std::numeric_limits<double>::epsilon())
120 double copy[3] = { p[0], p[1], p[2] };
132 double copy[7] = { p[0], p[1], p[2], p[3], p[4], p[5], p[6] };
142 moveit::core::RobotStatePtr prefix_state;
150 prefix_state = std::make_shared<moveit::core::RobotState>(start_state);
153 ROS_INFO(
"Starting state is just outside bounds (joint '%s'). Assuming within bounds.",
154 jmodel->getName().c_str());
158 std::stringstream joint_values;
159 std::stringstream joint_bounds_low;
160 std::stringstream joint_bounds_hi;
162 for (std::size_t k = 0; k < jmodel->getVariableCount(); ++k)
163 joint_values << p[k] <<
" ";
167 joint_bounds_low << variable_bounds.min_position_ <<
" ";
168 joint_bounds_hi << variable_bounds.max_position_ <<
" ";
171 <<
"' from the starting state is outside bounds by a significant margin: [ "
172 << joint_values.str() <<
"] should be in the range [ " << joint_bounds_low.str()
173 <<
"], [ " << joint_bounds_hi.str() <<
"] but the error above the ~"
197 res.
trajectory_->addPrefixWayPoint(prefix_state, 0.0);
199 for (std::size_t& added_index : added_path_index)
201 added_path_index.push_back(0);