Base class of trajectory generators. More...

#include <trajectory_generator.h>

Inheritance diagram for pilz::TrajectoryGenerator:

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

Public Member Functions
bool	generate (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::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::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 std::string &group_name) const

void	checkForValidGroupName (const std::string &group_name) const

void	checkGoalConstraints (const moveit_msgs::MotionPlanRequest::_goal_constraints_type &goal_constraints, const std::vector< std::string > &expected_joint_names, const std::string &group_name) const

void	checkJointGoalConstraint (const moveit_msgs::Constraints &constraint, const std::vector< std::string > &expected_joint_names, const std::string &group_name) const

void	checkStartState (const moveit_msgs::RobotState &start_state) 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

void	convertToRobotTrajectory (const trajectory_msgs::JointTrajectory &joint_trajectory, const moveit_msgs::RobotState &start_state, robot_trajectory::RobotTrajectory &robot_trajectory) const

virtual void	extractMotionPlanInfo (const planning_interface::MotionPlanRequest &req, MotionPlanInfo &info) const =0
	Extract needed information from a motion plan request in order to simplify further usages. More...

virtual void	plan (const planning_interface::MotionPlanRequest &req, const MotionPlanInfo &plan_info, const double &sampling_time, trajectory_msgs::JointTrajectory &joint_trajectory)=0

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

void	setSuccessResponse (const std::string &group_name, const moveit_msgs::RobotState &start_state, 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
	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 69 of file trajectory_generator.h.

Constructor & Destructor Documentation

pilz::TrajectoryGenerator::TrajectoryGenerator	(	const robot_model::RobotModelConstPtr &	robot_model,
		const pilz::LimitsContainer &	planner_limits
	)

inline

Definition at line 73 of file trajectory_generator.h.

virtual pilz::TrajectoryGenerator::~TrajectoryGenerator ( )

virtualdefault

Member Function Documentation

std::unique_ptr< KDL::VelocityProfile > pilz::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 241 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkAccelerationScaling ( const double & scaling_factor )

staticprivate

Definition at line 50 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkCartesianGoalConstraint	(	const moveit_msgs::Constraints &	constraint,
		const std::string &	group_name
	)		const

private

Definition at line 130 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkForValidGroupName ( const std::string & group_name ) const

private

Definition at line 62 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkGoalConstraints	(	const moveit_msgs::MotionPlanRequest::_goal_constraints_type &	goal_constraints,
		const std::vector< std::string > &	expected_joint_names,
		const std::string &	group_name
	)		const

private

Definition at line 170 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkJointGoalConstraint	(	const moveit_msgs::Constraints &	constraint,
		const std::vector< std::string > &	expected_joint_names,
		const std::string &	group_name
	)		const

private

Definition at line 98 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkStartState ( const moveit_msgs::RobotState & start_state ) 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 72 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::checkVelocityScaling ( const double & scaling_factor )

staticprivate

Definition at line 38 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::cmdSpecificRequestValidation ( const planning_interface::MotionPlanRequest & req ) const

privatevirtual

Reimplemented in pilz::TrajectoryGeneratorCIRC.

Definition at line 32 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::convertToRobotTrajectory	(	const trajectory_msgs::JointTrajectory &	joint_trajectory,
		const moveit_msgs::RobotState &	start_state,
		robot_trajectory::RobotTrajectory &	robot_trajectory
	)		const

private

Definition at line 207 of file trajectory_generator.cpp.

virtual void pilz::TrajectoryGenerator::extractMotionPlanInfo	(	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

req	motion plan request
info	information extracted from motion plan request which is necessary for the planning

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

bool pilz::TrajectoryGenerator::generate	(	const planning_interface::MotionPlanRequest &	req,
		planning_interface::MotionPlanResponse &	res,
		double	sampling_time = `0.1`
	)

generate robot trajectory with given sampling time

Parameters

req	motion plan request
res	motion plan response
sampling_time	sampling time of the generate trajectory

Returns: motion plan succeed/fail, detailed information in motion plan responce

Definition at line 262 of file trajectory_generator.cpp.

bool pilz::TrajectoryGenerator::isCartesianGoalGiven ( const moveit_msgs::Constraints & constraint )

inlinestaticprivate

Returns: True if ONE position + ONE orientation constraint given, otherwise false.

Definition at line 240 of file trajectory_generator.h.

bool pilz::TrajectoryGenerator::isJointGoalGiven ( const moveit_msgs::Constraints & constraint )

inlinestaticprivate

Returns: True if joint constraint given, otherwise false.

Definition at line 245 of file trajectory_generator.h.

bool pilz::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 250 of file trajectory_generator.h.

bool pilz::TrajectoryGenerator::isScalingFactorValid ( const double & scaling_factor )

inlinestaticprivate

Returns: True if scaling factor is valid, otherwise false.

Definition at line 235 of file trajectory_generator.h.

virtual void pilz::TrajectoryGenerator::plan	(	const planning_interface::MotionPlanRequest &	req,
		const MotionPlanInfo &	plan_info,
		const double &	sampling_time,
		trajectory_msgs::JointTrajectory &	joint_trajectory
	)

privatepure virtual

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

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

private

Definition at line 231 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::setSuccessResponse	(	const std::string &	group_name,
		const moveit_msgs::RobotState &	start_state,
		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 217 of file trajectory_generator.cpp.

void pilz::TrajectoryGenerator::validateRequest ( const planning_interface::MotionPlanRequest & req ) 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

req motion plan request

Definition at line 198 of file trajectory_generator.cpp.

Member Data Documentation

constexpr double pilz::TrajectoryGenerator::MAX_SCALING_FACTOR {1.}

staticprotected

Definition at line 231 of file trajectory_generator.h.

constexpr double pilz::TrajectoryGenerator::MIN_SCALING_FACTOR {0.0001}

staticprotected

Definition at line 230 of file trajectory_generator.h.

const pilz::LimitsContainer pilz::TrajectoryGenerator::planner_limits_

protected

Definition at line 229 of file trajectory_generator.h.

const robot_model::RobotModelConstPtr pilz::TrajectoryGenerator::robot_model_

protected

Definition at line 228 of file trajectory_generator.h.

constexpr double pilz::TrajectoryGenerator::VELOCITY_TOLERANCE {1e-8}

staticprotected

Definition at line 232 of file trajectory_generator.h.

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

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Private Member Functions

Static Private Member Functions

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation