OMPLTrajectory provides OMPL path utilities to the Trajectory class. More...

#include <ompl_trajectory.h>

Inheritance diagram for robowflex::OMPL::OMPLTrajectory:

Public Member Functions
	OMPLTrajectory (const RobotConstPtr &robot, const std::string &group)
	Constructor. More...

	OMPLTrajectory (robot_trajectory::RobotTrajectory &trajectory)
	Constructor. More...

ompl::geometric::PathGeometric	toOMPLPath (const ompl::geometric::SimpleSetupPtr &ss)
	Converts to an OMPL Path given an OMPL simple_setup. More...

void	fromOMPLPath (const robot_state::RobotState &reference_state, const ompl::geometric::PathGeometric &path)
	Sets the trajectory from an OMPL Path and a reference state. More...

Public Member Functions inherited from robowflex::Trajectory
	Trajectory (const RobotConstPtr &robot, const std::string &group)
	Constructor for an empty trajectory. More...

	Trajectory (const robot_trajectory::RobotTrajectory &trajectory)
	Constructor from moveit trajectory. More...

	Trajectory (const robot_trajectory::RobotTrajectoryPtr trajectory)
	Constructor from moveit trajectory. More...

void	useMessage (const robot_state::RobotState &reference_state, const moveit_msgs::RobotTrajectory &msg)
	Set the trajectory to be the same as a message. More...

void	useMessage (const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &msg)
	Set the trajectory to be the same as a message. More...

bool	toYAMLFile (const std::string &filename) const
	Dump a trajectory to a file. More...

bool	fromYAMLFile (const robot_state::RobotState &reference_state, const std::string &filename)
	Load a trajectory from a YAML file. More...

const robot_trajectory::RobotTrajectoryPtr &	getTrajectoryConst () const
	Get a const reference to the trajectory. More...

robot_trajectory::RobotTrajectoryPtr &	getTrajectory ()
	Get a reference to the trajectory. More...

moveit_msgs::RobotTrajectory	getMessage () const
	Get the message that describes the trajectory. More...

std::size_t	getNumWaypoints () const
	Returns the number of waypoints of the trajectory. More...

void	addSuffixWaypoint (const robot_state::RobotState &state, double dt=1.)
	Add a waypoint at the end of the trajectory. More...

double	getLength (const PathMetric &metric={}) const
	Get the length of a path. Optionally, a metric can be specified other than the default (the L2 norm). More...

bool	isCollisionFree (const SceneConstPtr &scene) const
	Checks if a path is collsion free. More...

std::tuple< double, double, double >	getClearance (const SceneConstPtr &scene) const
	Get the average, minimum, and maximum clearance of a path. More...

double	getSmoothness (const PathMetric &metric={}) const
	Get the smoothness of a path relative to some metric. See internal function documentation for details. More...

std::map< std::string, double >	getFinalPositions () const
	Returns the joint positions from the last state in a planned trajectory in response. More...

bool	computeTimeParameterization (double max_velocity=1., double max_acceleration=1.)
	Computes the time parameterization of a path according to a desired max velocity or acceleration. More...

void	interpolate (unsigned int count)
	Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states. More...

std::vector< std::vector< double > >	vectorize () const
	Converts a trajectory into a vector of position vectors. The values are in the same order as reported by getJointNames(), which is consistent within MoveIt. More...

std::vector< std::string >	getJointNames () const
	Get the names of the variables that make up this trajectory, in the same order as in MoveIt JointModelGroup. More...

Trajectory &	append (const Trajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
	Adds a specified part of a trajectory to the end of the current trajectory. The default, when start_index or end_index are ommitted, is to add the whole trajectory. More...

Additional Inherited Members
Static Public Member Functions inherited from robowflex::Trajectory
static bool	computeTimeParameterization (robot_trajectory::RobotTrajectory &trajectory, double max_velocity=1., double max_acceleration=1.)
	Computes the time parameterization of a path according to a desired max velocity or acceleration. More...

Protected Attributes inherited from robowflex::Trajectory
robot_trajectory::RobotTrajectoryPtr	trajectory_

Detailed Description

OMPLTrajectory provides OMPL path utilities to the Trajectory class.

Definition at line 37 of file ompl_trajectory.h.

Constructor & Destructor Documentation

◆ OMPLTrajectory() [1/2]

OMPL::OMPLTrajectory::OMPLTrajectory	(	const RobotConstPtr &	robot,
		const std::string &	group
	)

Constructor.

Parameters

[in]	robot	Robot to construct trajectory for.
[in]	group	Planning group of the trajectory.

Definition at line 13 of file ompl_trajectory.cpp.

   : Trajectory(robot, group)
 {
 }

◆ OMPLTrajectory() [2/2]

OMPL::OMPLTrajectory::OMPLTrajectory ( robot_trajectory::RobotTrajectory & trajectory )

Constructor.

Parameters

[in] trajectory Trajectory to initialize with.

Definition at line 18 of file ompl_trajectory.cpp.

                                                                               : Trajectory(trajectory)
 {
 }

Member Function Documentation

◆ fromOMPLPath()

void OMPL::OMPLTrajectory::fromOMPLPath	(	const robot_state::RobotState &	reference_state,
		const ompl::geometric::PathGeometric &	path
	)

Sets the trajectory from an OMPL Path and a reference state.

Parameters

[in]	reference_state	A full state that contains the values for all the joints.
[in]	path	The geometric OMPL path to convert.

Definition at line 38 of file ompl_trajectory.cpp.

 {
     const auto &mbss = std::dynamic_pointer_cast<ompl_interface::ModelBasedStateSpace>(
         path.getSpaceInformation()->getStateSpace());
  
     if (not mbss)
         throw Exception(1, "Failed to extract StateSpace from provided OMPL path!");
  
     moveit::core::RobotState ks = reference_state;
     for (std::size_t i = 0; i < path.getStateCount(); ++i)
     {
         mbss->copyToRobotState(ks, path.getState(i));
         trajectory_->addSuffixWayPoint(ks, 0.0);
     }
 }

◆ toOMPLPath()

ompl::geometric::PathGeometric OMPL::OMPLTrajectory::toOMPLPath ( const ompl::geometric::SimpleSetupPtr & ss )

Converts to an OMPL Path given an OMPL simple_setup.

Parameters

[in] ss a simple setup structure that has the correct stateSpaceInformation.

Returns: The Geometric Path equivalent of the trajectory.

Definition at line 22 of file ompl_trajectory.cpp.

 {
     auto path = ompl::geometric::PathGeometric(ss->getSpaceInformation());
     auto *tstate = ss->getSpaceInformation()->allocState();
     auto traj_msg = getMessage();
  
     // transform to Path geometric
     for (const auto &state_vec : traj_msg.joint_trajectory.points)
         for (unsigned int j = 0; j < state_vec.positions.size(); j++)
             tstate->as<ompl_interface::ModelBasedStateSpace::StateType>()->values[j] = state_vec.positions[j];
  
     path.append(tstate);
  
     return path;
 }

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

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ OMPLTrajectory() [1/2]

◆ OMPLTrajectory() [2/2]

Member Function Documentation

◆ fromOMPLPath()

◆ toOMPLPath()