moveit2
The MoveIt Motion Planning Framework for ROS 2.
chomp_trajectory.cpp
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 
38 
39 namespace chomp
40 {
41 ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, double duration,
42  double discretization, const std::string& group_name)
43  : ChompTrajectory(robot_model, static_cast<size_t>(duration / discretization) + 1, discretization, group_name)
44 {
45 }
46 
47 ChompTrajectory::ChompTrajectory(const moveit::core::RobotModelConstPtr& robot_model, size_t num_points,
48  double discretization, const std::string& group_name)
49  : planning_group_name_(group_name)
50  , num_points_(num_points)
51  , discretization_(discretization)
52  , duration_((num_points - 1) * discretization)
53  , start_index_(1)
54  , end_index_(num_points_ - 2)
55 {
56  const moveit::core::JointModelGroup* model_group = robot_model->getJointModelGroup(planning_group_name_);
57  num_joints_ = model_group->getActiveJointModels().size();
58  init();
59 }
60 
61 ChompTrajectory::ChompTrajectory(const ChompTrajectory& source_traj, const std::string& group_name, int diff_rule_length)
62  : planning_group_name_(group_name), discretization_(source_traj.discretization_)
63 {
64  num_joints_ = source_traj.getNumJoints();
65 
66  // figure out the num_points_:
67  // we need diff_rule_length-1 extra points on either side:
68  int start_extra = (diff_rule_length - 1) - source_traj.start_index_;
69  int end_extra = (diff_rule_length - 1) - ((source_traj.num_points_ - 1) - source_traj.end_index_);
70 
71  num_points_ = source_traj.num_points_ + start_extra + end_extra;
72  start_index_ = diff_rule_length - 1;
73  end_index_ = (num_points_ - 1) - (diff_rule_length - 1);
74  duration_ = (num_points_ - 1) * discretization_;
75 
76  // allocate the memory:
77  init();
78 
79  full_trajectory_index_.resize(num_points_);
80 
81  // now copy the trajectories over:
82  for (size_t i = 0; i < num_points_; ++i)
83  {
84  int source_traj_point = i - start_extra;
85  if (source_traj_point < 0)
86  source_traj_point = 0;
87  if (static_cast<size_t>(source_traj_point) >= source_traj.num_points_)
88  source_traj_point = source_traj.num_points_ - 1;
89  full_trajectory_index_[i] = source_traj_point;
90  getTrajectoryPoint(i) = const_cast<ChompTrajectory&>(source_traj).getTrajectoryPoint(source_traj_point);
91  }
92 }
93 
94 void ChompTrajectory::init()
95 {
96  trajectory_.resize(num_points_, num_joints_);
97 }
98 
100 {
101  size_t num_vars_free = end_index_ - start_index_ + 1;
102  trajectory_.block(start_index_, 0, num_vars_free, num_joints_) =
103  group_trajectory.trajectory_.block(group_trajectory.start_index_, 0, num_vars_free, num_joints_);
104 }
105 
107 {
108  double start_index = start_index_ - 1;
109  double end_index = end_index_ + 1;
110  for (size_t i = 0; i < num_joints_; ++i)
111  {
112  double theta = ((*this)(end_index, i) - (*this)(start_index, i)) / (end_index - 1);
113  for (size_t j = start_index + 1; j < end_index; ++j)
114  {
115  (*this)(j, i) = (*this)(start_index, i) + j * theta;
116  }
117  }
118 }
119 
121 {
122  double start_index = start_index_ - 1;
123  double end_index = end_index_ + 1;
124  double dt = 0.001;
125  std::vector<double> coeffs(4, 0);
126  double total_time = (end_index - 1) * dt;
127  for (size_t i = 0; i < num_joints_; ++i)
128  {
129  coeffs[0] = (*this)(start_index, i);
130  coeffs[2] = (3 / (pow(total_time, 2))) * ((*this)(end_index, i) - (*this)(start_index, i));
131  coeffs[3] = (-2 / (pow(total_time, 3))) * ((*this)(end_index, i) - (*this)(start_index, i));
132 
133  double t;
134  for (size_t j = start_index + 1; j < end_index; ++j)
135  {
136  t = j * dt;
137  (*this)(j, i) = coeffs[0] + coeffs[2] * pow(t, 2) + coeffs[3] * pow(t, 3);
138  }
139  }
140 }
141 
143 {
144  double start_index = start_index_ - 1;
145  double end_index = end_index_ + 1;
146  double td[6]; // powers of the time duration
147  td[0] = 1.0;
148  td[1] = (end_index - start_index) * discretization_;
149 
150  for (unsigned int i = 2; i <= 5; ++i)
151  td[i] = td[i - 1] * td[1];
152 
153  // calculate the spline coefficients for each joint:
154  // (these are for the special case of zero start and end vel and acc)
155  std::vector<std::array<double, 6>> coeff(num_joints_);
156  for (size_t i = 0; i < num_joints_; ++i)
157  {
158  double x0 = (*this)(start_index, i);
159  double x1 = (*this)(end_index, i);
160  coeff[i][0] = x0;
161  coeff[i][1] = 0;
162  coeff[i][2] = 0;
163  coeff[i][3] = (-20 * x0 + 20 * x1) / (2 * td[3]);
164  coeff[i][4] = (30 * x0 - 30 * x1) / (2 * td[4]);
165  coeff[i][5] = (-12 * x0 + 12 * x1) / (2 * td[5]);
166  }
167 
168  // now fill in the joint positions at each time step
169  for (size_t i = start_index + 1; i < end_index; ++i)
170  {
171  double ti[6]; // powers of the time index point
172  ti[0] = 1.0;
173  ti[1] = (i - start_index) * discretization_;
174  for (unsigned int k = 2; k <= 5; ++k)
175  ti[k] = ti[k - 1] * ti[1];
176 
177  for (size_t j = 0; j < num_joints_; ++j)
178  {
179  (*this)(i, j) = 0.0;
180  for (unsigned int k = 0; k <= 5; ++k)
181  {
182  (*this)(i, j) += ti[k] * coeff[j][k];
183  }
184  }
185  }
186 }
187 
189 {
190  // check if input trajectory has less than two states (start and goal), function returns false if condition is true
191  if (trajectory.getWayPointCount() < 2)
192  return false;
193 
194  const size_t max_output_index = getNumPoints() - 1;
195  const size_t max_input_index = trajectory.getWayPointCount() - 1;
196 
197  const moveit::core::JointModelGroup* group = trajectory.getGroup();
198  moveit::core::RobotState interpolated(trajectory.getRobotModel());
199  for (size_t i = 0; i <= max_output_index; ++i)
200  {
201  double fraction = static_cast<double>(i * max_input_index) / max_output_index;
202  const size_t prev_idx = std::trunc(fraction); // integer part
203  fraction = fraction - prev_idx; // fractional part
204  const size_t next_idx = prev_idx == max_input_index ? prev_idx : prev_idx + 1;
205  trajectory.getWayPoint(prev_idx).interpolate(trajectory.getWayPoint(next_idx), fraction, interpolated, group);
207  }
208  return true;
209 }
210 
212  size_t chomp_trajectory_point_index,
214 {
215  Eigen::MatrixXd::RowXpr target = getTrajectoryPoint(chomp_trajectory_point_index);
216  assert(group->getActiveJointModels().size() == static_cast<size_t>(target.cols()));
217  size_t joint_index = 0;
218  for (const moveit::core::JointModel* jm : group->getActiveJointModels())
219  {
220  assert(jm->getVariableCount() == 1);
221  target[joint_index++] = source.getVariablePosition(jm->getFirstVariableIndex());
222  }
223 }
224 
225 } // namespace chomp
Represents a discretized joint-space trajectory for CHOMP.
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.
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 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....
size_t getNumJoints() const
Gets the number of joints in each trajectory point.
Eigen::MatrixXd::RowXpr getTrajectoryPoint(int traj_point)
void fillInLinearInterpolation()
Generates a linearly interpolated trajectory from the start index to end index.
void fillInMinJerk()
Generates a minimum jerk trajectory from the start index to end index.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void interpolate(const RobotState &to, double t, RobotState &state) const
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:207
Maintain a sequence of waypoints and the time durations between these waypoints.
const moveit::core::RobotModelConstPtr & getRobotModel() const
const moveit::core::JointModelGroup * getGroup() const
const moveit::core::RobotState & getWayPoint(std::size_t index) const