moveit2
The MoveIt Motion Planning Framework for ROS 2.
chomp_trajectory.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan */
36 
37 #pragma once
38 
41 
42 #include <eigen3/Eigen/Core>
43 #include <moveit_msgs/msg/motion_plan_detailed_response.hpp>
44 #include <moveit_msgs/msg/motion_plan_request.hpp>
45 #include <trajectory_msgs/msg/joint_trajectory.hpp>
46 #include <vector>
47 
48 namespace chomp
49 {
54 {
55 public:
59  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration, double discretization,
60  const std::string& group_name);
61 
65  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points, double discretization,
66  const std::string& group_name);
67 
72  ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length);
73 
74  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name,
75  const trajectory_msgs::msg::JointTrajectory& traj);
76 
80  virtual ~ChompTrajectory() = default;
81 
82  double& operator()(size_t traj_point, size_t joint);
83 
84  double operator()(size_t traj_point, size_t joint) const;
85 
86  Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point);
87 
88  Eigen::MatrixXd::ColXpr getJointTrajectory(int joint);
89 
93  size_t getNumPoints() const;
94 
98  size_t getNumFreePoints() const;
99 
103  size_t getNumJoints() const;
104 
108  double getDiscretization() const;
109 
115  void fillInMinJerk();
116 
123 
130 
137 
145  void assignCHOMPTrajectoryPointFromRobotState(const moveit::core::RobotState& source, size_t chomp_trajectory_point,
147 
154  void setStartEndIndex(size_t start_index, size_t end_index);
155 
159  size_t getStartIndex() const;
160 
164  size_t getEndIndex() const;
165 
169  Eigen::MatrixXd& getTrajectory();
170 
174  Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeTrajectoryBlock();
175 
179  Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeJointTrajectoryBlock(size_t joint);
180 
184  void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory);
185 
189  size_t getFullTrajectoryIndex(size_t i) const;
190 
194  template <typename Derived>
195  void getJointVelocities(size_t traj_point, Eigen::MatrixBase<Derived>& velocities);
196 
197  double getDuration() const;
198 
199 private:
200  void init();
202  std::string planning_group_name_; //< Planning group that this trajectory corresponds to, if any
203  size_t num_points_; //< Number of points in the trajectory
204  size_t num_joints_; //< Number of joints in each trajectory point
205  double discretization_; //< Discretization of the trajectory
206  double duration_; //< Duration of the trajectory
207  Eigen::MatrixXd trajectory_; //< Storage for the actual trajectory
208  size_t start_index_; // Start index (inclusive) of trajectory to be optimized (everything before will be ignored)
209  size_t end_index_; //< End index (inclusive) of trajectory to be optimized (everything after will be ignored)
210  std::vector<size_t> full_trajectory_index_; //< If this is a "group" trajectory, the indices from the original traj
211 };
212 
214 
215 inline double& ChompTrajectory::operator()(size_t traj_point, size_t joint)
216 {
217  return trajectory_(traj_point, joint);
218 }
219 
220 inline double ChompTrajectory::operator()(size_t traj_point, size_t joint) const
221 {
222  return trajectory_(traj_point, joint);
223 }
224 
225 inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point)
226 {
227  return trajectory_.row(traj_point);
228 }
229 
230 inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint)
231 {
232  return trajectory_.col(joint);
233 }
234 
235 inline size_t ChompTrajectory::getNumPoints() const
236 {
237  return num_points_;
238 }
239 
241 {
242  return (end_index_ - start_index_) + 1;
243 }
244 
245 inline size_t ChompTrajectory::getNumJoints() const
246 {
247  return num_joints_;
248 }
249 
251 {
252  return discretization_;
253 }
254 
255 inline void ChompTrajectory::setStartEndIndex(size_t start_index, size_t end_index)
256 {
257  start_index_ = start_index;
258  end_index_ = end_index;
259 }
260 
261 inline size_t ChompTrajectory::getStartIndex() const
262 {
263  return start_index_;
264 }
265 
266 inline size_t ChompTrajectory::getEndIndex() const
267 {
268  return end_index_;
269 }
270 
271 inline Eigen::MatrixXd& ChompTrajectory::getTrajectory()
272 {
273  return trajectory_;
274 }
275 
276 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> ChompTrajectory::getFreeTrajectoryBlock()
277 {
278  return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints());
279 }
280 
281 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic>
283 {
284  return trajectory_.block(start_index_, joint, getNumFreePoints(), 1);
285 }
286 
287 inline size_t ChompTrajectory::getFullTrajectoryIndex(size_t i) const
288 {
289  return full_trajectory_index_[i];
290 }
291 
292 template <typename Derived>
293 void ChompTrajectory::getJointVelocities(size_t traj_point, Eigen::MatrixBase<Derived>& velocities)
294 {
295  velocities.setZero();
296  double inv_time = 1.0 / discretization_;
297 
298  for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; ++k)
299  {
300  velocities += (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose();
301  }
302 }
303 
304 inline double ChompTrajectory::getDuration() const
305 {
306  return duration_;
307 }
308 } // namespace chomp
Represents a discretized joint-space trajectory for CHOMP.
Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > getFreeTrajectoryBlock()
Gets the block of the trajectory which can be optimized.
double getDuration() const
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, const std::string &group_name, const trajectory_msgs::msg::JointTrajectory &traj)
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.
void updateFromGroupTrajectory(const ChompTrajectory &group_trajectory)
Updates the full trajectory (*this) from the group trajectory.
size_t getEndIndex() const
Gets the end index.
size_t getStartIndex() const
Gets the start index.
double getDiscretization() const
Gets the discretization time interval of the trajectory.
size_t getNumPoints() const
Gets the number of points in the trajectory.
void fillInCubicInterpolation()
Generates a cubic interpolation of the trajectory from the start index to end index.
bool fillInFromTrajectory(const robot_trajectory::RobotTrajectory &trajectory)
Receives the path obtained from a given MotionPlanDetailedResponse res object's trajectory (e....
Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > getFreeJointTrajectoryBlock(size_t joint)
Gets the block of free (optimizable) trajectory for a single joint.
void getJointVelocities(size_t traj_point, Eigen::MatrixBase< Derived > &velocities)
Gets the joint velocities at the given trajectory point.
size_t getNumJoints() const
Gets the number of joints in each trajectory point.
Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point)
virtual ~ChompTrajectory()=default
Destructor.
size_t getFullTrajectoryIndex(size_t i) const
Gets the index in the full trajectory which was copied to this group trajectory.
Eigen::MatrixXd::ColXpr getJointTrajectory(int joint)
void fillInLinearInterpolation()
Generates a linearly interpolated trajectory from the start index to end index.
Eigen::MatrixXd & getTrajectory()
Gets the entire trajectory matrix.
void fillInMinJerk()
Generates a minimum jerk trajectory from the start index to end index.
size_t getNumFreePoints() const
Gets the number of points (that are free to be optimized) in the trajectory.
double & operator()(size_t traj_point, size_t joint)
void setStartEndIndex(size_t start_index, size_t end_index)
Sets the start and end index for the modifiable part of the trajectory.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Maintain a sequence of waypoints and the time durations between these waypoints.