Represents a discretized joint-space trajectory for CHOMP. More...

#include <chomp_trajectory.h>

Public Member Functions
void	assignCHOMPTrajectoryPointFromInputTrajectoryPoint (moveit_msgs::RobotTrajectory trajectory_msg, int num_joints_trajectory, int trajectory_msgs_point, int chomp_trajectory_point)

	ChompTrajectory (const moveit::core::RobotModelConstPtr &robot_model, double duration, double discretization, std::string groupName)
	Constructs a trajectory for a given robot model, trajectory duration, and discretization. More...

	ChompTrajectory (const moveit::core::RobotModelConstPtr &robot_model, int num_points, double discretization, std::string groupName)
	Constructs a trajectory for a given robot model, number of trajectory points, and discretization. More...

	ChompTrajectory (const ChompTrajectory &source_traj, const std::string &planning_group, int diff_rule_length)
	Creates a new containing only the joints of interest, and adds padding to the start and end if needed, to have enough trajectory points for the differentiation rules. More...

	ChompTrajectory (const moveit::core::RobotModelConstPtr &robot_model, const std::string &planning_group, const trajectory_msgs::JointTrajectory &traj)

void	fillInCubicInterpolation ()
	Generates a cubic interpolation of the trajectory from the start index to end index. More...

bool	fillInFromTrajectory (moveit_msgs::MotionPlanDetailedResponse &res)
	Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e.g., trajectory produced by OMPL) and puts it into the appropriate trajectory format required for CHOMP. More...

void	fillInLinearInterpolation ()
	Generates a linearly interpolated trajectory from the start index to end index. More...

void	fillInMinJerk ()
	Generates a minimum jerk trajectory from the start index to end index. More...

double	getDiscretization () const
	Gets the discretization time interval of the trajectory. More...

double	getDuration () const

int	getEndIndex () const
	Gets the end index. More...

Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic >	getFreeJointTrajectoryBlock (int joint)
	Gets the block of free (optimizable) trajectory for a single joint. More...

Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic >	getFreeTrajectoryBlock ()
	Gets the block of the trajectory which can be optimized. More...

int	getFullTrajectoryIndex (int i) const
	Gets the index in the full trajectory which was copied to this group trajectory. More...

Eigen::MatrixXd::ColXpr	getJointTrajectory (int joint)

template<typename Derived >
void	getJointVelocities (int traj_point, Eigen::MatrixBase< Derived > &velocities)
	Gets the joint velocities at the given trajectory point. More...

int	getNumFreePoints () const
	Gets the number of points (that are free to be optimized) in the trajectory. More...

int	getNumJoints () const
	Gets the number of joints in each trajectory point. More...

int	getNumPoints () const
	Gets the number of points in the trajectory. More...

int	getStartIndex () const
	Gets the start index. More...

Eigen::MatrixXd &	getTrajectory ()
	Gets the entire trajectory matrix. More...

Eigen::MatrixXd::RowXpr	getTrajectoryPoint (int traj_point)

double &	operator() (int traj_point, int joint)

double	operator() (int traj_point, int joint) const

void	overwriteTrajectory (const trajectory_msgs::JointTrajectory &traj)

void	setStartEndIndex (int start_index, int end_index)
	Sets the start and end index for the modifiable part of the trajectory. More...

void	updateFromGroupTrajectory (const ChompTrajectory &group_trajectory)
	Updates the full trajectory (*this) from the group trajectory. More...

virtual	~ChompTrajectory ()
	Destructor. More...

Private Member Functions
void	init ()
	Allocates memory for the trajectory. More...

Private Attributes
double	discretization_

double	duration_

int	end_index_

std::vector< int >	full_trajectory_index_

int	num_joints_

int	num_points_

std::string	planning_group_name_

int	start_index_

Eigen::MatrixXd	trajectory_

Detailed Description

Represents a discretized joint-space trajectory for CHOMP.

Definition at line 55 of file chomp_trajectory.h.

Constructor & Destructor Documentation

chomp::ChompTrajectory::ChompTrajectory	(	const moveit::core::RobotModelConstPtr &	robot_model,
		double	duration,
		double	discretization,
		std::string	groupName
	)

Constructs a trajectory for a given robot model, trajectory duration, and discretization.

Definition at line 43 of file chomp_trajectory.cpp.

chomp::ChompTrajectory::ChompTrajectory	(	const moveit::core::RobotModelConstPtr &	robot_model,
		int	num_points,
		double	discretization,
		std::string	groupName
	)

Constructs a trajectory for a given robot model, number of trajectory points, and discretization.

Definition at line 57 of file chomp_trajectory.cpp.

chomp::ChompTrajectory::ChompTrajectory	(	const ChompTrajectory &	source_traj,
		const std::string &	planning_group,
		int	diff_rule_length
	)

Creates a new containing only the joints of interest, and adds padding to the start and end if needed, to have enough trajectory points for the differentiation rules.

Definition at line 71 of file chomp_trajectory.cpp.

chomp::ChompTrajectory::ChompTrajectory	(	const moveit::core::RobotModelConstPtr &	robot_model,
		const std::string &	planning_group,
		const trajectory_msgs::JointTrajectory &	traj
	)

Definition at line 108 of file chomp_trajectory.cpp.

chomp::ChompTrajectory::~ChompTrajectory ( )

virtual

Destructor.

Definition at line 142 of file chomp_trajectory.cpp.

Member Function Documentation

void chomp::ChompTrajectory::assignCHOMPTrajectoryPointFromInputTrajectoryPoint	(	moveit_msgs::RobotTrajectory	trajectory_msg,
		int	num_joints_trajectory,
		int	trajectory_msgs_point,
		int	chomp_trajectory_point
	)

This function assigns the chomp_trajectory row / robot pose at index 'chomp_trajectory_point' obtained from input trajectory_msgs at index 'trajectory_msgs_point'

Parameters

trajectory_msg	the input trajectory_msg
num_joints_trajectory	number of joints in the given robot trajectory
trajectory_msgs_point	index of the input trajectory_msg's point to get joint values from
chomp_trajectory_point	index of the chomp_trajectory's point to get joint values from

Definition at line 313 of file chomp_trajectory.cpp.

void chomp::ChompTrajectory::fillInCubicInterpolation ( )

Generates a cubic interpolation of the trajectory from the start index to end index.

Only modifies points from start_index_ to end_index_, inclusive

Definition at line 187 of file chomp_trajectory.cpp.

bool chomp::ChompTrajectory::fillInFromTrajectory ( moveit_msgs::MotionPlanDetailedResponse & res )

Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e.g., trajectory produced by OMPL) and puts it into the appropriate trajectory format required for CHOMP.

Parameters

res

Definition at line 255 of file chomp_trajectory.cpp.

void chomp::ChompTrajectory::fillInLinearInterpolation ( )

Generates a linearly interpolated trajectory from the start index to end index.

Only modifies points from start_index_ to end_index_, inclusive

Definition at line 173 of file chomp_trajectory.cpp.

void chomp::ChompTrajectory::fillInMinJerk ( )

Generates a minimum jerk trajectory from the start index to end index.

Only modifies points from start_index_ to end_index_, inclusive.

Definition at line 209 of file chomp_trajectory.cpp.

double chomp::ChompTrajectory::getDiscretization ( ) const

inline

Gets the discretization time interval of the trajectory.

Definition at line 259 of file chomp_trajectory.h.

double chomp::ChompTrajectory::getDuration ( ) const

inline

Definition at line 313 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getEndIndex ( ) const

inline

Gets the end index.

Definition at line 275 of file chomp_trajectory.h.

Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > chomp::ChompTrajectory::getFreeJointTrajectoryBlock ( int joint )

inline

Gets the block of free (optimizable) trajectory for a single joint.

Definition at line 291 of file chomp_trajectory.h.

Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > chomp::ChompTrajectory::getFreeTrajectoryBlock ( )

inline

Gets the block of the trajectory which can be optimized.

Definition at line 285 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getFullTrajectoryIndex ( int i ) const

inline

Gets the index in the full trajectory which was copied to this group trajectory.

Definition at line 296 of file chomp_trajectory.h.

Eigen::MatrixXd::ColXpr chomp::ChompTrajectory::getJointTrajectory ( int joint )

inline

Definition at line 239 of file chomp_trajectory.h.

template<typename Derived >

void chomp::ChompTrajectory::getJointVelocities	(	int	traj_point,
		Eigen::MatrixBase< Derived > &	velocities
	)

Gets the joint velocities at the given trajectory point.

Definition at line 302 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getNumFreePoints ( ) const

inline

Gets the number of points (that are free to be optimized) in the trajectory.

Definition at line 249 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getNumJoints ( ) const

inline

Gets the number of joints in each trajectory point.

Definition at line 254 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getNumPoints ( ) const

inline

Gets the number of points in the trajectory.

Definition at line 244 of file chomp_trajectory.h.

int chomp::ChompTrajectory::getStartIndex ( ) const

inline

Gets the start index.

Definition at line 270 of file chomp_trajectory.h.

Eigen::MatrixXd & chomp::ChompTrajectory::getTrajectory ( )

inline

Gets the entire trajectory matrix.

Definition at line 280 of file chomp_trajectory.h.

Eigen::MatrixXd::RowXpr chomp::ChompTrajectory::getTrajectoryPoint ( int traj_point )

inline

Definition at line 234 of file chomp_trajectory.h.

void chomp::ChompTrajectory::init ( )

private

Allocates memory for the trajectory.

Definition at line 157 of file chomp_trajectory.cpp.

double & chomp::ChompTrajectory::operator()	(	int	traj_point,
		int	joint
	)

inline

Definition at line 224 of file chomp_trajectory.h.

double chomp::ChompTrajectory::operator()	(	int	traj_point,
		int	joint
	)		const

inline

Definition at line 229 of file chomp_trajectory.h.

void chomp::ChompTrajectory::overwriteTrajectory ( const trajectory_msgs::JointTrajectory & traj )

Definition at line 146 of file chomp_trajectory.cpp.

void chomp::ChompTrajectory::setStartEndIndex	(	int	start_index,
		int	end_index
	)

inline

Sets the start and end index for the modifiable part of the trajectory.

(Everything before the start and after the end index is considered fixed) The values default to 1 and getNumPoints()-2

Definition at line 264 of file chomp_trajectory.h.

void chomp::ChompTrajectory::updateFromGroupTrajectory ( const ChompTrajectory & group_trajectory )

Updates the full trajectory (*this) from the group trajectory.

Definition at line 163 of file chomp_trajectory.cpp.

Member Data Documentation

double chomp::ChompTrajectory::discretization_

private

Discretization of the trajectory

Definition at line 211 of file chomp_trajectory.h.

double chomp::ChompTrajectory::duration_

private

Duration of the trajectory

Definition at line 212 of file chomp_trajectory.h.

int chomp::ChompTrajectory::end_index_

private

End index (inclusive) of trajectory to be optimized (everything after it will not be modified)

Definition at line 216 of file chomp_trajectory.h.

std::vector<int> chomp::ChompTrajectory::full_trajectory_index_

private

If this is a "group" trajectory, the index from the original traj which each element here was copied

Definition at line 217 of file chomp_trajectory.h.

int chomp::ChompTrajectory::num_joints_

private

Number of joints in each trajectory point

Definition at line 210 of file chomp_trajectory.h.

int chomp::ChompTrajectory::num_points_

private

Number of points in the trajectory

Definition at line 209 of file chomp_trajectory.h.

std::string chomp::ChompTrajectory::planning_group_name_

private

Planning group that this trajectory corresponds to, if any

Definition at line 208 of file chomp_trajectory.h.

int chomp::ChompTrajectory::start_index_

private

Start index (inclusive) of trajectory to be optimized (everything before it will not be modified)

Definition at line 214 of file chomp_trajectory.h.

Eigen::MatrixXd chomp::ChompTrajectory::trajectory_

private

Storage for the actual trajectory

Definition at line 213 of file chomp_trajectory.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation