18 #ifndef TRAJECTORYGENERATOR_H 19 #define TRAJECTORYGENERATOR_H 26 #include <Eigen/Geometry> 27 #include <kdl/frames.hpp> 28 #include <kdl/trajectory.hpp> 75 :robot_model_(robot_model),
76 planner_limits_(planner_limits)
91 double sampling_time=0.1);
115 std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(
116 const double& max_velocity_scaling_factor,
117 const double& max_acceleration_scaling_factor,
118 const std::unique_ptr<KDL::Path> &path)
const;
134 const double& sampling_time,
135 trajectory_msgs::JointTrajectory& joint_trajectory) = 0;
165 void setSuccessResponse(
const std::string& group_name,
166 const moveit_msgs::RobotState& start_state,
167 const trajectory_msgs::JointTrajectory& joint_trajectory,
171 void setFailureResponse(
const ros::Time &planning_start,
174 void checkForValidGroupName(
const std::string& group_name)
const;
185 void checkStartState(
const moveit_msgs::RobotState &start_state)
const;
187 void checkGoalConstraints(
const moveit_msgs::MotionPlanRequest::_goal_constraints_type &goal_constraints,
188 const std::vector<std::string> &expected_joint_names,
189 const std::string &group_name)
const;
191 void checkJointGoalConstraint(
const moveit_msgs::Constraints& constraint,
192 const std::vector<std::string>& expected_joint_names,
193 const std::string &group_name)
const;
195 void checkCartesianGoalConstraint(
const moveit_msgs::Constraints& constraint,
196 const std::string &group_name)
const;
198 void convertToRobotTrajectory(
const trajectory_msgs::JointTrajectory& joint_trajectory,
199 const moveit_msgs::RobotState &start_state,
206 static bool isScalingFactorValid(
const double& scaling_factor);
207 static void checkVelocityScaling(
const double& scaling_factor);
208 static void checkAccelerationScaling(
const double& scaling_factor);
214 static bool isCartesianGoalGiven(
const moveit_msgs::Constraints& constraint);
219 static bool isJointGoalGiven(
const moveit_msgs::Constraints& constraint);
225 static bool isOnlyOneGoalTypeGiven(
const moveit_msgs::Constraints& constraint);
230 static constexpr
double MIN_SCALING_FACTOR {0.0001};
231 static constexpr
double MAX_SCALING_FACTOR {1.};
232 static constexpr
double VELOCITY_TOLERANCE {1e-8};
235 inline bool TrajectoryGenerator::isScalingFactorValid(
const double& scaling_factor)
237 return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR);
240 inline bool TrajectoryGenerator::isCartesianGoalGiven(
const moveit_msgs::Constraints &constraint)
242 return constraint.position_constraints.size() == 1 && constraint.orientation_constraints.size() == 1;
245 inline bool TrajectoryGenerator::isJointGoalGiven(
const moveit_msgs::Constraints& constraint)
247 return constraint.joint_constraints.size() >= 1;
250 inline bool TrajectoryGenerator::isOnlyOneGoalTypeGiven(
const moveit_msgs::Constraints& constraint)
252 return (isJointGoalGiven(constraint) && !isCartesianGoalGiven(constraint))
253 || (!isJointGoalGiven(constraint) && isCartesianGoalGiven(constraint));
257 #endif // TRAJECTORYGENERATOR_H std::map< std::string, double > start_joint_position
std::map< std::string, double > goal_joint_position
Base class of trajectory generators.
Eigen::Affine3d goal_pose
Eigen::Affine3d start_pose
const robot_model::RobotModelConstPtr robot_model_
std::pair< std::string, Eigen::Vector3d > circ_path_point
const pilz::LimitsContainer planner_limits_
CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NegativeBlendRadiusException, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
This class combines CartesianLimit and JointLimits into on single class.
moveit_msgs::MotionPlanRequest MotionPlanRequest
This class is used to extract needed information from motion plan request.
TrajectoryGenerator(const robot_model::RobotModelConstPtr &robot_model, const pilz::LimitsContainer &planner_limits)