37 #include <rclcpp/logger.hpp>
38 #include <rclcpp/logging.hpp>
47 #include "cartesian_limits_parameters.hpp"
65 const moveit::core::RobotModelConstPtr& model)
66 : node_(node), model_(model)
76 params_ = param_listener_->get_params();
83 plan_comp_builder_.
setBlender(std::unique_ptr<pilz_industrial_motion_planner::TrajectoryBlender>(
89 const moveit_msgs::msg::MotionSequenceRequest& req_list)
91 if (req_list.items.empty())
96 checkForNegativeRadii(req_list);
97 checkLastBlendRadiusZero(req_list);
98 checkStartStates(req_list);
103 RadiiCont radii{ extractBlendRadii(*model_, req_list) };
104 checkForOverlappingRadii(resp_cont, radii);
106 plan_comp_builder_.
reset();
107 for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i)
113 (i > 0 ? radii.at(i - 1) : 0.));
116 return plan_comp_builder_.
build();
121 const double radii_B)
const
129 auto sum_radii{ radii_A + radii_B };
135 const std::string& blend_frame{ getSolverTipFrame(model_->getJointModelGroup(traj_A.
getGroupName())) };
139 return distance_endpoints <= sum_radii;
142 void CommandListManager::checkForOverlappingRadii(
const MotionResponseCont& resp_cont,
const RadiiCont& radii)
const
144 if (resp_cont.empty())
148 if (resp_cont.size() < 3)
153 for (MotionResponseCont::size_type i = 0; i < resp_cont.size() - 2; ++i)
155 if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory), radii.at(i), *(resp_cont.at(i + 1).trajectory),
158 std::ostringstream os;
159 os <<
"Overlapping blend radii between command [" << i <<
"] and [" << i + 1 <<
"].";
160 throw OverlappingBlendRadiiException(os.str());
165 CommandListManager::RobotState_OptRef
166 CommandListManager::getPreviousEndState(
const MotionResponseCont& motion_plan_responses,
const std::string& group_name)
168 for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin();
169 it != motion_plan_responses.crend(); ++it)
171 if (it->trajectory->getGroupName() == group_name)
173 return std::reference_wrapper(it->trajectory->getLastWayPoint());
179 void CommandListManager::setStartState(
const MotionResponseCont& motion_plan_responses,
const std::string& group_name,
180 moveit_msgs::msg::RobotState& start_state)
182 RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) };
186 start_state.is_diff =
true;
191 const moveit_msgs::msg::MotionSequenceItem& item_A,
192 const moveit_msgs::msg::MotionSequenceItem& item_B)
195 if (item_A.blend_radius == 0.)
201 if (item_A.req.group_name != item_B.req.group_name)
203 RCLCPP_WARN_STREAM(
getLogger(),
"Blending between different groups (in this case: \""
204 << item_A.req.group_name <<
"\" and \"" << item_B.req.group_name
205 <<
"\") not allowed");
212 RCLCPP_WARN_STREAM(
getLogger(),
"Blending for groups without solver not allowed");
219 CommandListManager::RadiiCont
221 const moveit_msgs::msg::MotionSequenceRequest& req_list)
223 RadiiCont radii(req_list.items.size(), 0.);
224 for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i)
226 if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1)))
228 RCLCPP_WARN_STREAM(
getLogger(),
"Invalid blend radii between commands: [" << i <<
"] and [" << i + 1
229 <<
"] => Blend radii set to zero");
232 radii.at(i) = req_list.items.at(i).blend_radius;
237 CommandListManager::MotionResponseCont
238 CommandListManager::solveSequenceItems(
const planning_scene::PlanningSceneConstPtr&
planning_scene,
240 const moveit_msgs::msg::MotionSequenceRequest& req_list)
const
242 MotionResponseCont motion_plan_responses;
243 size_t curr_req_index{ 0 };
244 const size_t num_req{ req_list.items.size() };
245 for (
const auto& seq_item : req_list.items)
248 setStartState(motion_plan_responses, req.group_name, req.start_state);
253 RCLCPP_ERROR(
getLogger(),
"Generating a plan with planning pipeline failed.");
254 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
258 std::ostringstream os;
259 os <<
"Could not solve request\n";
260 throw PlanningPipelineException(os.str(), res.
error_code.val);
262 motion_plan_responses.emplace_back(res);
263 RCLCPP_DEBUG_STREAM(
getLogger(),
"Solved [" << ++curr_req_index <<
'/' << num_req <<
']');
265 return motion_plan_responses;
268 void CommandListManager::checkForNegativeRadii(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
270 if (!std::all_of(req_list.items.begin(), req_list.items.end(),
271 [](
const moveit_msgs::msg::MotionSequenceItem& req) { return (req.blend_radius >= 0.); }))
273 throw NegativeBlendRadiusException(
"All blending radii MUST be non negative");
277 void CommandListManager::checkStartStatesOfGroup(
const moveit_msgs::msg::MotionSequenceRequest& req_list,
278 const std::string& group_name)
280 bool first_elem{
true };
281 for (
const moveit_msgs::msg::MotionSequenceItem& item : req_list.items)
283 if (item.req.group_name != group_name)
294 if (!(item.req.start_state.joint_state.position.empty() && item.req.start_state.joint_state.velocity.empty() &&
295 item.req.start_state.joint_state.effort.empty() && item.req.start_state.joint_state.name.empty()))
297 std::ostringstream os;
298 os <<
"Only the first request is allowed to have a start state, but"
299 <<
" the requests for group: \"" << group_name <<
"\" violate the rule";
300 throw StartStateSetException(os.str());
305 void CommandListManager::checkStartStates(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
307 if (req_list.items.size() <= 1)
312 GroupNamesCont group_names{ getGroupNames(req_list) };
313 for (
const auto& curr_group_name : group_names)
315 checkStartStatesOfGroup(req_list, curr_group_name);
319 CommandListManager::GroupNamesCont
320 CommandListManager::getGroupNames(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
322 GroupNamesCont group_names;
323 std::for_each(req_list.items.cbegin(), req_list.items.cend(),
324 [&group_names](
const moveit_msgs::msg::MotionSequenceItem& item) {
325 if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend())
327 group_names.emplace_back(item.req.group_name);
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
CommandListManager(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &model)
RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_pipeline::PlanningPipelinePtr &planning_pipeline, const moveit_msgs::msg::MotionSequenceRequest &req_list)
Generates trajectories for the specified list of motion commands.
static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string ¶m_namespace, const std::vector< const moveit::core::JointModel * > &joint_models)
Aggregates(combines) the joint limits from joint model and node parameters. The rules for the combina...
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
This class combines CartesianLimit and JointLimits into on single class.
void setCartesianLimits(cartesian_limits::Params &cartesian_limit)
Set cartesian limits.
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
std::vector< robot_trajectory::RobotTrajectoryPtr > build() const
void reset()
Clears the trajectory container under construction.
void setModel(const moveit::core::RobotModelConstPtr &model)
Sets the robot model needed to create new trajectory elements.
void setBlender(std::unique_ptr< pilz_industrial_motion_planner::TrajectoryBlender > blender)
Sets the blender used to blend two trajectories.
void append(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_trajectory::RobotTrajectoryPtr &other, const double blend_radius)
Appends the specified trajectory to the trajectory container under construction.
Trajectory blender implementing transition window algorithm.
Maintain a sequence of waypoints and the time durations between these waypoints.
const std::string & getGroupName() const
const moveit::core::RobotState & getLastWayPoint() const
rclcpp::Logger getLogger()
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
std::vector< robot_trajectory::RobotTrajectoryPtr > RobotTrajCont
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
Response to a planning query.
moveit::core::MoveItErrorCode error_code
const std::string PARAM_NAMESPACE_LIMITS