Classes | Public Member Functions | Protected Member Functions | Protected Attributes | Static Protected Attributes | Private Member Functions | Static Private Member Functions | List of all members
pilz_industrial_motion_planner::TrajectoryGenerator Class Referenceabstract

Base class of trajectory generators. More...

#include <trajectory_generator.h>

Inheritance diagram for pilz_industrial_motion_planner::TrajectoryGenerator:
Inheritance graph
[legend]

Classes

class  MotionPlanInfo
 This class is used to extract needed information from motion plan request. More...
 

Public Member Functions

bool 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 More...
 
 TrajectoryGenerator (const robot_model::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits)
 
virtual ~TrajectoryGenerator ()=default
 

Protected Member Functions

std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile (const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
 build cartesian velocity profile for the path More...
 

Protected Attributes

const pilz_industrial_motion_planner::LimitsContainer planner_limits_
 
const robot_model::RobotModelConstPtr robot_model_
 

Static Protected Attributes

static constexpr double MAX_SCALING_FACTOR { 1. }
 
static constexpr double MIN_SCALING_FACTOR { 0.0001 }
 
static constexpr double VELOCITY_TOLERANCE { 1e-8 }
 

Private Member Functions

void checkCartesianGoalConstraint (const moveit_msgs::Constraints &constraint, const moveit::core::RobotState &robot_state, const moveit::core::JointModelGroup *const jmg) const
 
void checkForValidGroupName (const std::string &group_name) const
 
void checkGoalConstraints (const moveit_msgs::MotionPlanRequest::_goal_constraints_type &goal_constraints, const std::string &group_name, const moveit::core::RobotState &rstate) const
 
void checkJointGoalConstraint (const moveit_msgs::Constraints &constraint, const std::string &group_name) const
 
void checkStartState (const moveit_msgs::RobotState &start_state, const std::string &group) const
 Validate that the start state of the request matches the requirements of the trajectory generator. More...
 
virtual void cmdSpecificRequestValidation (const planning_interface::MotionPlanRequest &req) const
 
virtual void extractMotionPlanInfo (const planning_scene::PlanningSceneConstPtr &scene, const planning_interface::MotionPlanRequest &req, MotionPlanInfo &info) const =0
 Extract needed information from a motion plan request in order to simplify further usages. More...
 
sensor_msgs::JointState filterGroupValues (const sensor_msgs::JointState &robot_state, const std::string &group) const
 
virtual void plan (const planning_scene::PlanningSceneConstPtr &scene, const planning_interface::MotionPlanRequest &req, const MotionPlanInfo &plan_info, double sampling_time, trajectory_msgs::JointTrajectory &joint_trajectory)=0
 
void setFailureResponse (const ros::Time &planning_start, planning_interface::MotionPlanResponse &res) const
 
void setSuccessResponse (const moveit::core::RobotState &start_rs, const std::string &group_name, const trajectory_msgs::JointTrajectory &joint_trajectory, const ros::Time &planning_start, planning_interface::MotionPlanResponse &res) const
 set MotionPlanResponse from joint trajectory More...
 
void validateRequest (const planning_interface::MotionPlanRequest &req, const moveit::core::RobotState &rstate) const
 Validate the motion plan request based on the common requirements of trajectroy generator Checks that: More...
 

Static Private Member Functions

static void checkAccelerationScaling (const double scaling_factor)
 
static void checkVelocityScaling (const double scaling_factor)
 
static bool isCartesianGoalGiven (const moveit_msgs::Constraints &constraint)
 
static bool isJointGoalGiven (const moveit_msgs::Constraints &constraint)
 
static bool isOnlyOneGoalTypeGiven (const moveit_msgs::Constraints &constraint)
 
static bool isScalingFactorValid (const double scaling_factor)
 

Detailed Description

Base class of trajectory generators.

Note: All derived classes cannot have a start velocity

Definition at line 90 of file trajectory_generator.h.

Constructor & Destructor Documentation

◆ TrajectoryGenerator()

pilz_industrial_motion_planner::TrajectoryGenerator::TrajectoryGenerator ( const robot_model::RobotModelConstPtr &  robot_model,
const pilz_industrial_motion_planner::LimitsContainer &  planner_limits 
)
inline

Definition at line 93 of file trajectory_generator.h.

◆ ~TrajectoryGenerator()

virtual pilz_industrial_motion_planner::TrajectoryGenerator::~TrajectoryGenerator ( )
virtualdefault

Member Function Documentation

◆ cartesianTrapVelocityProfile()

std::unique_ptr< KDL::VelocityProfile > pilz_industrial_motion_planner::TrajectoryGenerator::cartesianTrapVelocityProfile ( const double  max_velocity_scaling_factor,
const double  max_acceleration_scaling_factor,
const std::unique_ptr< KDL::Path > &  path 
) const
protected

build cartesian velocity profile for the path

Uses the path to get the cartesian length and the angular distance from start to goal. The trap profile returns uses the longer distance of translational and rotational motion.

Definition at line 301 of file trajectory_generator.cpp.

◆ checkAccelerationScaling()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkAccelerationScaling ( const double  scaling_factor)
staticprivate

Definition at line 121 of file trajectory_generator.cpp.

◆ checkCartesianGoalConstraint()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkCartesianGoalConstraint ( const moveit_msgs::Constraints &  constraint,
const moveit::core::RobotState &  robot_state,
const moveit::core::JointModelGroup *const  jmg 
) const
private

Definition at line 198 of file trajectory_generator.cpp.

◆ checkForValidGroupName()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkForValidGroupName ( const std::string &  group_name) const
private

Definition at line 132 of file trajectory_generator.cpp.

◆ checkGoalConstraints()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkGoalConstraints ( const moveit_msgs::MotionPlanRequest::_goal_constraints_type &  goal_constraints,
const std::string &  group_name,
const moveit::core::RobotState &  rstate 
) const
private

Definition at line 240 of file trajectory_generator.cpp.

◆ checkJointGoalConstraint()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkJointGoalConstraint ( const moveit_msgs::Constraints &  constraint,
const std::string &  group_name 
) const
private

Definition at line 175 of file trajectory_generator.cpp.

◆ checkStartState()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkStartState ( const moveit_msgs::RobotState &  start_state,
const std::string &  group 
) const
private

Validate that the start state of the request matches the requirements of the trajectory generator.

These requirements are:

  • Names of the joints and given joint position match in size and are non-zero
  • The start state is withing the position limits
  • The start state velocity is below TrajectoryGenerator::VELOCITY_TOLERANCE

Definition at line 142 of file trajectory_generator.cpp.

◆ checkVelocityScaling()

void pilz_industrial_motion_planner::TrajectoryGenerator::checkVelocityScaling ( const double  scaling_factor)
staticprivate

Definition at line 110 of file trajectory_generator.cpp.

◆ cmdSpecificRequestValidation()

void pilz_industrial_motion_planner::TrajectoryGenerator::cmdSpecificRequestValidation ( const planning_interface::MotionPlanRequest &  req) const
privatevirtual

Reimplemented in pilz_industrial_motion_planner::TrajectoryGeneratorCIRC.

Definition at line 104 of file trajectory_generator.cpp.

◆ extractMotionPlanInfo()

virtual void pilz_industrial_motion_planner::TrajectoryGenerator::extractMotionPlanInfo ( const planning_scene::PlanningSceneConstPtr &  scene,
const planning_interface::MotionPlanRequest &  req,
MotionPlanInfo &  info 
) const
privatepure virtual

Extract needed information from a motion plan request in order to simplify further usages.

Parameters
sceneplanning scene
reqmotion plan request
infoinformation extracted from motion plan request which is necessary for the planning

Implemented in pilz_industrial_motion_planner::TrajectoryGeneratorPTP, pilz_industrial_motion_planner::TrajectoryGeneratorCIRC, and pilz_industrial_motion_planner::TrajectoryGeneratorLIN.

◆ filterGroupValues()

sensor_msgs::JointState pilz_industrial_motion_planner::TrajectoryGenerator::filterGroupValues ( const sensor_msgs::JointState &  robot_state,
const std::string &  group 
) const
private
Returns
joint state message including only active joints in group

Definition at line 80 of file trajectory_generator.cpp.

◆ generate()

bool pilz_industrial_motion_planner::TrajectoryGenerator::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

Parameters
reqmotion plan request
resmotion plan response
sampling_timesampling time of the generate trajectory
Returns
motion plan succeed/fail, detailed information in motion plan response

Definition at line 320 of file trajectory_generator.cpp.

◆ isCartesianGoalGiven()

bool pilz_industrial_motion_planner::TrajectoryGenerator::isCartesianGoalGiven ( const moveit_msgs::Constraints &  constraint)
inlinestaticprivate
Returns
True if ONE position + ONE orientation constraint given, otherwise false.

Definition at line 283 of file trajectory_generator.h.

◆ isJointGoalGiven()

bool pilz_industrial_motion_planner::TrajectoryGenerator::isJointGoalGiven ( const moveit_msgs::Constraints &  constraint)
inlinestaticprivate
Returns
True if joint constraint given, otherwise false.

Definition at line 288 of file trajectory_generator.h.

◆ isOnlyOneGoalTypeGiven()

bool pilz_industrial_motion_planner::TrajectoryGenerator::isOnlyOneGoalTypeGiven ( const moveit_msgs::Constraints &  constraint)
inlinestaticprivate
Returns
True if ONLY joint constraint or ONLY cartesian constraint (position+orientation) given, otherwise false.

Definition at line 293 of file trajectory_generator.h.

◆ isScalingFactorValid()

bool pilz_industrial_motion_planner::TrajectoryGenerator::isScalingFactorValid ( const double  scaling_factor)
inlinestaticprivate
Returns
True if scaling factor is valid, otherwise false.

Definition at line 278 of file trajectory_generator.h.

◆ plan()

virtual void pilz_industrial_motion_planner::TrajectoryGenerator::plan ( const planning_scene::PlanningSceneConstPtr &  scene,
const planning_interface::MotionPlanRequest &  req,
const MotionPlanInfo &  plan_info,
double  sampling_time,
trajectory_msgs::JointTrajectory &  joint_trajectory 
)
privatepure virtual

Implemented in pilz_industrial_motion_planner::TrajectoryGeneratorCIRC, pilz_industrial_motion_planner::TrajectoryGeneratorPTP, and pilz_industrial_motion_planner::TrajectoryGeneratorLIN.

◆ setFailureResponse()

void pilz_industrial_motion_planner::TrajectoryGenerator::setFailureResponse ( const ros::Time &  planning_start,
planning_interface::MotionPlanResponse &  res 
) const
private

Definition at line 290 of file trajectory_generator.cpp.

◆ setSuccessResponse()

void pilz_industrial_motion_planner::TrajectoryGenerator::setSuccessResponse ( const moveit::core::RobotState &  start_rs,
const std::string &  group_name,
const trajectory_msgs::JointTrajectory &  joint_trajectory,
const ros::Time &  planning_start,
planning_interface::MotionPlanResponse &  res 
) const
private

set MotionPlanResponse from joint trajectory

Definition at line 277 of file trajectory_generator.cpp.

◆ validateRequest()

void pilz_industrial_motion_planner::TrajectoryGenerator::validateRequest ( const planning_interface::MotionPlanRequest &  req,
const moveit::core::RobotState &  rstate 
) const
private

Validate the motion plan request based on the common requirements of trajectroy generator Checks that:

  • req.max_velocity_scaling_factor [0.0001, 1], moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on failure
  • req.max_acceleration_scaling_factor [0.0001, 1] , moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN on failure
  • req.group_name is a JointModelGroup of the Robotmodel, moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME on failure
  • req.start_state.joint_state is not empty, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure
  • req.start_state.joint_state is within the limits, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure
  • req.start_state.joint_state is all zero, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE on failure
  • req.goal_constraints must have exactly 1 defined cartesian oder joint constraint moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure A joint goal is checked for:
  • StartState joint-names matching goal joint-names, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
  • Beeing defined in the req.group_name JointModelGroup
  • Beeing with the defined limits A cartesian goal is checked for:
  • A defined link_name for the constraint, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
  • Matching link_name for position and orientation constraints, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
  • A IK solver exists for the given req.group_name and constraint link_name, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION on failure
  • A goal pose define in position_constraints[0].constraint_region.primitive_poses, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
    Parameters
    reqmotion plan request

Definition at line 267 of file trajectory_generator.cpp.

Member Data Documentation

◆ MAX_SCALING_FACTOR

constexpr double pilz_industrial_motion_planner::TrajectoryGenerator::MAX_SCALING_FACTOR { 1. }
staticconstexprprotected

Definition at line 274 of file trajectory_generator.h.

◆ MIN_SCALING_FACTOR

constexpr double pilz_industrial_motion_planner::TrajectoryGenerator::MIN_SCALING_FACTOR { 0.0001 }
staticconstexprprotected

Definition at line 273 of file trajectory_generator.h.

◆ planner_limits_

const pilz_industrial_motion_planner::LimitsContainer pilz_industrial_motion_planner::TrajectoryGenerator::planner_limits_
protected

Definition at line 272 of file trajectory_generator.h.

◆ robot_model_

const robot_model::RobotModelConstPtr pilz_industrial_motion_planner::TrajectoryGenerator::robot_model_
protected

Definition at line 271 of file trajectory_generator.h.

◆ VELOCITY_TOLERANCE

constexpr double pilz_industrial_motion_planner::TrajectoryGenerator::VELOCITY_TOLERANCE { 1e-8 }
staticconstexprprotected

Definition at line 275 of file trajectory_generator.h.

The documentation for this class was generated from the following files:

pilz_industrial_motion_planner
Author(s):
autogenerated on Sat May 3 2025 02:28:35