edge_velocity.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  * 
00036  * Notes:
00037  * The following class is derived from a class defined by the
00038  * g2o-framework. g2o is licensed under the terms of the BSD License.
00039  * Refer to the base class source for detailed licensing information.
00040  *
00041  * Author: Christoph Rösmann
00042  *********************************************************************/
00043 
00044 #ifndef EDGE_VELOCITY_H
00045 #define EDGE_VELOCITY_H
00046 
00047 #include <teb_local_planner/g2o_types/vertex_pose.h>
00048 #include <teb_local_planner/g2o_types/vertex_timediff.h>
00049 #include <teb_local_planner/g2o_types/penalties.h>
00050 #include <teb_local_planner/teb_config.h>
00051 
00052 #include <g2o/core/base_multi_edge.h>
00053 
00054 #include <iostream>
00055 
00056 namespace teb_local_planner
00057 {
00058 
00059   
00075 class EdgeVelocity : public g2o::BaseMultiEdge<2, double>
00076 {
00077 public:
00078   
00082   EdgeVelocity()
00083   {
00084     this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
00085     for(unsigned int i=0;i<3;i++) _vertices[i] = NULL;
00086   }
00087   
00094   virtual ~EdgeVelocity()
00095   {
00096     for(unsigned int i=0;i<3;i++)
00097     {
00098       if(_vertices[i])
00099         _vertices[i]->edges().erase(this);
00100     }
00101   }
00102 
00106   void computeError()
00107   {
00108     ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
00109     const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
00110     const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
00111     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
00112     Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position();
00113     double vel = deltaS.norm() / deltaT->estimate();
00114 //     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
00115     vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
00116     
00117     double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate();
00118   
00119     _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
00120     _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
00121 
00122     ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]);
00123   }
00124 
00125 #ifdef USE_ANALYTIC_JACOBI
00126 #if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value)
00127       // Change accordingly...
00128 
00132   void linearizeOplus()
00133   {
00134     ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()");
00135     const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
00136     const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
00137     const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]);
00138     
00139     Eigen::Vector2d deltaS = conf2->position() - conf1->position();
00140     double dist = deltaS.norm();
00141     double aux1 = dist*deltaT->estimate();
00142     double aux2 = 1/deltaT->estimate();
00143     
00144     double vel = dist * aux2;
00145     double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2;
00146     
00147     double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon);
00148     double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon);
00149     
00150     _jacobianOplus[0].resize(2,3); // conf1
00151     _jacobianOplus[1].resize(2,3); // conf2
00152     _jacobianOplus[2].resize(2,1); // deltaT
00153     
00154 //  if (aux1==0) aux1=1e-6;
00155 //  if (aux2==0) aux2=1e-6;
00156     
00157     if (dev_border_vel!=0)
00158     {
00159       double aux3 = dev_border_vel / aux1;
00160       _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1
00161       _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1     
00162       _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2
00163       _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2
00164       _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT
00165     }
00166     else
00167     {
00168       _jacobianOplus[0](0,0) = 0; // vel x1
00169       _jacobianOplus[0](0,1) = 0; // vel y1     
00170       _jacobianOplus[1](0,0) = 0; // vel x2
00171       _jacobianOplus[1](0,1) = 0; // vel y2     
00172       _jacobianOplus[2](0,0) = 0; // vel deltaT
00173     }
00174     
00175     if (dev_border_omega!=0)
00176     {
00177       double aux4 = aux2 * dev_border_omega;
00178       _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT
00179       _jacobianOplus[0](1,2) = -aux4; // omega angle1
00180       _jacobianOplus[1](1,2) = aux4; // omega angle2
00181     }
00182     else
00183     {
00184       _jacobianOplus[2](1,0) = 0; // omega deltaT
00185       _jacobianOplus[0](1,2) = 0; // omega angle1
00186       _jacobianOplus[1](1,2) = 0; // omega angle2                       
00187     }
00188 
00189     _jacobianOplus[0](1,0) = 0; // omega x1
00190     _jacobianOplus[0](1,1) = 0; // omega y1
00191     _jacobianOplus[1](1,0) = 0; // omega x2
00192     _jacobianOplus[1](1,1) = 0; // omega y2
00193     _jacobianOplus[0](0,2) = 0; // vel angle1
00194     _jacobianOplus[1](0,2) = 0; // vel angle2
00195   }
00196 #endif
00197 #endif
00198  
00205   ErrorVector& getError()
00206   {
00207     computeError();
00208     return _error;
00209   }
00210 
00214   virtual bool read(std::istream& is)
00215   {
00216     is >> _measurement;
00217     is >> information()(0,0);
00218     return true;
00219   }
00220 
00224   virtual bool write(std::ostream& os) const
00225   {
00226     //os << measurement() << " ";
00227     os << information()(0,0) << " Error Vel: " << _error[0] << ", Error Omega: " << _error[1];
00228     return os.good();
00229   }
00230 
00235   void setTebConfig(const TebConfig& cfg)
00236   {
00237     cfg_ = &cfg;
00238   }
00239 
00240 protected:
00241   
00242   const TebConfig* cfg_; 
00243   
00244   
00245 public:
00246   
00247   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00248 
00249 };
00250 
00251 } // end namespace
00252 
00253 #endif


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15