92 return "Fix Start State In Collision";
97 std::vector<std::size_t>& added_path_index)
const override
118 ROS_INFO(
"Start state appears to be in collision");
125 const std::vector<const moveit::core::JointModel*>& jmodels =
126 planning_scene->getRobotModel()->hasJointModelGroup(req.group_name) ?
127 planning_scene->getRobotModel()->getJointModelGroup(req.group_name)->getJointModels() :
133 for (std::size_t i = 0; !found && i < jmodels.size(); ++i)
135 std::vector<double> sampled_variable_values(jmodels[i]->getVariableCount());
136 const double* original_values = prefix_state->getJointPositions(jmodels[i]);
137 jmodels[i]->getVariableRandomPositionsNearBy(rng, &sampled_variable_values[0], original_values,
145 ROS_INFO(
"Found a valid state near the start state at distance %lf after %d attempts",
146 prefix_state->distance(start_state), c);
162 res.
trajectory_->addPrefixWayPoint(prefix_state, 0.0);
164 for (std::size_t& added_index : added_path_index)
166 added_path_index.push_back(0);
172 ROS_WARN(
"Unable to find a valid state nearby the start state "
173 "(using jiggle fraction of %lf and %u sampling attempts).",
175 res.
error_code_.val = moveit_msgs::MoveItErrorCodes::START_STATE_IN_COLLISION;