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 #ifndef CHOMP_TRAJECTORY_H_
38 #define CHOMP_TRAJECTORY_H_
39 
40 #include <trajectory_msgs/JointTrajectory.h>
43 
44 #include <moveit_msgs/MotionPlanDetailedResponse.h>
45 #include <moveit_msgs/MotionPlanRequest.h>
46 
47 #include <vector>
48 #include <eigen3/Eigen/Core>
49 
50 namespace chomp
51 {
56 {
57 public:
61  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration, double discretization,
62  std::string groupName);
63 
67  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, int num_points, double discretization,
68  std::string groupName);
69 
74  ChompTrajectory(const ChompTrajectory& source_traj, const std::string& planning_group, int diff_rule_length);
75 
76  ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group,
77  const trajectory_msgs::JointTrajectory& traj);
78 
82  virtual ~ChompTrajectory();
83 
84  double& operator()(int traj_point, int joint);
85 
86  double operator()(int traj_point, int joint) const;
87 
88  Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point);
89 
90  Eigen::MatrixXd::ColXpr getJointTrajectory(int joint);
91 
92  void overwriteTrajectory(const trajectory_msgs::JointTrajectory& traj);
93 
97  int getNumPoints() const;
98 
102  int getNumFreePoints() const;
103 
107  int getNumJoints() const;
108 
112  double getDiscretization() const;
113 
119  void fillInMinJerk();
120 
127 
134 
140  bool fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse& res);
141 
150  void assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg,
151  int num_joints_trajectory, int trajectory_msgs_point,
152  int chomp_trajectory_point);
153 
160  void setStartEndIndex(int start_index, int end_index);
161 
165  int getStartIndex() const;
166 
170  int getEndIndex() const;
171 
175  Eigen::MatrixXd& getTrajectory();
176 
180  Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeTrajectoryBlock();
181 
185  Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> getFreeJointTrajectoryBlock(int joint);
186 
190  void updateFromGroupTrajectory(const ChompTrajectory& group_trajectory);
191 
195  int getFullTrajectoryIndex(int i) const;
196 
200  template <typename Derived>
201  void getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities);
202 
203  double getDuration() const;
204 
205 private:
206  void init();
208  std::string planning_group_name_;
212  double duration_;
213  Eigen::MatrixXd trajectory_;
217  std::vector<int> full_trajectory_index_;
220 };
221 
223 
224 inline double& ChompTrajectory::operator()(int traj_point, int joint)
225 {
226  return trajectory_(traj_point, joint);
227 }
228 
229 inline double ChompTrajectory::operator()(int traj_point, int joint) const
230 {
231  return trajectory_(traj_point, joint);
232 }
233 
234 inline Eigen::MatrixXd::RowXpr ChompTrajectory::getTrajectoryPoint(int traj_point)
235 {
236  return trajectory_.row(traj_point);
237 }
238 
239 inline Eigen::MatrixXd::ColXpr ChompTrajectory::getJointTrajectory(int joint)
240 {
241  return trajectory_.col(joint);
242 }
243 
245 {
246  return num_points_;
247 }
248 
250 {
251  return (end_index_ - start_index_) + 1;
252 }
253 
255 {
256  return num_joints_;
257 }
258 
260 {
261  return discretization_;
262 }
263 
264 inline void ChompTrajectory::setStartEndIndex(int start_index, int end_index)
265 {
266  start_index_ = start_index;
267  end_index_ = end_index;
268 }
269 
271 {
272  return start_index_;
273 }
274 
276 {
277  return end_index_;
278 }
279 
280 inline Eigen::MatrixXd& ChompTrajectory::getTrajectory()
281 {
282  return trajectory_;
283 }
284 
285 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic> ChompTrajectory::getFreeTrajectoryBlock()
286 {
287  return trajectory_.block(start_index_, 0, getNumFreePoints(), getNumJoints());
288 }
289 
290 inline Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic>
292 {
293  return trajectory_.block(start_index_, joint, getNumFreePoints(), 1);
294 }
295 
297 {
298  return full_trajectory_index_[i];
299 }
300 
301 template <typename Derived>
302 void ChompTrajectory::getJointVelocities(int traj_point, Eigen::MatrixBase<Derived>& velocities)
303 {
304  velocities.setZero();
305  double invTime = 1.0 / discretization_;
306 
307  for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++)
308  {
309  velocities += (invTime * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * trajectory_.row(traj_point + k).transpose();
310  }
311 }
312 
313 inline double ChompTrajectory::getDuration() const
314 {
315  return duration_;
316 }
317 }
318 
319 #endif /* CHOMP_TRAJECTORY_H_ */
double & operator()(int traj_point, int joint)
Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_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.
bool fillInFromTrajectory(moveit_msgs::MotionPlanDetailedResponse &res)
Receives the path obtained from a given MotionPlanDetailedResponse res object&#39;s trajectory (e...
virtual ~ChompTrajectory()
Destructor.
void fillInCubicInterpolation()
Generates a cubic interpolation of the trajectory from the start index to end index.
void getJointVelocities(int traj_point, Eigen::MatrixBase< Derived > &velocities)
Gets the joint velocities at the given trajectory point.
std::vector< int > full_trajectory_index_
static const int DIFF_RULE_LENGTH
Definition: chomp_utils.h:46
int getStartIndex() const
Gets the start index.
void assignCHOMPTrajectoryPointFromInputTrajectoryPoint(moveit_msgs::RobotTrajectory trajectory_msg, int num_joints_trajectory, int trajectory_msgs_point, int chomp_trajectory_point)
Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > getFreeJointTrajectoryBlock(int joint)
Gets the block of free (optimizable) trajectory for a single joint.
int getNumPoints() const
Gets the number of points in the trajectory.
Eigen::MatrixXd trajectory_
void init()
Allocates memory for the trajectory.
int getFullTrajectoryIndex(int i) const
Gets the index in the full trajectory which was copied to this group trajectory.
Eigen::MatrixXd::ColXpr getJointTrajectory(int joint)
int getEndIndex() const
Gets the end index.
void fillInMinJerk()
Generates a minimum jerk trajectory from the start index to end index.
Eigen::MatrixXd & getTrajectory()
Gets the entire trajectory matrix.
std::string planning_group_name_
void updateFromGroupTrajectory(const ChompTrajectory &group_trajectory)
Updates the full trajectory (*this) from the group trajectory.
static const double DIFF_RULES[3][DIFF_RULE_LENGTH]
Definition: chomp_utils.h:49
Represents a discretized joint-space trajectory for CHOMP.
double getDuration() const
void overwriteTrajectory(const trajectory_msgs::JointTrajectory &traj)
int getNumFreePoints() const
Gets the number of points (that are free to be optimized) in the trajectory.
void fillInLinearInterpolation()
Generates a linearly interpolated trajectory from the start index to end index.
double getDiscretization() const
Gets the discretization time interval of the trajectory.
void setStartEndIndex(int start_index, int end_index)
Sets the start and end index for the modifiable part of the trajectory.
int getNumJoints() const
Gets the number of joints in each trajectory point.
Eigen::Block< Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic > getFreeTrajectoryBlock()
Gets the block of the trajectory which can be optimized.


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Sun Oct 18 2020 13:17:08