40#include <Eigen/Geometry>
41#include <kdl/frames.hpp>
42#include <kdl/trajectory.hpp>
55 moveit_msgs::msg::MoveItErrorCodes::FAILURE);
59 moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
65 moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
69 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
73 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
75 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
79 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
81 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
83 moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
138 double max_acceleration_scaling_factor,
139 const std::unique_ptr<KDL::Path>& path)
const;
153 virtual void extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr& scene,
156 virtual void plan(
const planning_scene::PlanningSceneConstPtr& scene,
158 double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0;
208 const trajectory_msgs::msg::JointTrajectory& joint_trajectory,
213 void checkForValidGroupName(
const std::string& group_name)
const;
226 void checkStartState(
const moveit_msgs::msg::RobotState& start_state,
const std::string& group)
const;
228 void checkGoalConstraints(
const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
229 const std::vector<std::string>& expected_joint_names,
const std::string& group_name)
const;
231 void checkJointGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
232 const std::vector<std::string>& expected_joint_names,
233 const std::string& group_name)
const;
235 void checkCartesianGoalConstraint(
const moveit_msgs::msg::Constraints& constraint,
236 const std::string& group_name)
const;
242 sensor_msgs::msg::JointState filterGroupValues(
const sensor_msgs::msg::JointState& robot_state,
243 const std::string& group)
const;
248 static bool isScalingFactorValid(
double scaling_factor);
249 static void checkVelocityScaling(
double scaling_factor);
250 static void checkAccelerationScaling(
double scaling_factor);
256 static bool isCartesianGoalGiven(
const moveit_msgs::msg::Constraints& constraint);
261 static bool isJointGoalGiven(
const moveit_msgs::msg::Constraints& constraint);
267 static bool isOnlyOneGoalTypeGiven(
const moveit_msgs::msg::Constraints& constraint);
275 const std::unique_ptr<rclcpp::Clock>
clock_;
278inline bool TrajectoryGenerator::isScalingFactorValid(
double scaling_factor)
283inline bool TrajectoryGenerator::isCartesianGoalGiven(
const moveit_msgs::msg::Constraints& constraint)
285 return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1;
288inline bool TrajectoryGenerator::isJointGoalGiven(
const moveit_msgs::msg::Constraints& constraint)
290 return !constraint.joint_constraints.empty();
293inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(
const moveit_msgs::msg::Constraints& constraint)
295 return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint)) ||
296 (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint));
Representation of a robot's state. This includes position, velocity, acceleration and effort.
This class combines CartesianLimit and JointLimits into on single class.
This class is used to extract needed information from motion plan request.
std::pair< std::string, Eigen::Vector3d > circ_path_point
std::map< std::string, double > goal_joint_position
Eigen::Isometry3d start_pose
Eigen::Isometry3d goal_pose
std::map< std::string, double > start_joint_position
Base class of trajectory generators.
const moveit::core::RobotModelConstPtr robot_model_
static constexpr double VELOCITY_TOLERANCE
TrajectoryGenerator(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits)
static constexpr double MIN_SCALING_FACTOR
virtual ~TrajectoryGenerator()=default
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
const std::unique_ptr< rclcpp::Clock > clock_
void generate(const planning_scene::PlanningSceneConstPtr &scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, double sampling_time=0.1)
generate robot trajectory with given sampling time
static constexpr double MAX_SCALING_FACTOR
std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
build cartesian velocity profile for the path
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Response to a planning query.
#define CREATE_MOVEIT_ERROR_CODE_EXCEPTION(EXCEPTION_CLASS_NAME, ERROR_CODE)