edge_acceleration.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_ACCELERATION_H_
45 #define EDGE_ACCELERATION_H_
46 
52 
53 #include <geometry_msgs/Twist.h>
54 
55 
56 
57 namespace teb_local_planner
58 {
59 
78 class EdgeAcceleration : public BaseTebMultiEdge<2, double>
79 {
80 public:
81 
86  {
87  this->resize(5);
88  }
89 
93  void computeError()
94  {
95  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
96  const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
97  const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
98  const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
99  const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
100  const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
101 
102  // VELOCITY & ACCELERATION
103  const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
104  const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
105 
106  double dist1 = diff1.norm();
107  double dist2 = diff2.norm();
108  const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta());
109  const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta());
110 
111  if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation
112  {
113  if (angle_diff1 != 0)
114  {
115  const double radius = dist1/(2*sin(angle_diff1/2));
116  dist1 = fabs( angle_diff1 * radius ); // actual arg length!
117  }
118  if (angle_diff2 != 0)
119  {
120  const double radius = dist2/(2*sin(angle_diff2/2));
121  dist2 = fabs( angle_diff2 * radius ); // actual arg length!
122  }
123  }
124 
125  double vel1 = dist1 / dt1->dt();
126  double vel2 = dist2 / dt2->dt();
127 
128 
129  // consider directions
130 // vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta()));
131 // vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta()));
132  vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) );
133  vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) );
134 
135  const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() );
136 
137 
139 
140  // ANGULAR ACCELERATION
141  const double omega1 = angle_diff1 / dt1->dt();
142  const double omega2 = angle_diff2 / dt2->dt();
143  const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() );
144 
146 
147 
148  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
149  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]);
150  }
151 
152 
153 
154 #ifdef USE_ANALYTIC_JACOBI
155 #if 0
156  /*
157  * @brief Jacobi matrix of the cost function specified in computeError().
158  */
159  void linearizeOplus()
160  {
161  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
162  const VertexPointXY* conf1 = static_cast<const VertexPointXY*>(_vertices[0]);
163  const VertexPointXY* conf2 = static_cast<const VertexPointXY*>(_vertices[1]);
164  const VertexPointXY* conf3 = static_cast<const VertexPointXY*>(_vertices[2]);
165  const VertexTimeDiff* deltaT1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
166  const VertexTimeDiff* deltaT2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
167  const VertexOrientation* angle1 = static_cast<const VertexOrientation*>(_vertices[5]);
168  const VertexOrientation* angle2 = static_cast<const VertexOrientation*>(_vertices[6]);
169  const VertexOrientation* angle3 = static_cast<const VertexOrientation*>(_vertices[7]);
170 
171  Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate();
172  Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate();
173  double dist1 = deltaS1.norm();
174  double dist2 = deltaS2.norm();
175 
176  double sum_time = deltaT1->estimate() + deltaT2->estimate();
177  double sum_time_inv = 1 / sum_time;
178  double dt1_inv = 1/deltaT1->estimate();
179  double dt2_inv = 1/deltaT2->estimate();
180  double aux0 = 2/sum_time_inv;
181  double aux1 = dist1 * deltaT1->estimate();
182  double aux2 = dist2 * deltaT2->estimate();
183 
184  double vel1 = dist1 * dt1_inv;
185  double vel2 = dist2 * dt2_inv;
186  double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv;
187  double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv;
188  double acc = (vel2 - vel1) * aux0;
189  double omegadot = (omega2 - omega1) * aux0;
190  double aux3 = -acc/2;
191  double aux4 = -omegadot/2;
192 
193  double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
194  double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order);
195 
196  _jacobianOplus[0].resize(2,2); // conf1
197  _jacobianOplus[1].resize(2,2); // conf2
198  _jacobianOplus[2].resize(2,2); // conf3
199  _jacobianOplus[3].resize(2,1); // deltaT1
200  _jacobianOplus[4].resize(2,1); // deltaT2
201  _jacobianOplus[5].resize(2,1); // angle1
202  _jacobianOplus[6].resize(2,1); // angle2
203  _jacobianOplus[7].resize(2,1); // angle3
204 
205  if (aux1==0) aux1=1e-20;
206  if (aux2==0) aux2=1e-20;
207 
208  if (dev_border_acc!=0)
209  {
210  // TODO: double aux = aux0 * dev_border_acc;
211  // double aux123 = aux / aux1;
212  _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1
213  _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1
214  _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2
215  _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2
216  _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3
217  _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3
218  _jacobianOplus[2](0,0) = 0;
219  _jacobianOplus[2](0,1) = 0;
220  _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1
221  _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2
222  }
223  else
224  {
225  _jacobianOplus[0](0,0) = 0; // acc x1
226  _jacobianOplus[0](0,1) = 0; // acc y1
227  _jacobianOplus[1](0,0) = 0; // acc x2
228  _jacobianOplus[1](0,1) = 0; // acc y2
229  _jacobianOplus[2](0,0) = 0; // acc x3
230  _jacobianOplus[2](0,1) = 0; // acc y3
231  _jacobianOplus[3](0,0) = 0; // acc deltaT1
232  _jacobianOplus[4](0,0) = 0; // acc deltaT2
233  }
234 
235  if (dev_border_omegadot!=0)
236  {
237  _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1
238  _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2
239  _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1
240  _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2
241  _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3
242  }
243  else
244  {
245  _jacobianOplus[3](1,0) = 0; // omegadot deltaT1
246  _jacobianOplus[4](1,0) = 0; // omegadot deltaT2
247  _jacobianOplus[5](1,0) = 0; // omegadot angle1
248  _jacobianOplus[6](1,0) = 0; // omegadot angle2
249  _jacobianOplus[7](1,0) = 0; // omegadot angle3
250  }
251 
252  _jacobianOplus[0](1,0) = 0; // omegadot x1
253  _jacobianOplus[0](1,1) = 0; // omegadot y1
254  _jacobianOplus[1](1,0) = 0; // omegadot x2
255  _jacobianOplus[1](1,1) = 0; // omegadot y2
256  _jacobianOplus[2](1,0) = 0; // omegadot x3
257  _jacobianOplus[2](1,1) = 0; // omegadot y3
258  _jacobianOplus[5](0,0) = 0; // acc angle1
259  _jacobianOplus[6](0,0) = 0; // acc angle2
260  _jacobianOplus[7](0,0) = 0; // acc angle3
261  }
262 #endif
263 #endif
264 
265 
266 public:
267  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
268 
269 };
270 
271 
291 class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::Twist*>
292 {
293 public:
294 
299  {
300  _measurement = NULL;
301  this->resize(3);
302  }
303 
304 
309  {
310  ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
311  const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
312  const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
313  const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
314 
315  // VELOCITY & ACCELERATION
316  const Eigen::Vector2d diff = pose2->position() - pose1->position();
317  double dist = diff.norm();
318  const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta());
319  if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
320  {
321  const double radius = dist/(2*sin(angle_diff/2));
322  dist = fabs( angle_diff * radius ); // actual arg length!
323  }
324 
325  const double vel1 = _measurement->linear.x;
326  double vel2 = dist / dt->dt();
327 
328  // consider directions
329  //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta()));
330  vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) );
331 
332  const double acc_lin = (vel2 - vel1) / dt->dt();
333 
335 
336  // ANGULAR ACCELERATION
337  const double omega1 = _measurement->angular.z;
338  const double omega2 = angle_diff / dt->dt();
339  const double acc_rot = (omega2 - omega1) / dt->dt();
340 
342 
343  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
344  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]);
345  }
346 
352  {
353  _measurement = &vel_start;
354  }
355 
356 public:
357  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
358 };
359 
360 
361 
362 
382 class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::Twist*>
383 {
384 public:
385 
390  {
391  _measurement = NULL;
392  this->resize(3);
393  }
394 
395 
400  {
401  ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
402  const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
403  const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
404  const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
405 
406  // VELOCITY & ACCELERATION
407 
408  const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
409  double dist = diff.norm();
410  const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta());
411  if (cfg_->trajectory.exact_arc_length && angle_diff != 0)
412  {
413  double radius = dist/(2*sin(angle_diff/2));
414  dist = fabs( angle_diff * radius ); // actual arg length!
415  }
416 
417  double vel1 = dist / dt->dt();
418  const double vel2 = _measurement->linear.x;
419 
420  // consider directions
421  //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta()));
422  vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) );
423 
424  const double acc_lin = (vel2 - vel1) / dt->dt();
425 
427 
428  // ANGULAR ACCELERATION
429  const double omega1 = angle_diff / dt->dt();
430  const double omega2 = _measurement->angular.z;
431  const double acc_rot = (omega2 - omega1) / dt->dt();
432 
434 
435  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
436  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]);
437  }
438 
443  void setGoalVelocity(const geometry_msgs::Twist& vel_goal)
444  {
445  _measurement = &vel_goal;
446  }
447 
448 public:
449  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
450 };
451 
452 
453 
454 
475 {
476 public:
477 
482  {
483  this->resize(5);
484  }
485 
490  {
491  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()");
492  const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
493  const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
494  const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
495  const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
496  const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
497 
498  // VELOCITY & ACCELERATION
499  Eigen::Vector2d diff1 = pose2->position() - pose1->position();
500  Eigen::Vector2d diff2 = pose3->position() - pose2->position();
501 
502  double cos_theta1 = std::cos(pose1->theta());
503  double sin_theta1 = std::sin(pose1->theta());
504  double cos_theta2 = std::cos(pose2->theta());
505  double sin_theta2 = std::sin(pose2->theta());
506 
507  // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
508  double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y();
509  double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y();
510  // transform pose3 into robot frame pose2 (inverse 2d rotation matrix)
511  double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y();
512  double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y();
513 
514  double vel1_x = p1_dx / dt1->dt();
515  double vel1_y = p1_dy / dt1->dt();
516  double vel2_x = p2_dx / dt2->dt();
517  double vel2_y = p2_dy / dt2->dt();
518 
519  double dt12 = dt1->dt() + dt2->dt();
520 
521  double acc_x = (vel2_x - vel1_x)*2 / dt12;
522  double acc_y = (vel2_y - vel1_y)*2 / dt12;
523 
526 
527  // ANGULAR ACCELERATION
528  double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt();
529  double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt();
530  double acc_rot = (omega2 - omega1)*2 / dt12;
531 
533 
534 
535  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]);
536  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]);
537  ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]);
538  }
539 
540 public:
541  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
542 
543 };
544 
545 
566 class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::Twist*>
567 {
568 public:
569 
574  {
575  this->resize(3);
576  _measurement = NULL;
577  }
578 
583  {
584  ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()");
585  const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
586  const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
587  const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
588 
589  // VELOCITY & ACCELERATION
590  Eigen::Vector2d diff = pose2->position() - pose1->position();
591 
592  double cos_theta1 = std::cos(pose1->theta());
593  double sin_theta1 = std::sin(pose1->theta());
594 
595  // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
596  double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
597  double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
598 
599  double vel1_x = _measurement->linear.x;
600  double vel1_y = _measurement->linear.y;
601  double vel2_x = p1_dx / dt->dt();
602  double vel2_y = p1_dy / dt->dt();
603 
604  double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
605  double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
606 
609 
610  // ANGULAR ACCELERATION
611  double omega1 = _measurement->angular.z;
612  double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt();
613  double acc_rot = (omega2 - omega1) / dt->dt();
614 
616 
617  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]);
618  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]);
619  ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]);
620  }
621 
627  {
628  _measurement = &vel_start;
629  }
630 
631 public:
632  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
633 };
634 
635 
636 
657 class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::Twist*>
658 {
659 public:
660 
665  {
666  _measurement = NULL;
667  this->resize(3);
668  }
669 
674  {
675  ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()");
676  const VertexPose* pose_pre_goal = static_cast<const VertexPose*>(_vertices[0]);
677  const VertexPose* pose_goal = static_cast<const VertexPose*>(_vertices[1]);
678  const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
679 
680  // VELOCITY & ACCELERATION
681 
682  Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position();
683 
684  double cos_theta1 = std::cos(pose_pre_goal->theta());
685  double sin_theta1 = std::sin(pose_pre_goal->theta());
686 
687  // transform pose2 into robot frame pose1 (inverse 2d rotation matrix)
688  double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y();
689  double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y();
690 
691  double vel1_x = p1_dx / dt->dt();
692  double vel1_y = p1_dy / dt->dt();
693  double vel2_x = _measurement->linear.x;
694  double vel2_y = _measurement->linear.y;
695 
696  double acc_lin_x = (vel2_x - vel1_x) / dt->dt();
697  double acc_lin_y = (vel2_y - vel1_y) / dt->dt();
698 
701 
702  // ANGULAR ACCELERATION
703  double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt();
704  double omega2 = _measurement->angular.z;
705  double acc_rot = (omega2 - omega1) / dt->dt();
706 
708 
709  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]);
710  ROS_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]);
711  ROS_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]);
712  }
713 
714 
719  void setGoalVelocity(const geometry_msgs::Twist& vel_goal)
720  {
721  _measurement = &vel_goal;
722  }
723 
724 
725 public:
726  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
727 };
728 
729 
730 
731 
732 }; // end namespace
733 
734 #endif /* EDGE_ACCELERATION_H_ */
Edge defining the cost function for limiting the translational and rotational acceleration at the end...
double penaltyBoundToInterval(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var to the interval .
Definition: penalties.h:57
double acc_lim_x
Maximum translational acceleration of the robot.
Definition: teb_config.h:94
Edge defining the cost function for limiting the translational and rotational acceleration at the end...
void setInitialVelocity(const geometry_msgs::Twist &vel_start)
Set the initial velocity that is taken into account for calculating the acceleration.
Edge defining the cost function for limiting the translational and rotational acceleration.
Edge defining the cost function for limiting the translational and rotational acceleration at the beg...
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
struct teb_local_planner::TebConfig::Robot robot
Robot related parameters.
struct teb_local_planner::TebConfig::Trajectory trajectory
Trajectory related parameters.
double fast_sigmoid(double x)
Calculate a fast approximation of a sigmoid function.
Definition: misc.h:95
void setInitialVelocity(const geometry_msgs::Twist &vel_start)
Set the initial velocity that is taken into account for calculating the acceleration.
Edge defining the cost function for limiting the translational and rotational acceleration.
void computeError()
Actual cost function.
void setGoalVelocity(const geometry_msgs::Twist &vel_goal)
Set the goal / final velocity that is taken into account for calculating the acceleration.
double acc_lim_theta
Maximum angular acceleration of the robot.
Definition: teb_config.h:96
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: vertex_pose.h:178
Base edge connecting two vertices in the TEB optimization problem.
#define ROS_ASSERT_MSG(cond,...)
double penaltyBoundToIntervalDerivative(const double &var, const double &a, const double &epsilon)
Derivative of the linear penalty function for bounding var to the interval .
Definition: penalties.h:127
Edge defining the cost function for limiting the translational and rotational acceleration at the beg...
double penalty_epsilon
Add a small safety margin to penalty functions for hard-constraint approximations.
Definition: teb_config.h:140
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
const TebConfig * cfg_
Store TebConfig class for parameters.
double & dt()
Access the timediff value of the vertex.
void setGoalVelocity(const geometry_msgs::Twist &vel_goal)
Set the goal / final velocity that is taken into account for calculating the acceleration.
void computeError()
Actual cost function.
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
void computeError()
Actual cost function.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
struct teb_local_planner::TebConfig::Optimization optim
Optimization related parameters.
double acc_lim_y
Maximum strafing acceleration of the robot.
Definition: teb_config.h:95
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
This class stores and wraps a time difference into a vertex that can be optimized via g2o...


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