Add files via upload

cfg/TebLocalPlannerReconfigure.cfg

@@ -119,6 +119,10 @@ grp_robot_carlike.add("min_turning_radius", double_t, 0,
   "Minimum turning radius of a carlike robot (diff-drive robot: zero)", 
   0.0, 0.0, 50.0)   
 
+grp_robot_carlike.add("max_steering_rate", double_t, 0,
+  "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]",
+  1.5, 1.0, 10.0)
+
 grp_robot_carlike.add("wheelbase", double_t, 0,
   "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!", 
   1.0, -10.0, 10.0)  
@@ -279,6 +283,10 @@ grp_optimization.add("weight_kinematics_turning_radius", double_t, 0,
   "Optimization weight for enforcing a minimum turning radius (carlike robots)", 
   1, 0, 1000) 
 
+grp_optimization.add("weight_max_steering_rate", double_t, 0, 
+  "EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)",
+  0, 0, 1000)
+
 grp_optimization.add("weight_optimaltime", double_t, 0, 
         "Optimization weight for contracting the trajectory w.r.t. transition time",
 	1, 0, 1000) 

include/teb_local_planner/g2o_types/edge_velocity.h

@@ -106,7 +106,7 @@ class EdgeVelocity : public BaseTebMultiEdge<2, double>
     double vel = dist / deltaT->estimate();
 
 //     vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction
-    vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
+    vel *= fast_sigmoid( 100.0 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction
 
     const double omega = angle_diff / deltaT->estimate();
 
@@ -198,10 +198,6 @@ class EdgeVelocity : public BaseTebMultiEdge<2, double>
 };
 
 
-
-
-
-
 /**
  * @class EdgeVelocityHolonomic
  * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta.
@@ -260,13 +256,236 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double>
                    "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]);
   }
 
-  
+
 public:
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+ /**
+ * @class EdgeSteeringRate
+ * @brief Edge defining the cost function for limiting the steering rate w.r.t. the current wheelbase parameter
+ *
+ * The edge depends on four vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2} \Delta T_i \f$ .
+ * @remarks This edge requires the TebConfig::Robot::whelbase parameter to be set.
+ * @remarks Do not forget to call setTebConfig()
+ */ 
+class EdgeSteeringRate : public BaseTebMultiEdge<1, double>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */	      
+  EdgeSteeringRate()
+  {
+    this->resize(5); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+
+  /**
+   * @brief Actual cost function
+   */  
+  void computeError()
+  {
+    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeSteeringRate()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexPose* conf3 = static_cast<const VertexPose*>(_vertices[2]);
+    const VertexTimeDiff* dt1 = static_cast<const VertexTimeDiff*>(_vertices[3]);
+    const VertexTimeDiff* dt2 = static_cast<const VertexTimeDiff*>(_vertices[4]);
+
+    Eigen::Vector2d delta_s1 = conf2->estimate().position() - conf1->estimate().position();
+    Eigen::Vector2d delta_s2 = conf3->estimate().position() - conf2->estimate().position();
+    double dist1 = delta_s1.norm();
+    double dist2 = delta_s2.norm();
+    double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() );
+    double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() );
+
+    double phi1, phi2;
+    if (std::abs(dist1) < 1e-12)
+    {
+        phi1 = 0; // TODO previous phi?
+        //ROS_INFO("phi 1 is zero!");
+    }
+    else
+    {
+      //dist1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction
+       //if (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta()) < 0)
+        //dist1 = -dist1;
+
+        if (cfg_->trajectory.exact_arc_length)
+            phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2.0*std::sin(angle_diff1/2.0));
+        else
+            phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1);
+
+        // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan)
+        // In case if we apply the sign to the angle directly, it seems to work:
+        phi1 *= fast_sigmoid( 100.0 * (delta_s1.x()*cos(conf1->theta()) + delta_s1.y()*sin(conf1->theta())) ); // consider direction
+    }
+
+    if (std::abs(dist2) < 1e-12)
+    {
+        phi2 = phi1;
+        ROS_INFO("phi 2 is phi1!");
+    }
+    else
+    {
+        //dist2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction
+        //if (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta()) < 0)
+       //   dist2 = -dist2;
+
+        if (cfg_->trajectory.exact_arc_length)
+            phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2.0*std::sin(angle_diff2/2.0));
+        else
+            phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2);
+
+        // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan).
+        // In case if we apply the sign to the angle directly, it seems to work:
+        phi2 *= fast_sigmoid( 100.0 * (delta_s2.x()*cos(conf2->theta()) + delta_s2.y()*sin(conf2->theta())) ); // consider direction
+    }
+
+    _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2.0 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0);
+
+    ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]);
+  }
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
 };
 
+//! Corresponds to EdgeSteeringRate but with initial steering angle for the predecessor configuration
+class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */	      
+  EdgeSteeringRateStart()
+  {
+    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+
+  /**
+   * @brief Actual cost function
+   */  
+  void computeError()
+  {
+    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeSteeringRateStart()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position();
+    double dist = delta_s.norm();
+    double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
+
+    double phi;
+    if (std::abs(dist) < 1e-12)
+    {
+        ROS_INFO("Start phi equals pervious phi!");
+        phi = _measurement;
+    }
+    else
+    {
+       //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction
+       //dist *= (double)g2o::sign( delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta()) ); // consider direction
+
+        if (cfg_->trajectory.exact_arc_length)
+            phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0));
+        else
+            phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff);
+
+        // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan).
+        // In case if we apply the sign to the angle directly, it seems to work:
+        phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction
+    }
+
+    _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / dt->dt(), cfg_->robot.max_steering_rate, 0.0);
+
+
+    ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateStart::computeError() _error[0]\n",_error[0]);
+  }
+
+  void setInitialSteeringAngle(double steering_angle)
+  {
+      _measurement = steering_angle;
+  }
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+};
+
+//! Corresponds to EdgeSteeringRate but with initial steering angle for the successor configuration
+class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double>
+{
+public:
+
+  /**
+   * @brief Construct edge.
+   */	      
+  EdgeSteeringRateGoal()
+  {
+    this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices
+  }
+
+  /**
+   * @brief Actual cost function
+   */  
+  void computeError()
+  {
+    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeSteeringRateGoal()");
+    const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
+    const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
+    const VertexTimeDiff* dt = static_cast<const VertexTimeDiff*>(_vertices[2]);
+
+    Eigen::Vector2d delta_s = conf2->estimate().position() - conf1->estimate().position();
+    double dist = delta_s.norm();
+    double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() );
+
+    double phi;
+    if (std::abs(dist) < 1e-12)
+    {
+      ROS_INFO("Goal phi is zero!");
+        phi = 0;
+    }
+    else
+    {
+        //dist *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction
+
+        if (cfg_->trajectory.exact_arc_length)
+            phi = std::atan(cfg_->robot.wheelbase / dist * 2.0*std::sin(angle_diff/2.0));
+        else
+            phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff);
+
+        // if we compute the sign of dist1 using the following method, the optimizer get's stuck (possibly due to non-smoothness and atan).
+        // In case if we apply the sign to the angle directly, it seems to work:
+        phi *= fast_sigmoid( 100.0 * (delta_s.x()*cos(conf1->theta()) + delta_s.y()*sin(conf1->theta())) ); // consider direction
+    }
+
+    _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / dt->dt(), cfg_->robot.max_steering_rate, 0.0);
+
+
+    ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRateGoal::computeError() _error[0]\n",_error[0]);
+  }
+
+  void setGoalSteeringAngle(double steering_angle)
+  {
+      _measurement = steering_angle;
+  }
+
+public:
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+}; 
+
+
 
 } // end namespace
 

include/teb_local_planner/misc.h

@@ -94,7 +94,7 @@ inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(
 */
 inline double fast_sigmoid(double x)
 {
-  return x / (1 + fabs(x));
+  return x / (1.0 + fabs(x));
 }
 
 /**

include/teb_local_planner/optimal_planner.h

@@ -659,6 +659,9 @@ class TebOptimalPlanner : public PlannerInterface
    * @see buildGraph
    * @see optimizeGraph
    */
+
+  void AddEdgesSteeringRate();
+
   void AddEdgesPreferRotDir(); 
 
   /**
@@ -694,6 +697,7 @@ class TebOptimalPlanner : public PlannerInterface
   boost::shared_ptr<g2o::SparseOptimizer> optimizer_; //!< g2o optimizer for trajectory optimization
   std::pair<bool, geometry_msgs::Twist> vel_start_; //!< Store the initial velocity at the start pose
   std::pair<bool, geometry_msgs::Twist> vel_goal_; //!< Store the final velocity at the goal pose
+  double recent_steering_angle_ = 0.0; //!< Store last measured steering angle (for car-like setup)
 
   bool initialized_; //!< Keeps track about the correct initialization of this class
   bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful

include/teb_local_planner/teb_config.h

@@ -99,6 +99,7 @@ class TebConfig
     double acc_lim_y; //!< Maximum strafing acceleration of the robot
     double acc_lim_theta; //!< Maximum angular acceleration of the robot
     double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero);
+    double max_steering_rate; //!< EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]
     double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
     bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
     bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
@@ -159,6 +160,7 @@ class TebConfig
     double weight_kinematics_nh; //!< Optimization weight for satisfying the non-holonomic kinematics
     double weight_kinematics_forward_drive; //!< Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)
     double weight_kinematics_turning_radius; //!< Optimization weight for enforcing a minimum turning radius (carlike robots)
+    double weight_max_steering_rate; //!< EXPERIMENTAL: Optimization weight for enforcing a minimum steering rate of a carlike robot (TRY TO KEEP THE WEIGHT LOW OR DEACTIVATE, SINCE IT SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT)
     double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t. transition time
     double weight_shortest_path; //!< Optimization weight for contracting the trajectory w.r.t. path length
     double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles
@@ -273,6 +275,7 @@ class TebConfig
     robot.acc_lim_y = 0.5;
     robot.acc_lim_theta = 0.5;
     robot.min_turning_radius = 0;
+    robot.max_steering_rate = 0;
     robot.wheelbase = 1.0;
     robot.cmd_angle_instead_rotvel = false;
     robot.is_footprint_dynamic = false;
@@ -322,6 +325,7 @@ class TebConfig
     optim.weight_kinematics_nh = 1000;
     optim.weight_kinematics_forward_drive = 1;
     optim.weight_kinematics_turning_radius = 1;
+    optim.weight_max_steering_rate = 1;
     optim.weight_optimaltime = 1;
     optim.weight_shortest_path = 0;
     optim.weight_obstacle = 50;