simple_trajectory_generator.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: TKruse
00036  *********************************************************************/
00037 
00038 #include <base_local_planner/simple_trajectory_generator.h>
00039 
00040 #include <cmath>
00041 
00042 #include <base_local_planner/velocity_iterator.h>
00043 
00044 namespace base_local_planner {
00045 
00046 void SimpleTrajectoryGenerator::initialise(
00047     const Eigen::Vector3f& pos,
00048     const Eigen::Vector3f& vel,
00049     const Eigen::Vector3f& goal,
00050     base_local_planner::LocalPlannerLimits* limits,
00051     const Eigen::Vector3f& vsamples,
00052     std::vector<Eigen::Vector3f> additional_samples,
00053     bool discretize_by_time) {
00054   initialise(pos, vel, goal, limits, vsamples, discretize_by_time);
00055   // add static samples if any
00056   sample_params_.insert(sample_params_.end(), additional_samples.begin(), additional_samples.end());
00057 }
00058 
00059 
00060 void SimpleTrajectoryGenerator::initialise(
00061     const Eigen::Vector3f& pos,
00062     const Eigen::Vector3f& vel,
00063     const Eigen::Vector3f& goal,
00064     base_local_planner::LocalPlannerLimits* limits,
00065     const Eigen::Vector3f& vsamples,
00066     bool discretize_by_time) {
00067   /*
00068    * We actually generate all velocity sample vectors here, from which to generate trajectories later on
00069    */
00070   double max_vel_th = limits->max_rot_vel;
00071   double min_vel_th = -1.0 * max_vel_th;
00072   discretize_by_time_ = discretize_by_time;
00073   Eigen::Vector3f acc_lim = limits->getAccLimits();
00074   pos_ = pos;
00075   vel_ = vel;
00076   limits_ = limits;
00077   next_sample_index_ = 0;
00078   sample_params_.clear();
00079 
00080   double min_vel_x = limits->min_vel_x;
00081   double max_vel_x = limits->max_vel_x;
00082   double min_vel_y = limits->min_vel_y;
00083   double max_vel_y = limits->max_vel_y;
00084 
00085   // if sampling number is zero in any dimension, we don't generate samples generically
00086   if (vsamples[0] * vsamples[1] * vsamples[2] > 0) {
00087     //compute the feasible velocity space based on the rate at which we run
00088     Eigen::Vector3f max_vel = Eigen::Vector3f::Zero();
00089     Eigen::Vector3f min_vel = Eigen::Vector3f::Zero();
00090 
00091     if ( ! use_dwa_) {
00092       // there is no point in overshooting the goal, and it also may break the
00093       // robot behavior, so we limit the velocities to those that do not overshoot in sim_time
00094       double dist = hypot(goal[0] - pos[0], goal[1] - pos[1]);
00095       max_vel_x = std::max(std::min(max_vel_x, dist / sim_time_), min_vel_x);
00096       max_vel_y = std::max(std::min(max_vel_y, dist / sim_time_), min_vel_y);
00097 
00098       // if we use continous acceleration, we can sample the max velocity we can reach in sim_time_
00099       max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_time_);
00100       max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_time_);
00101       max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_time_);
00102 
00103       min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_time_);
00104       min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_time_);
00105       min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_time_);
00106     } else {
00107       // with dwa do not accelerate beyond the first step, we only sample within velocities we reach in sim_period
00108       max_vel[0] = std::min(max_vel_x, vel[0] + acc_lim[0] * sim_period_);
00109       max_vel[1] = std::min(max_vel_y, vel[1] + acc_lim[1] * sim_period_);
00110       max_vel[2] = std::min(max_vel_th, vel[2] + acc_lim[2] * sim_period_);
00111 
00112       min_vel[0] = std::max(min_vel_x, vel[0] - acc_lim[0] * sim_period_);
00113       min_vel[1] = std::max(min_vel_y, vel[1] - acc_lim[1] * sim_period_);
00114       min_vel[2] = std::max(min_vel_th, vel[2] - acc_lim[2] * sim_period_);
00115     }
00116 
00117     Eigen::Vector3f vel_samp = Eigen::Vector3f::Zero();
00118     VelocityIterator x_it(min_vel[0], max_vel[0], vsamples[0]);
00119     VelocityIterator y_it(min_vel[1], max_vel[1], vsamples[1]);
00120     VelocityIterator th_it(min_vel[2], max_vel[2], vsamples[2]);
00121     for(; !x_it.isFinished(); x_it++) {
00122       vel_samp[0] = x_it.getVelocity();
00123       for(; !y_it.isFinished(); y_it++) {
00124         vel_samp[1] = y_it.getVelocity();
00125         for(; !th_it.isFinished(); th_it++) {
00126           vel_samp[2] = th_it.getVelocity();
00127           //ROS_DEBUG("Sample %f, %f, %f", vel_samp[0], vel_samp[1], vel_samp[2]);
00128           sample_params_.push_back(vel_samp);
00129         }
00130         th_it.reset();
00131       }
00132       y_it.reset();
00133     }
00134   }
00135 }
00136 
00137 void SimpleTrajectoryGenerator::setParameters(
00138     double sim_time,
00139     double sim_granularity,
00140     double angular_sim_granularity,
00141     bool use_dwa,
00142     double sim_period) {
00143   sim_time_ = sim_time;
00144   sim_granularity_ = sim_granularity;
00145   angular_sim_granularity_ = angular_sim_granularity;
00146   use_dwa_ = use_dwa;
00147   continued_acceleration_ = ! use_dwa_;
00148   sim_period_ = sim_period;
00149 }
00150 
00154 bool SimpleTrajectoryGenerator::hasMoreTrajectories() {
00155   return next_sample_index_ < sample_params_.size();
00156 }
00157 
00161 bool SimpleTrajectoryGenerator::nextTrajectory(Trajectory &comp_traj) {
00162   bool result = false;
00163   if (hasMoreTrajectories()) {
00164     if (generateTrajectory(
00165         pos_,
00166         vel_,
00167         sample_params_[next_sample_index_],
00168         comp_traj)) {
00169       result = true;
00170     }
00171   }
00172   next_sample_index_++;
00173   return result;
00174 }
00175 
00180 bool SimpleTrajectoryGenerator::generateTrajectory(
00181       Eigen::Vector3f pos,
00182       Eigen::Vector3f vel,
00183       Eigen::Vector3f sample_target_vel,
00184       base_local_planner::Trajectory& traj) {
00185   double vmag = hypot(sample_target_vel[0], sample_target_vel[1]);
00186   double eps = 1e-4;
00187   traj.cost_   = -1.0; // placed here in case we return early
00188   //trajectory might be reused so we'll make sure to reset it
00189   traj.resetPoints();
00190 
00191   // make sure that the robot would at least be moving with one of
00192   // the required minimum velocities for translation and rotation (if set)
00193   if ((limits_->min_trans_vel >= 0 && vmag + eps < limits_->min_trans_vel) &&
00194       (limits_->min_rot_vel >= 0 && fabs(sample_target_vel[2]) + eps < limits_->min_rot_vel)) {
00195     return false;
00196   }
00197   // make sure we do not exceed max diagonal (x+y) translational velocity (if set)
00198   if (limits_->max_trans_vel >=0 && vmag - eps > limits_->max_trans_vel) {
00199     return false;
00200   }
00201 
00202   int num_steps;
00203   if (discretize_by_time_) {
00204     num_steps = ceil(sim_time_ / sim_granularity_);
00205   } else {
00206     //compute the number of steps we must take along this trajectory to be "safe"
00207     double sim_time_distance = vmag * sim_time_; // the distance the robot would travel in sim_time if it did not change velocity
00208     double sim_time_angle = fabs(sample_target_vel[2]) * sim_time_; // the angle the robot would rotate in sim_time
00209     num_steps =
00210         ceil(std::max(sim_time_distance / sim_granularity_,
00211             sim_time_angle    / angular_sim_granularity_));
00212   }
00213 
00214   //compute a timestep
00215   double dt = sim_time_ / num_steps;
00216   traj.time_delta_ = dt;
00217 
00218   Eigen::Vector3f loop_vel;
00219   if (continued_acceleration_) {
00220     // assuming the velocity of the first cycle is the one we want to store in the trajectory object
00221     loop_vel = computeNewVelocities(sample_target_vel, vel, limits_->getAccLimits(), dt);
00222     traj.xv_     = loop_vel[0];
00223     traj.yv_     = loop_vel[1];
00224     traj.thetav_ = loop_vel[2];
00225   } else {
00226     // assuming sample_vel is our target velocity within acc limits for one timestep
00227     loop_vel = sample_target_vel;
00228     traj.xv_     = sample_target_vel[0];
00229     traj.yv_     = sample_target_vel[1];
00230     traj.thetav_ = sample_target_vel[2];
00231   }
00232 
00233   //simulate the trajectory and check for collisions, updating costs along the way
00234   for (int i = 0; i < num_steps; ++i) {
00235 
00236     //add the point to the trajectory so we can draw it later if we want
00237     traj.addPoint(pos[0], pos[1], pos[2]);
00238 
00239     if (continued_acceleration_) {
00240       //calculate velocities
00241       loop_vel = computeNewVelocities(sample_target_vel, loop_vel, limits_->getAccLimits(), dt);
00242       //ROS_WARN_NAMED("Generator", "Flag: %d, Loop_Vel %f, %f, %f", continued_acceleration_, loop_vel[0], loop_vel[1], loop_vel[2]);
00243     }
00244 
00245     //update the position of the robot using the velocities passed in
00246     pos = computeNewPositions(pos, loop_vel, dt);
00247 
00248   } // end for simulation steps
00249 
00250   return num_steps > 0; // true if trajectory has at least one point
00251 }
00252 
00253 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewPositions(const Eigen::Vector3f& pos,
00254     const Eigen::Vector3f& vel, double dt) {
00255   Eigen::Vector3f new_pos = Eigen::Vector3f::Zero();
00256   new_pos[0] = pos[0] + (vel[0] * cos(pos[2]) + vel[1] * cos(M_PI_2 + pos[2])) * dt;
00257   new_pos[1] = pos[1] + (vel[0] * sin(pos[2]) + vel[1] * sin(M_PI_2 + pos[2])) * dt;
00258   new_pos[2] = pos[2] + vel[2] * dt;
00259   return new_pos;
00260 }
00261 
00265 Eigen::Vector3f SimpleTrajectoryGenerator::computeNewVelocities(const Eigen::Vector3f& sample_target_vel,
00266     const Eigen::Vector3f& vel, Eigen::Vector3f acclimits, double dt) {
00267   Eigen::Vector3f new_vel = Eigen::Vector3f::Zero();
00268   for (int i = 0; i < 3; ++i) {
00269     if (vel[i] < sample_target_vel[i]) {
00270       new_vel[i] = std::min(double(sample_target_vel[i]), vel[i] + acclimits[i] * dt);
00271     } else {
00272       new_vel[i] = std::max(double(sample_target_vel[i]), vel[i] - acclimits[i] * dt);
00273     }
00274   }
00275   return new_vel;
00276 }
00277 
00278 } /* namespace base_local_planner */


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:38