edge_kinematics.h
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  * Notes:
37  * The following class is derived from a class defined by the
38  * g2o-framework. g2o is licensed under the terms of the BSD License.
39  * Refer to the base class source for detailed licensing information.
40  *
41  * Author: Christoph Rösmann
42  *********************************************************************/
43 
44 #ifndef _EDGE_KINEMATICS_H
45 #define _EDGE_KINEMATICS_H
46 
51 
52 #include <cmath>
53 
54 namespace teb_local_planner
55 {
56 
73 class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
74 {
75 public:
76 
81  {
82  this->setMeasurement(0.);
83  }
84 
88  void computeError()
89  {
90  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
91  const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
92  const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
93 
94  Eigen::Vector2d deltaS = conf2->position() - conf1->position();
95 
96  // non holonomic constraint
97  _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
98 
99  // positive-drive-direction constraint
100  Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) );
101  _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0);
102  // epsilon=0, otherwise it pushes the first bandpoints away from start
103 
104  ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
105  }
106 
107 #ifdef USE_ANALYTIC_JACOBI
108 #if 1
109 
112  void linearizeOplus()
113  {
114  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()");
115  const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
116  const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
117 
118  Eigen::Vector2d deltaS = conf2->position() - conf1->position();
119 
120  double cos1 = cos(conf1->theta());
121  double cos2 = cos(conf2->theta());
122  double sin1 = sin(conf1->theta());
123  double sin2 = sin(conf2->theta());
124  double aux1 = sin1 + sin2;
125  double aux2 = cos1 + cos2;
126 
127  double dd_error_1 = deltaS[0]*cos1;
128  double dd_error_2 = deltaS[1]*sin1;
129  double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0);
130 
131  double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] -
132  ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
133 
134  // conf1
135  _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1
136  _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1
137  _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1
138  _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1
139  _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle
140  _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1
141 
142  // conf2
143  _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2
144  _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2
145  _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2
146  _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2
147  _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle
148  _jacobianOplusXj(1,2) = 0; // drive-dir angle1
149  }
150 #endif
151 #endif
152 
153 public:
154  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 };
156 
157 
158 
159 
182 class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose>
183 {
184 public:
185 
190  {
191  this->setMeasurement(0.);
192  }
193 
198  {
199  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()");
200  const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
201  const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
202 
203  Eigen::Vector2d deltaS = conf2->position() - conf1->position();
204 
205  // non holonomic constraint
206  _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] );
207 
208  // limit minimum turning radius
209  double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
210  if (angle_diff == 0)
211  _error[1] = 0; // straight line motion
212  else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius
213  _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0);
214  else
215  _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0);
216  // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter.
217 
218  ROS_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
219  }
220 
221 public:
222  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223 };
224 
225 
226 
227 
228 } // end namespace
229 
230 #endif
double penaltyBoundFromBelow(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var from below: .
Definition: penalties.h:107
struct teb_local_planner::TebConfig::Robot robot
Robot related parameters.
double min_turning_radius
Minimum turning radius of a carlike robot (diff-drive robot: zero);.
Definition: teb_config.h:97
struct teb_local_planner::TebConfig::Trajectory trajectory
Trajectory related parameters.
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: vertex_pose.h:178
void computeError()
Actual cost function.
#define ROS_ASSERT_MSG(cond,...)
const TebConfig * cfg_
Store TebConfig class for parameters.
double penaltyBoundFromBelowDerivative(const double &var, const double &a, const double &epsilon)
Derivative of the linear penalty function for bounding var from below: .
Definition: penalties.h:177
Base edge connecting two vertices in the TEB optimization problem.
bool exact_arc_length
If true, the planner uses the exact arc length in velocity, acceleration and turning rate computation...
Definition: teb_config.h:81
Eigen::Vector2d & position()
Access the 2D position part.
Definition: vertex_pose.h:141
void computeError()
Actual cost function.
Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot...
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
def sign(number)
Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive m...


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10