simple_trajectory_generator.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: TKruse
36  *********************************************************************/
37 
39 
40 #include <cmath>
41 
43 
44 namespace base_local_planner {
45 
47  const Eigen::Vector3f& pos,
48  const Eigen::Vector3f& vel,
49  const Eigen::Vector3f& goal,
51  const Eigen::Vector3f& vsamples,
52  std::vector<Eigen::Vector3f> additional_samples,
53  bool discretize_by_time) {
54  initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
55  // add static samples if any
56  sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
57 }
58 
59 
61  const Eigen::Vector3f& pos,
62  const Eigen::Vector3f& vel,
63  const Eigen::Vector3f& goal,
65  const Eigen::Vector3f& vsamples,
66  bool discretize_by_time) {
67  /*
68  * We actually generate all velocity sample vectors here, from which to generate trajectories later on
69  */
70  double max_vel_th = limits->max_rot_vel;
71  double min_vel_th = -1.0 * max_vel_th;
72  discretize_by_time_ = discretize_by_time;
73  Eigen::Vector3f acc_lim = limits->getAccLimits();
74  pos_ = pos;
75  vel_ = vel;
76  limits_ = limits;
78  sample_params_.clear();
79 
80  double min_vel_x = limits->min_vel_x;
81  double max_vel_x = limits->max_vel_x;
82  double min_vel_y = limits->min_vel_y;
83  double max_vel_y = limits->max_vel_y;
84 
85  // if sampling number is zero in any dimension, we don't generate samples generically
86  if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
87  //compute the feasible velocity space based on the rate at which we run
88  Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
89  Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
90 
91  if ( ! use_dwa_) {
92  // there is no point in overshooting the goal, and it also may break the
93  // robot behavior, so we limit the velocities to those that do not overshoot in sim_time
94  double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
95  max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
96  max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
97 
98  // if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
99  max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
100  max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
101  max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
102 
103  min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
104  min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
105  min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
106  } else {
107  // with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
108  max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
109  max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
110  max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
111 
112  min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
113  min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
114  min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
115  }
116 
117  Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
118  VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
119  VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
120  VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
121  for(; !x_it.isFinished(); x_it++) {
122  vel_samp[0] = x_it.getVelocity();
123  for(; !y_it.isFinished(); y_it++) {
124  vel_samp[1] = y_it.getVelocity();
125  for(; !th_it.isFinished(); th_it++) {
126  vel_samp[2] = th_it.getVelocity();
127  //ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
128  sample_params_.push_back(vel_samp);
129  }
130  th_it.reset();
131  }
132  y_it.reset();
133  }
134  }
135 }
136 
138  double sim_time,
139  double sim_granularity,
140  double angular_sim_granularity,
141  bool use_dwa,
142  double sim_period) {
143  sim_time_ = sim_time;
144  sim_granularity_ = sim_granularity;
145  angular_sim_granularity_ = angular_sim_granularity;
146  use_dwa_ = use_dwa;
148  sim_period_ = sim_period;
149 }
150 
155  return next_sample_index_ < sample_params_.size();
156 }
157 
162  bool result = false;
163  if (hasMoreTrajectories()) {
164  if (generateTrajectory(
165  pos_,
166  vel_,
168  comp_traj)) {
169  result = true;
170  }
171  }
173  return result;
174 }
175 
181  Eigen::Vector3f pos,
182  Eigen::Vector3f vel,
183  Eigen::Vector3f sample_target_vel,
185  double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
186  double eps = 1e-4;
187  traj.cost_ = -1.0; // placed here in case we return early
188  //trajectory might be reused so we'll make sure to reset it
189  traj.resetPoints();
190 
191  // make sure that the robot would at least be moving with one of
192  // the required minimum velocities for translation and rotation (if set)
193  if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
194  (limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
195  return false;
196  }
197  // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
198  if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
199  return false;
200  }
201 
202  int num_steps;
203  if (discretize_by_time_) {
204  num_steps = ceil(sim_time_ / sim_granularity_);
205  } else {
206  //compute the number of steps we must take along this trajectory to be "safe"
207  double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
208  double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
209  num_steps =
210  ceil(std::max(sim_time_distance / sim_granularity_,
211  sim_time_angle / angular_sim_granularity_));
212  }
213 
214  //compute a timestep
215  double dt = sim_time_ / num_steps;
216  traj.time_delta_ = dt;
217 
218  Eigen::Vector3f loop_vel;
220  // assuming the velocity of the first cycle is the one we want to store in the trajectory object
221  loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
222  traj.xv_ = loop_vel[0];
223  traj.yv_ = loop_vel[1];
224  traj.thetav_ = loop_vel[2];
225  } else {
226  // assuming sample_vel is our target velocity within acc limits for one timestep
227  loop_vel = sample_target_vel;
228  traj.xv_ = sample_target_vel[0];
229  traj.yv_ = sample_target_vel[1];
230  traj.thetav_ = sample_target_vel[2];
231  }
232 
233  //simulate the trajectory and check for collisions, updating costs along the way
234  for (int i = 0; i < num_steps; ++i) {
235 
236  //add the point to the trajectory so we can draw it later if we want
237  traj.addPoint(pos[0], pos[1], pos[2]);
238 
240  //calculate velocities
241  loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
242  //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
243  }
244 
245  //update the position of the robot using the velocities passed in
246  pos = computeNewPositions(pos, loop_vel, dt);
247 
248  } // end for simulation steps
249 
250  return num_steps > 0; // true if trajectory has at least one point
251 }
252 
253 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f& pos,
254  const Eigen::Vector3f& vel, double dt) {
255  Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
256  new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
257  new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
258  new_pos[2] = pos[2] + vel[2] * dt;
259  return new_pos;
260 }
261 
265 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
266  const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt) {
267  Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
268  for (int i = 0; i < 3; ++i) {
269  if (vel[i] < sample_target_vel[i]) {
270  new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
271  } else {
272  new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
273  }
274  }
275  return new_vel;
276 }
277 
278 } /* namespace base_local_planner */
void initialise(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, const Eigen::Vector3f &goal, base_local_planner::LocalPlannerLimits *limits, const Eigen::Vector3f &vsamples, std::vector< Eigen::Vector3f > additional_samples, bool discretize_by_time=false)
Eigen::Vector3f getAccLimits()
Get the acceleration limits of the robot.
bool generateTrajectory(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f sample_target_vel, base_local_planner::Trajectory &traj)
static Eigen::Vector3f computeNewVelocities(const Eigen::Vector3f &sample_target_vel, const Eigen::Vector3f &vel, Eigen::Vector3f acclimits, double dt)
double time_delta_
The time gap between points.
Definition: trajectory.h:64
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
double cost_
The cost/score of the trajectory.
Definition: trajectory.h:62
void resetPoints()
Clear the trajectory&#39;s points.
Definition: trajectory.cpp:65
double thetav_
The x, y, and theta velocities of the trajectory.
Definition: trajectory.h:60
static Eigen::Vector3f computeNewPositions(const Eigen::Vector3f &pos, const Eigen::Vector3f &vel, double dt)
base_local_planner::LocalPlannerLimits * limits_
void setParameters(double sim_time, double sim_granularity, double angular_sim_granularity, bool use_dwa=false, double sim_period=0.0)
void addPoint(double x, double y, double th)
Add a point to the end of a trajectory.
Definition: trajectory.cpp:59
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49