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

#include <chomp_trajectory.h>

Public Member Functions
void	assignCHOMPTrajectoryPointFromRobotState (const moveit::core::RobotState &source, size_t chomp_trajectory_point, const moveit::core::JointModelGroup *group)
	This function assigns the given source RobotState to the row at index chomp_trajectory_point. More...

	ChompTrajectory (const ChompTrajectory &source_traj, const std::string &group_name, 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 &group_name, const trajectory_msgs::JointTrajectory &traj)

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

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

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

bool	fillInFromTrajectory (const robot_trajectory::RobotTrajectory &trajectory)
	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

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

Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic >	getFreeJointTrajectoryBlock (size_t 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...

size_t	getFullTrajectoryIndex (size_t 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 (size_t traj_point, Eigen::MatrixBase< Derived > &velocities)
	Gets the joint velocities at the given trajectory point. More...

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

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

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

size_t	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() (size_t traj_point, size_t joint)

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

void	setStartEndIndex (size_t start_index, size_t 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 ()=default
	Destructor. More...

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

Private Attributes
double	discretization_

double	duration_

size_t	end_index_

std::vector< size_t >	full_trajectory_index_

size_t	num_joints_

size_t	num_points_

std::string	planning_group_name_

size_t	start_index_

Eigen::MatrixXd	trajectory_

Detailed Description

Represents a discretized joint-space trajectory for CHOMP.

Definition at line 86 of file chomp_trajectory.h.

Constructor & Destructor Documentation

◆ ChompTrajectory() [1/4]

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

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

Definition at line 74 of file chomp_trajectory.cpp.

◆ ChompTrajectory() [2/4]

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

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

Definition at line 80 of file chomp_trajectory.cpp.

◆ ChompTrajectory() [3/4]

chomp::ChompTrajectory::ChompTrajectory	(	const ChompTrajectory &	source_traj,
		const std::string &	group_name,
		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 94 of file chomp_trajectory.cpp.

◆ ChompTrajectory() [4/4]

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

◆ ~ChompTrajectory()

virtual chomp::ChompTrajectory::~ChompTrajectory ( )

virtualdefault

Destructor.

Member Function Documentation

◆ assignCHOMPTrajectoryPointFromRobotState()

void chomp::ChompTrajectory::assignCHOMPTrajectoryPointFromRobotState	(	const moveit::core::RobotState &	source,
		size_t	chomp_trajectory_point,
		const moveit::core::JointModelGroup *	group
	)

This function assigns the given source RobotState to the row at index chomp_trajectory_point.

Parameters

source	The source RobotState
chomp_trajectory_point	index of the chomp_trajectory's point (row)
group	JointModelGroup determining the joints to copy

Definition at line 244 of file chomp_trajectory.cpp.

◆ fillInCubicInterpolation()

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 153 of file chomp_trajectory.cpp.

◆ fillInFromTrajectory()

bool chomp::ChompTrajectory::fillInFromTrajectory ( const robot_trajectory::RobotTrajectory & trajectory )

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 221 of file chomp_trajectory.cpp.

◆ fillInLinearInterpolation()

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 139 of file chomp_trajectory.cpp.

◆ fillInMinJerk()

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 175 of file chomp_trajectory.cpp.

◆ getDiscretization()

double chomp::ChompTrajectory::getDiscretization ( ) const

inline

Gets the discretization time interval of the trajectory.

Definition at line 283 of file chomp_trajectory.h.

◆ getDuration()

double chomp::ChompTrajectory::getDuration ( ) const

inline

Definition at line 337 of file chomp_trajectory.h.

◆ getEndIndex()

size_t chomp::ChompTrajectory::getEndIndex ( ) const

inline

Gets the end index.

Definition at line 299 of file chomp_trajectory.h.

◆ getFreeJointTrajectoryBlock()

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

inline

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

Definition at line 315 of file chomp_trajectory.h.

◆ getFreeTrajectoryBlock()

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 309 of file chomp_trajectory.h.

◆ getFullTrajectoryIndex()

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

inline

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

Definition at line 320 of file chomp_trajectory.h.

◆ getJointTrajectory()

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

inline

Definition at line 263 of file chomp_trajectory.h.

◆ getJointVelocities()

template<typename Derived >

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

Gets the joint velocities at the given trajectory point.

Definition at line 326 of file chomp_trajectory.h.

◆ getNumFreePoints()

size_t chomp::ChompTrajectory::getNumFreePoints ( ) const

inline

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

Definition at line 273 of file chomp_trajectory.h.

◆ getNumJoints()

size_t chomp::ChompTrajectory::getNumJoints ( ) const

inline

Gets the number of joints in each trajectory point.

Definition at line 278 of file chomp_trajectory.h.

◆ getNumPoints()

size_t chomp::ChompTrajectory::getNumPoints ( ) const

inline

Gets the number of points in the trajectory.

Definition at line 268 of file chomp_trajectory.h.

◆ getStartIndex()

size_t chomp::ChompTrajectory::getStartIndex ( ) const

inline

Gets the start index.

Definition at line 294 of file chomp_trajectory.h.

◆ getTrajectory()

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

inline

Gets the entire trajectory matrix.

Definition at line 304 of file chomp_trajectory.h.

◆ getTrajectoryPoint()

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

inline

Definition at line 258 of file chomp_trajectory.h.

◆ init()

void chomp::ChompTrajectory::init ( )

private

Allocates memory for the trajectory.

Definition at line 127 of file chomp_trajectory.cpp.

◆ operator()() [1/2]

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

inline

Definition at line 248 of file chomp_trajectory.h.

◆ operator()() [2/2]

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

inline

Definition at line 253 of file chomp_trajectory.h.

◆ setStartEndIndex()

void chomp::ChompTrajectory::setStartEndIndex	(	size_t	start_index,
		size_t	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 288 of file chomp_trajectory.h.

◆ updateFromGroupTrajectory()

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

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

Definition at line 132 of file chomp_trajectory.cpp.

Member Data Documentation

◆ discretization_

double chomp::ChompTrajectory::discretization_

private

Definition at line 270 of file chomp_trajectory.h.

◆ duration_

double chomp::ChompTrajectory::duration_

private

Definition at line 271 of file chomp_trajectory.h.

◆ end_index_

size_t chomp::ChompTrajectory::end_index_

private

Definition at line 274 of file chomp_trajectory.h.

◆ full_trajectory_index_

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

private

Definition at line 275 of file chomp_trajectory.h.

◆ num_joints_

size_t chomp::ChompTrajectory::num_joints_

private

Definition at line 269 of file chomp_trajectory.h.

◆ num_points_

size_t chomp::ChompTrajectory::num_points_

private

Definition at line 268 of file chomp_trajectory.h.

◆ planning_group_name_

std::string chomp::ChompTrajectory::planning_group_name_

private

Definition at line 267 of file chomp_trajectory.h.

◆ start_index_

size_t chomp::ChompTrajectory::start_index_

private

Definition at line 273 of file chomp_trajectory.h.

◆ trajectory_

Eigen::MatrixXd chomp::ChompTrajectory::trajectory_

private

Definition at line 272 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

◆ ChompTrajectory() [1/4]

◆ ChompTrajectory() [2/4]

◆ ChompTrajectory() [3/4]

◆ ChompTrajectory() [4/4]

◆ ~ChompTrajectory()

Member Function Documentation

◆ assignCHOMPTrajectoryPointFromRobotState()

◆ fillInCubicInterpolation()

◆ fillInFromTrajectory()

◆ fillInLinearInterpolation()

◆ fillInMinJerk()

◆ getDiscretization()

◆ getDuration()

◆ getEndIndex()

◆ getFreeJointTrajectoryBlock()

◆ getFreeTrajectoryBlock()

◆ getFullTrajectoryIndex()

◆ getJointTrajectory()

◆ getJointVelocities()

◆ getNumFreePoints()

◆ getNumJoints()

◆ getNumPoints()

◆ getStartIndex()

◆ getTrajectory()

◆ getTrajectoryPoint()

◆ init()

◆ operator()() [1/2]

◆ operator()() [2/2]

◆ setStartEndIndex()

◆ updateFromGroupTrajectory()

Member Data Documentation

◆ discretization_

◆ duration_

◆ end_index_

◆ full_trajectory_index_

◆ num_joints_

◆ num_points_

◆ planning_group_name_

◆ start_index_

◆ trajectory_