timed_elastic_band.hpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
40 
41 namespace teb_local_planner
42 {
43 
44 
45 
46 template<typename BidirIter, typename Fun>
47 bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta,
48  boost::optional<double> max_acc_x, boost::optional<double> max_acc_theta,
49  boost::optional<double> start_orientation, boost::optional<double> goal_orientation, int min_samples, bool guess_backwards_motion)
50 {
51  Eigen::Vector2d start_position = fun_position( *path_start );
52  Eigen::Vector2d goal_position = fun_position( *boost::prior(path_end) );
53 
54  bool backwards = false;
55 
56  double start_orient, goal_orient;
57  if (start_orientation)
58  {
59  start_orient = *start_orientation;
60 
61  // check if the goal is behind the start pose (w.r.t. start orientation)
62  if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0)
63  backwards = true;
64  }
65  else
66  {
67  Eigen::Vector2d start2goal = goal_position - start_position;
68  start_orient = atan2(start2goal[1],start2goal[0]);
69  }
70 
71  double timestep = 1; // TODO: time
72 
73  if (goal_orientation)
74  {
75  goal_orient = *goal_orientation;
76  }
77  else
78  {
79  goal_orient = start_orient;
80  }
81 
82  if (!isInit())
83  {
84  addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization
85 
86  // we insert middle points now (increase start by 1 and decrease goal by 1)
87  std::advance(path_start,1);
88  std::advance(path_end,-1);
89  int idx=0;
90  for (; path_start != path_end; ++path_start) // insert middle-points
91  {
92  //Eigen::Vector2d point_to_goal = path.back()-*it;
93  //double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal
94  // Alternative: Direction from last path
95  Eigen::Vector2d curr_point = fun_position(*path_start);
96  Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use boost::prior(*path_start) for those cases,
97  // where fun_position() does not return a reference or is expensive.
98  double diff_norm = diff_last.norm();
99 
100  double timestep_vel = diff_norm/max_vel_x; // constant velocity
101  double timestep_acc;
102 
103  if (max_acc_x)
104  {
105  timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
106  if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc;
107  else timestep = timestep_vel;
108  }
109  else timestep = timestep_vel;
110 
111  if (timestep<0) timestep=0.2; // TODO: this is an assumption
112 
113  double yaw = atan2(diff_last[1],diff_last[0]);
114  if (backwards)
115  yaw = g2o::normalize_theta(yaw + M_PI);
116  addPoseAndTimeDiff(curr_point, yaw ,timestep);
117 
118  /*
119  // TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time.
120 
121  Eigen::Vector2d diff_next = fun_position(*boost::next(path_start))-curr_point; // TODO maybe store the boost::next for the following iteration
122  double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0])
123  -atan2(diff_last[1],diff_last[0]) ) );
124 
125  timestep_vel = ang_diff/max_vel_theta; // constant velocity
126  if (max_acc_theta)
127  {
128  timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration
129  if (timestep_vel < timestep_acc) timestep = timestep_acc;
130  else timestep = timestep_vel;
131  }
132  else timestep = timestep_vel;
133 
134  if (timestep<0) timestep=0.2; // TODO: this is an assumption
135 
136  yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished
137  if (backwards)
138  yaw = g2o::normalize_theta(yaw + M_PI);
139  addPoseAndTimeDiff(curr_point, yaw ,timestep);
140 
141  */
142 
143  ++idx;
144  }
145  Eigen::Vector2d diff = goal_position-Pose(idx).position();
146  double diff_norm = diff.norm();
147  double timestep_vel = diff_norm/max_vel_x; // constant velocity
148  if (max_acc_x)
149  {
150  double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration
151  if (timestep_vel < timestep_acc) timestep = timestep_acc;
152  else timestep = timestep_vel;
153  }
154  else timestep = timestep_vel;
155 
156 
157  PoseSE2 goal(goal_position, goal_orient);
158 
159  // if number of samples is not larger than min_samples, insert manually
160  if ( sizePoses() < min_samples-1 )
161  {
162  ROS_DEBUG("initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples...");
163  while (sizePoses() < min_samples-1) // subtract goal point that will be added later
164  {
165  // Each inserted point bisects the remaining distance. Thus the timestep is also bisected.
166  timestep /= 2;
167  // simple strategy: interpolate between the current pose and the goal
168  addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization
169  }
170  }
171 
172  // now add goal
173  addPoseAndTimeDiff(goal, timestep); // add goal point
174  setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization
175  }
176  else // size!=0
177  {
178  ROS_WARN("Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!");
179  ROS_WARN("Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs());
180  return false;
181  }
182  return true;
183 }
184 
185 
186 } // namespace teb_local_planner
187 
188 
189 
Eigen::Vector2d & position()
Access the 2D position part.
Definition: pose_se2.h:145
static PoseSE2 average(const PoseSE2 &pose1, const PoseSE2 &pose2)
Get the mean / average of two poses and return the result (static) For the position part: 0...
Definition: pose_se2.h:266
bool initTrajectoryToGoal(const PoseSE2 &start, const PoseSE2 &goal, double diststep=0, double max_vel_x=0.5, int min_samples=3, bool guess_backwards_motion=false)
Initialize a trajectory between a given start and goal pose.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void addPoseAndTimeDiff(const PoseSE2 &pose, double dt)
Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequenc...
PoseSE2 & BackPose()
Access the last PoseSE2 in the pose sequence.
#define ROS_WARN(...)
void addPose(const PoseSE2 &pose, bool fixed=false)
Append a new pose vertex to the back of the pose sequence.
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
int sizeTimeDiffs() const
Get the length of the internal timediff sequence.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
PoseSE2 & Pose(int index)
Access the pose at pos index of the pose sequence.
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
Definition: pose_se2.h:57
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void setPoseVertexFixed(int index, bool status)
Set a pose vertex at pos index of the pose sequence to be fixed or unfixed during optimization...
int sizePoses() const
Get the length of the internal pose sequence.
bool isInit() const
Check whether the trajectory is initialized (nonzero pose and timediff sequences) ...
#define ROS_DEBUG(...)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08