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;
142void 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());
165CommandListManager::RobotState_OptRef
166CommandListManager::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());
179void 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) };
190 const moveit_msgs::msg::MotionSequenceItem& item_A,
191 const moveit_msgs::msg::MotionSequenceItem& item_B)
194 if (item_A.blend_radius == 0.)
200 if (item_A.req.group_name != item_B.req.group_name)
202 RCLCPP_WARN_STREAM(
getLogger(),
"Blending between different groups (in this case: \""
203 << item_A.req.group_name <<
"\" and \"" << item_B.req.group_name
204 <<
"\") not allowed");
211 RCLCPP_WARN_STREAM(
getLogger(),
"Blending for groups without solver not allowed");
218CommandListManager::RadiiCont
220 const moveit_msgs::msg::MotionSequenceRequest& req_list)
222 RadiiCont radii(req_list.items.size(), 0.);
223 for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i)
225 if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1)))
227 RCLCPP_WARN_STREAM(
getLogger(),
"Invalid blend radii between commands: [" << i <<
"] and [" << i + 1
228 <<
"] => Blend radii set to zero");
231 radii.at(i) = req_list.items.at(i).blend_radius;
236CommandListManager::MotionResponseCont
237CommandListManager::solveSequenceItems(
const planning_scene::PlanningSceneConstPtr&
planning_scene,
239 const moveit_msgs::msg::MotionSequenceRequest& req_list)
const
241 MotionResponseCont motion_plan_responses;
242 size_t curr_req_index{ 0 };
243 const size_t num_req{ req_list.items.size() };
244 for (
const auto& seq_item : req_list.items)
247 setStartState(motion_plan_responses, req.group_name, req.start_state);
252 RCLCPP_ERROR(
getLogger(),
"Generating a plan with planning pipeline failed.");
253 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
257 std::ostringstream os;
258 os <<
"Could not solve request\n";
259 throw PlanningPipelineException(os.str(), res.
error_code.val);
261 motion_plan_responses.emplace_back(res);
262 RCLCPP_DEBUG_STREAM(
getLogger(),
"Solved [" << ++curr_req_index <<
'/' << num_req <<
']');
264 return motion_plan_responses;
267void CommandListManager::checkForNegativeRadii(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
269 if (!std::all_of(req_list.items.begin(), req_list.items.end(),
270 [](
const moveit_msgs::msg::MotionSequenceItem& req) { return (req.blend_radius >= 0.); }))
272 throw NegativeBlendRadiusException(
"All blending radii MUST be non negative");
276void CommandListManager::checkStartStatesOfGroup(
const moveit_msgs::msg::MotionSequenceRequest& req_list,
277 const std::string& group_name)
279 bool first_elem{
true };
280 for (
const moveit_msgs::msg::MotionSequenceItem& item : req_list.items)
282 if (item.req.group_name != group_name)
293 if (!(item.req.start_state.joint_state.position.empty() && item.req.start_state.joint_state.velocity.empty() &&
294 item.req.start_state.joint_state.effort.empty() && item.req.start_state.joint_state.name.empty()))
296 std::ostringstream os;
297 os <<
"Only the first request is allowed to have a start state, but"
298 <<
" the requests for group: \"" << group_name <<
"\" violate the rule";
299 throw StartStateSetException(os.str());
304void CommandListManager::checkStartStates(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
306 if (req_list.items.size() <= 1)
311 GroupNamesCont group_names{ getGroupNames(req_list) };
312 for (
const auto& curr_group_name : group_names)
314 checkStartStatesOfGroup(req_list, curr_group_name);
318CommandListManager::GroupNamesCont
319CommandListManager::getGroupNames(
const moveit_msgs::msg::MotionSequenceRequest& req_list)
321 GroupNamesCont group_names;
322 std::for_each(req_list.items.cbegin(), req_list.items.cend(),
323 [&group_names](
const moveit_msgs::msg::MotionSequenceItem& item) {
324 if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend())
326 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