Class ChompTrajectory

Class Documentation

class ChompTrajectory

Represents a discretized joint-space trajectory for CHOMP.

Public Functions

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.

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.

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.

ChompTrajectory(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const trajectory_msgs::msg::JointTrajectory &traj)
virtual ~ChompTrajectory() = default

Destructor.

inline double &operator()(size_t traj_point, size_t joint)
inline double operator()(size_t traj_point, size_t joint) const
inline Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point)
inline Eigen::MatrixXd::ColXpr getJointTrajectory(int joint)
inline size_t getNumPoints() const

Gets the number of points in the trajectory.

inline size_t getNumFreePoints() const

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

inline size_t getNumJoints() const

Gets the number of joints in each trajectory point.

inline double getDiscretization() const

Gets the discretization time interval of the trajectory.

void fillInMinJerk()

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

Only modifies points from start_index_ to end_index_, inclusive.

void fillInLinearInterpolation()

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

Only modifies points from start_index_ to end_index_, inclusive

void 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

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.

Parameters:

res –

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.

Parameters:

  • source – The source RobotState

  • chomp_trajectory_point – index of the chomp_trajectory’s point (row)

  • group – JointModelGroup determining the joints to copy

inline void setStartEndIndex(size_t start_index, size_t end_index)

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

inline size_t getStartIndex() const

Gets the start index.

inline size_t getEndIndex() const

Gets the end index.

inline Eigen::MatrixXd &getTrajectory()

Gets the entire trajectory matrix.

inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeTrajectoryBlock()

Gets the block of the trajectory which can be optimized.

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

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

void updateFromGroupTrajectory(const ChompTrajectory &group_trajectory)

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

inline size_t getFullTrajectoryIndex(size_t i) const

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

template<typename Derived>
void getJointVelocities(size_t traj_point, Eigen::MatrixBase<Derived> &velocities)

Gets the joint velocities at the given trajectory point.

inline double getDuration() const