From 0c7f768e116b3152aec1d652f341c46e9eae4f75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Tue, 20 Sep 2016 15:20:25 +0200 Subject: [PATCH 1/5] Added edges for limiting the maximum steering rate w.r.t. the wheelbase --- cfg/TebLocalPlannerReconfigure.cfg | 9 ++ .../g2o_types/edge_velocity.h | 136 +++++++++++++++++- include/teb_local_planner/optimal_planner.h | 8 ++ include/teb_local_planner/teb_config.h | 4 + scripts/visualize_vel_and_steering.py | 89 ++++++++++++ src/optimal_planner.cpp | 25 ++++ src/teb_config.cpp | 4 + 7 files changed, 271 insertions(+), 4 deletions(-) create mode 100755 scripts/visualize_vel_and_steering.py diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index cee85daf..17bd185d 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -97,6 +97,11 @@ gen.add("min_turning_radius", double_t, 0, "Minimum turning radius of a carlike robot (diff-drive robot: zero)", 0.0, 0.0, 50.0) +gen.add("max_steering_rate", double_t, 0, + "Maximum bound of the steering wheel (requires the definition of the wheelbase) [deactivate: zero]", + 0.0, 0.0, 50.0) + + gen.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) @@ -209,6 +214,10 @@ gen.add("weight_kinematics_forward_drive", double_t, 0, gen.add("weight_kinematics_turning_radius", double_t, 0, "Optimization weight for enforcing a minimum turning radius (carlike robots)", 1, 0, 1000) + +gen.add("weight_max_steering_rate", double_t, 0, + "Optimization weight for enforcing a maximum steering rate (carlike robots)", + 1, 0, 1000) gen.add("weight_optimaltime", double_t, 0, "Optimization weight for contracting the trajectory w.r.t transition time", diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h index 47efcece..cd0abde1 100755 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -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. @@ -268,6 +264,138 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double> }; + + +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 EdgeVelocity()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexPose* conf3 = static_cast(_vertices[2]); + const VertexTimeDiff* dt1 = static_cast(_vertices[3]); + const VertexTimeDiff* dt2 = static_cast(_vertices[4]); + + double dist1 = (conf2->estimate().position() - conf1->estimate().position()).norm(); + double dist2 = (conf3->estimate().position() - conf2->estimate().position()).norm(); + double omega_t1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + double omega_t2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + + double phi1, phi2; + if (dist1 == 0) + phi1 = 0; // TODO previous phi? + else + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * omega_t1); + if (dist2 == 0) + phi2 = 0; + else + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * omega_t2); + + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); + + + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); + } + +#ifdef USE_ANALYTIC_JACOBI +#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value) + // Change accordingly... + + /** + * @brief Jacobi matrix of the cost function specified in computeError(). + */ + void linearizeOplus() + { + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); + const VertexPose* conf1 = static_cast(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* deltaT = static_cast(_vertices[2]); + + Eigen::Vector2d deltaS = conf2->position() - conf1->position(); + double dist = deltaS.norm(); + double aux1 = dist*deltaT->estimate(); + double aux2 = 1/deltaT->estimate(); + + double vel = dist * aux2; + double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; + + double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); + double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); + + _jacobianOplus[0].resize(2,3); // conf1 + _jacobianOplus[1].resize(2,3); // conf2 + _jacobianOplus[2].resize(2,1); // deltaT + +// if (aux1==0) aux1=1e-6; +// if (aux2==0) aux2=1e-6; + + if (dev_border_vel!=0) + { + double aux3 = dev_border_vel / aux1; + _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 + _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 + _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 + _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 + _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT + } + else + { + _jacobianOplus[0](0,0) = 0; // vel x1 + _jacobianOplus[0](0,1) = 0; // vel y1 + _jacobianOplus[1](0,0) = 0; // vel x2 + _jacobianOplus[1](0,1) = 0; // vel y2 + _jacobianOplus[2](0,0) = 0; // vel deltaT + } + + if (dev_border_omega!=0) + { + double aux4 = aux2 * dev_border_omega; + _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT + _jacobianOplus[0](1,2) = -aux4; // omega angle1 + _jacobianOplus[1](1,2) = aux4; // omega angle2 + } + else + { + _jacobianOplus[2](1,0) = 0; // omega deltaT + _jacobianOplus[0](1,2) = 0; // omega angle1 + _jacobianOplus[1](1,2) = 0; // omega angle2 + } + + _jacobianOplus[0](1,0) = 0; // omega x1 + _jacobianOplus[0](1,1) = 0; // omega y1 + _jacobianOplus[1](1,0) = 0; // omega x2 + _jacobianOplus[1](1,1) = 0; // omega y2 + _jacobianOplus[0](0,2) = 0; // vel angle1 + _jacobianOplus[1](0,2) = 0; // vel angle2 + } +#endif +#endif + + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + +}; + + + + } // end namespace #endif diff --git a/include/teb_local_planner/optimal_planner.h b/include/teb_local_planner/optimal_planner.h index 84b65cf9..835fa661 100644 --- a/include/teb_local_planner/optimal_planner.h +++ b/include/teb_local_planner/optimal_planner.h @@ -641,6 +641,14 @@ class TebOptimalPlanner : public PlannerInterface */ void AddEdgesKinematicsCarlike(); + /** + * @brief Add all edges (local cost functions) for satisfying a maximum steering rate of a carlike robot + * @warning requires a valid wheelbase parameter value! + * @see buildGraph + * @see optimizeGraph + */ + void AddEdgesSteeringRate(); + //@} diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 88183dd3..67749cfa 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -94,6 +94,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; //!< Maximum bound of the steering wheel (requires the definition of the wheelbase) [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') } robot; //!< Robot related parameters @@ -143,6 +144,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; //!< Optimization weight for enforcing a maximum steering rate (carlike robots) double weight_optimaltime; //!< Optimization weight for contracting the trajectory w.r.t transition time double weight_obstacle; //!< Optimization weight for satisfying a minimum separation from obstacles double weight_inflation; //!< Optimization weight for the inflation penalty (should be small) @@ -222,6 +224,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; @@ -261,6 +264,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_obstacle = 10; optim.weight_inflation = 0.1; diff --git a/scripts/visualize_vel_and_steering.py b/scripts/visualize_vel_and_steering.py new file mode 100755 index 00000000..f5f3ff02 --- /dev/null +++ b/scripts/visualize_vel_and_steering.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python + +# This small script subscribes to the FeedbackMsg message of teb_local_planner +# and plots the current velocity. +# publish_feedback must be turned on such that the planner publishes this information. +# Author: christoph.roesmann@tu-dortmund.de + +import rospy, math +from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg +from geometry_msgs.msg import PolygonStamped, Point32 +import numpy as np +import matplotlib.pyplot as plotter + +def feedback_callback(data): + global trajectory + + if not data.trajectories: # empty + trajectory = [] + return + trajectory = data.trajectories[data.selected_trajectory_idx].trajectory + + +def plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, t, v, omega, steering): + ax_v.cla() + ax_v.grid() + ax_v.set_ylabel('Trans. velocity [m/s]') + ax_v.plot(t, v, '-bx') + ax_omega.cla() + ax_omega.grid() + ax_omega.set_ylabel('Rot. velocity [rad/s]') + ax_omega.set_xlabel('Time [s]') + ax_omega.plot(t, omega, '-bx') + ax_steering.cla() + ax_steering.grid() + ax_steering.set_ylabel('Steering angle [rad]') + ax_steering.set_xlabel('Time [s]') + ax_steering.plot(t, steering, '-bx') + fig.canvas.draw() + + + +def velocity_plotter(): + global trajectory + rospy.init_node("visualize_vel_and_steering", anonymous=True) + + topic_name = "/test_optim_node/teb_feedback" + topic_name = rospy.get_param('~feedback_topic', topic_name) + rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! + + wheelbase = 1.0 + wheelbase = rospy.get_param('~wheelbase', wheelbase) + + rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) + rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") + + # two subplots sharing the same t axis + fig, (ax_v, ax_omega, ax_steering) = plotter.subplots(3, sharex=True) + plotter.ion() + plotter.show() + + + r = rospy.Rate(2) # define rate here + while not rospy.is_shutdown(): + + t = [] + v = [] + omega = [] + steering = [] + + for point in trajectory: + t.append(point.time_from_start.to_sec()) + v.append(point.velocity.linear.x) + omega.append(point.velocity.angular.z) + if point.velocity.linear.x == 0: + steering.append( 0.0 ) + else: + steering.append( math.atan( wheelbase / point.velocity.linear.x * point.velocity.angular.z ) ) + + plot_velocity_profile(fig, ax_v, ax_omega, ax_steering, np.asarray(t), np.asarray(v), np.asarray(omega), np.asarray(steering)) + + r.sleep() + +if __name__ == '__main__': + try: + trajectory = [] + velocity_plotter() + except rospy.ROSInterruptException: + pass + diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index 9e76c7e3..4f97d20f 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -129,6 +129,7 @@ void TebOptimalPlanner::registerG2OTypes() factory->registerType("EDGE_ACCELERATION_HOLONOMIC", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator); @@ -308,6 +309,8 @@ bool TebOptimalPlanner::buildGraph() AddEdgesVelocity(); AddEdgesAcceleration(); + + AddEdgesSteeringRate(); AddEdgesTimeOptimal(); @@ -887,6 +890,28 @@ void TebOptimalPlanner::AddEdgesKinematicsCarlike() } } +void TebOptimalPlanner::AddEdgesSteeringRate() +{ + if (cfg_->robot.max_steering_rate==0 || cfg_->optim.weight_max_steering_rate==0) + return; // if weight equals zero skip adding edges! + + // create edge for satisfiying kinematic constraints + Eigen::Matrix information_steering_rate; + information_steering_rate(0, 0) = cfg_->optim.weight_max_steering_rate; + + for (int i=0; i < teb_.sizePoses()-2; i++) // ignore twiced start only + { + EdgeSteeringRate* steering_rate_edge = new EdgeSteeringRate; + steering_rate_edge->setVertex(0,teb_.PoseVertex(i)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(i+1)); + steering_rate_edge->setVertex(2,teb_.PoseVertex(i+2)); + steering_rate_edge->setVertex(3,teb_.TimeDiffVertex(i)); + steering_rate_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } +} void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) { diff --git a/src/teb_config.cpp b/src/teb_config.cpp index 6c5d0a3a..db301010 100644 --- a/src/teb_config.cpp +++ b/src/teb_config.cpp @@ -71,6 +71,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) nh.param("acc_lim_y", robot.acc_lim_y, robot.acc_lim_y); nh.param("acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta); nh.param("min_turning_radius", robot.min_turning_radius, robot.min_turning_radius); + nh.param("max_steering_rate", robot.max_steering_rate, robot.max_steering_rate); nh.param("wheelbase", robot.wheelbase, robot.wheelbase); nh.param("cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel); @@ -106,6 +107,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh) nh.param("weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh); nh.param("weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive); nh.param("weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius); + nh.param("weight_max_steering_rate", optim.weight_max_steering_rate, optim.weight_max_steering_rate); nh.param("weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); nh.param("weight_obstacle", optim.weight_obstacle, optim.weight_obstacle); nh.param("weight_inflation", optim.weight_inflation, optim.weight_inflation); @@ -162,6 +164,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) robot.acc_lim_y = cfg.acc_lim_y; robot.acc_lim_theta = cfg.acc_lim_theta; robot.min_turning_radius = cfg.min_turning_radius; + robot.max_steering_rate = cfg.max_steering_rate; robot.wheelbase = cfg.wheelbase; robot.cmd_angle_instead_rotvel = cfg.cmd_angle_instead_rotvel; @@ -196,6 +199,7 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg) optim.weight_kinematics_nh = cfg.weight_kinematics_nh; optim.weight_kinematics_forward_drive = cfg.weight_kinematics_forward_drive; optim.weight_kinematics_turning_radius = cfg.weight_kinematics_turning_radius; + optim.weight_max_steering_rate = cfg.weight_max_steering_rate; optim.weight_optimaltime = cfg.weight_optimaltime; optim.weight_obstacle = cfg.weight_obstacle; optim.weight_inflation = cfg.weight_inflation; From 51b48fc30e14cb2be6da0fb226999842689d5c90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Thu, 22 Sep 2016 14:52:58 +0200 Subject: [PATCH 2/5] added exact_arc_length option to the steering-rate-edge; minor fixes --- .../g2o_types/edge_velocity.h | 101 ++++-------------- 1 file changed, 18 insertions(+), 83 deletions(-) diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h index cd0abde1..94086eb1 100755 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -290,102 +290,37 @@ class EdgeSteeringRate : public BaseTebMultiEdge<1, double> const VertexTimeDiff* dt1 = static_cast(_vertices[3]); const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - double dist1 = (conf2->estimate().position() - conf1->estimate().position()).norm(); - double dist2 = (conf3->estimate().position() - conf2->estimate().position()).norm(); - double omega_t1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - double omega_t2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); - + const double dist1 = (conf2->estimate().position() - conf1->estimate().position()).norm(); + const double dist2 = (conf3->estimate().position() - conf2->estimate().position()).norm(); + const double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + const double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + double phi1, phi2; if (dist1 == 0) phi1 = 0; // TODO previous phi? else - phi1 = std::atan(cfg_->robot.wheelbase / dist1 * omega_t1); + { + if (cfg_->trajectory.exact_arc_length) + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * 2*sin(angle_diff1/2)); + else + phi1 = std::atan(cfg_->robot.wheelbase / dist1 * angle_diff1); + } + if (dist2 == 0) phi2 = 0; else - phi2 = std::atan(cfg_->robot.wheelbase / dist2 * omega_t2); - + { + if (cfg_->trajectory.exact_arc_length) + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * 2*sin(angle_diff2/2)); + else + phi2 = std::atan(cfg_->robot.wheelbase / dist2 * angle_diff2); + } _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); } - -#ifdef USE_ANALYTIC_JACOBI -#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value) - // Change accordingly... - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - double dist = deltaS.norm(); - double aux1 = dist*deltaT->estimate(); - double aux2 = 1/deltaT->estimate(); - - double vel = dist * aux2; - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; - - double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - _jacobianOplus[0].resize(2,3); // conf1 - _jacobianOplus[1].resize(2,3); // conf2 - _jacobianOplus[2].resize(2,1); // deltaT - -// if (aux1==0) aux1=1e-6; -// if (aux2==0) aux2=1e-6; - - if (dev_border_vel!=0) - { - double aux3 = dev_border_vel / aux1; - _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 - _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 - _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 - _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 - _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT - } - else - { - _jacobianOplus[0](0,0) = 0; // vel x1 - _jacobianOplus[0](0,1) = 0; // vel y1 - _jacobianOplus[1](0,0) = 0; // vel x2 - _jacobianOplus[1](0,1) = 0; // vel y2 - _jacobianOplus[2](0,0) = 0; // vel deltaT - } - - if (dev_border_omega!=0) - { - double aux4 = aux2 * dev_border_omega; - _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT - _jacobianOplus[0](1,2) = -aux4; // omega angle1 - _jacobianOplus[1](1,2) = aux4; // omega angle2 - } - else - { - _jacobianOplus[2](1,0) = 0; // omega deltaT - _jacobianOplus[0](1,2) = 0; // omega angle1 - _jacobianOplus[1](1,2) = 0; // omega angle2 - } - - _jacobianOplus[0](1,0) = 0; // omega x1 - _jacobianOplus[0](1,1) = 0; // omega y1 - _jacobianOplus[1](1,0) = 0; // omega x2 - _jacobianOplus[1](1,1) = 0; // omega y2 - _jacobianOplus[0](0,2) = 0; // vel angle1 - _jacobianOplus[1](0,2) = 0; // vel angle2 - } -#endif -#endif - public: From 4954275ef2af0a84820d7df6b30af80283e7fd8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Thu, 22 Sep 2016 15:15:19 +0200 Subject: [PATCH 3/5] steering rate bounds: added boundary values for start and goal --- .../g2o_types/edge_velocity.h | 120 +++++++++++++++++- src/optimal_planner.cpp | 38 +++++- 2 files changed, 154 insertions(+), 4 deletions(-) diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h index 94086eb1..e9d5a5fb 100755 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/include/teb_local_planner/g2o_types/edge_velocity.h @@ -265,7 +265,14 @@ class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double> - +/** + * @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: @@ -283,7 +290,7 @@ class EdgeSteeringRate : public BaseTebMultiEdge<1, double> */ void computeError() { - ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); + ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeSteeringRate()"); const VertexPose* conf1 = static_cast(_vertices[0]); const VertexPose* conf2 = static_cast(_vertices[1]); const VertexPose* conf3 = static_cast(_vertices[2]); @@ -319,7 +326,7 @@ class EdgeSteeringRate : public BaseTebMultiEdge<1, double> _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi2 - phi1)*2 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); - ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); + ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeSteeringRate::computeError() _error[0]\n",_error[0]); } public: @@ -328,6 +335,113 @@ class EdgeSteeringRate : public BaseTebMultiEdge<1, double> }; +//! 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(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + const double dist = (conf2->estimate().position() - conf1->estimate().position()).norm(); + const double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (dist == 0) + phi = 0; + else + { + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2*sin(angle_diff/2)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(_measurement - phi) / 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(_vertices[0]); + const VertexPose* conf2 = static_cast(_vertices[1]); + const VertexTimeDiff* dt = static_cast(_vertices[2]); + + const double dist = (conf2->estimate().position() - conf1->estimate().position()).norm(); + const double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + + double phi; + if (dist == 0) + phi = 0; + else + { + if (cfg_->trajectory.exact_arc_length) + phi = std::atan(cfg_->robot.wheelbase / dist * 2*sin(angle_diff/2)); + else + phi = std::atan(cfg_->robot.wheelbase / dist * angle_diff); + } + + _error[0] = penaltyBoundToInterval(g2o::normalize_theta(phi - _measurement) / 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 + +}; diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index 4f97d20f..422272c7 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -130,6 +130,8 @@ void TebOptimalPlanner::registerG2OTypes() factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_STEERING_RATE", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_START", new g2o::HyperGraphElementCreator); + factory->registerType("EDGE_STEERING_RATE_GOAL", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_KINEMATICS_CARLIKE", new g2o::HyperGraphElementCreator); factory->registerType("EDGE_OBSTACLE", new g2o::HyperGraphElementCreator); @@ -898,8 +900,26 @@ void TebOptimalPlanner::AddEdgesSteeringRate() // create edge for satisfiying kinematic constraints Eigen::Matrix information_steering_rate; information_steering_rate(0, 0) = cfg_->optim.weight_max_steering_rate; + + int n = teb_.sizePoses(); + + // check if an initial velocity should be taken into accound (we apply the same for the steering rate) + if (vel_start_.first) + { + EdgeSteeringRateStart* steering_rate_edge = new EdgeSteeringRateStart; + steering_rate_edge->setVertex(0,teb_.PoseVertex(0)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex(0)); + if (vel_start_.second.linear.x==0.0) + steering_rate_edge->setInitialSteeringAngle(0.0); + else + steering_rate_edge->setInitialSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z) ); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } - for (int i=0; i < teb_.sizePoses()-2; i++) // ignore twiced start only + for (int i=0; i < n-2; i++) // ignore twiced start only { EdgeSteeringRate* steering_rate_edge = new EdgeSteeringRate; steering_rate_edge->setVertex(0,teb_.PoseVertex(i)); @@ -911,6 +931,22 @@ void TebOptimalPlanner::AddEdgesSteeringRate() steering_rate_edge->setTebConfig(*cfg_); optimizer_->addEdge(steering_rate_edge); } + + // check if a goal velocity should be taken into accound (we apply the same for the steering rate) + if (vel_goal_.first) + { + EdgeSteeringRateGoal* steering_rate_edge = new EdgeSteeringRateGoal; + steering_rate_edge->setVertex(0,teb_.PoseVertex(n-2)); + steering_rate_edge->setVertex(1,teb_.PoseVertex(n-1)); + steering_rate_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); + if (vel_goal_.second.linear.x==0.0) + steering_rate_edge->setGoalSteeringAngle(0.0); + else + steering_rate_edge->setGoalSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_goal_.second.linear.x * vel_goal_.second.angular.z) ); + steering_rate_edge->setInformation(information_steering_rate); + steering_rate_edge->setTebConfig(*cfg_); + optimizer_->addEdge(steering_rate_edge); + } } void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) From 0fc9f12e940f1f10b97ed3341e2b7e9146b81558 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Wed, 30 May 2018 15:47:42 +0200 Subject: [PATCH 4/5] steering rate edge does now consider the driving direction. This crucially reduces performance at the moment... --- .../g2o_types/edge_acceleration.h | 6 +- .../g2o_types/edge_velocity.h | 90 ++++++++++++++----- include/teb_local_planner/misc.h | 2 +- include/teb_local_planner/optimal_planner.h | 2 + src/optimal_planner.cpp | 14 ++- 5 files changed, 83 insertions(+), 31 deletions(-) diff --git a/include/teb_local_planner/g2o_types/edge_acceleration.h b/include/teb_local_planner/g2o_types/edge_acceleration.h index 8eccf6de..61fc2b91 100644 --- a/include/teb_local_planner/g2o_types/edge_acceleration.h +++ b/include/teb_local_planner/g2o_types/edge_acceleration.h @@ -121,7 +121,7 @@ class EdgeAcceleration : public BaseTebMultiEdge<2, double> dist2 = fabs( angle_diff2 * radius ); // actual arg length! } } - + double vel1 = dist1 / dt1->dt(); double vel2 = dist2 / dt2->dt(); @@ -129,8 +129,8 @@ class EdgeAcceleration : public BaseTebMultiEdge<2, double> // consider directions // vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); // vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); - vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); - vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); + vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); + vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() ); diff --git a/include/teb_local_planner/g2o_types/edge_velocity.h b/include/teb_local_planner/g2o_types/edge_velocity.h index e9d5a5fb..5fa3fbf7 100755 --- a/include/teb_local_planner/g2o_types/edge_velocity.h +++ b/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(); @@ -297,35 +297,58 @@ class EdgeSteeringRate : public BaseTebMultiEdge<1, double> const VertexTimeDiff* dt1 = static_cast(_vertices[3]); const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - const double dist1 = (conf2->estimate().position() - conf1->estimate().position()).norm(); - const double dist2 = (conf3->estimate().position() - conf2->estimate().position()).norm(); - const double angle_diff1 = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - const double angle_diff2 = g2o::normalize_theta( conf3->theta() - conf2->theta() ); + 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 (dist1 == 0) + 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*sin(angle_diff1/2)); + 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 (dist2 == 0) - phi2 = 0; + 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*sin(angle_diff2/2)); + 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 / (dt1->dt() + dt2->dt()), cfg_->robot.max_steering_rate, 0.0); + _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]); } @@ -358,21 +381,32 @@ class EdgeSteeringRateStart : public BaseTebMultiEdge<1, double> const VertexPose* conf2 = static_cast(_vertices[1]); const VertexTimeDiff* dt = static_cast(_vertices[2]); - const double dist = (conf2->estimate().position() - conf1->estimate().position()).norm(); - const double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + 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 (dist == 0) - phi = 0; + 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*sin(angle_diff/2)); + 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); + _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]); @@ -412,21 +446,31 @@ class EdgeSteeringRateGoal : public BaseTebMultiEdge<1, double> const VertexPose* conf2 = static_cast(_vertices[1]); const VertexTimeDiff* dt = static_cast(_vertices[2]); - const double dist = (conf2->estimate().position() - conf1->estimate().position()).norm(); - const double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); + 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 (dist == 0) + 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*sin(angle_diff/2)); + 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); + _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]); diff --git a/include/teb_local_planner/misc.h b/include/teb_local_planner/misc.h index 09f055af..fadd60e1 100644 --- a/include/teb_local_planner/misc.h +++ b/include/teb_local_planner/misc.h @@ -94,7 +94,7 @@ inline bool smaller_than_abs(double i, double j) {return std::fabs(i) optimizer_; //!< g2o optimizer for trajectory optimization std::pair vel_start_; //!< Store the initial velocity at the start pose std::pair 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 diff --git a/src/optimal_planner.cpp b/src/optimal_planner.cpp index 36c18fe5..e03f2744 100644 --- a/src/optimal_planner.cpp +++ b/src/optimal_planner.cpp @@ -903,7 +903,7 @@ void TebOptimalPlanner::AddEdgesKinematicsDiffDrive() void TebOptimalPlanner::AddEdgesKinematicsCarlike() { - if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius) + if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0) return; // if weight equals zero skip adding edges! // create edge for satisfiying kinematic constraints @@ -941,10 +941,16 @@ void TebOptimalPlanner::AddEdgesSteeringRate() steering_rate_edge->setVertex(0,teb_.PoseVertex(0)); steering_rate_edge->setVertex(1,teb_.PoseVertex(1)); steering_rate_edge->setVertex(2,teb_.TimeDiffVertex(0)); - if (vel_start_.second.linear.x==0.0) - steering_rate_edge->setInitialSteeringAngle(0.0); + if (std::abs(vel_start_.second.linear.x) < 1e-6) + { + //ROS_INFO("TebOptimalPlanner::AddEdgesSteeringRate(): current v close to zero. Using last measured steering angle"); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); // TODO(roesmann): it would be better to measure the actual steering angle + } else - steering_rate_edge->setInitialSteeringAngle(std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z) ); + { + recent_steering_angle_ = std::atan(cfg_->robot.wheelbase/vel_start_.second.linear.x * vel_start_.second.angular.z); + steering_rate_edge->setInitialSteeringAngle(recent_steering_angle_); + } steering_rate_edge->setInformation(information_steering_rate); steering_rate_edge->setTebConfig(*cfg_); optimizer_->addEdge(steering_rate_edge); From 8ff25c0c81eba443f1de5c1887bdf06c4f9d3c40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Wed, 30 May 2018 16:49:51 +0200 Subject: [PATCH 5/5] parameter description updated --- cfg/TebLocalPlannerReconfigure.cfg | 8 ++++---- include/teb_local_planner/teb_config.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/cfg/TebLocalPlannerReconfigure.cfg b/cfg/TebLocalPlannerReconfigure.cfg index 5a5284ce..6468099f 100755 --- a/cfg/TebLocalPlannerReconfigure.cfg +++ b/cfg/TebLocalPlannerReconfigure.cfg @@ -98,8 +98,8 @@ gen.add("min_turning_radius", double_t, 0, 0.0, 0.0, 50.0) gen.add("max_steering_rate", double_t, 0, - "Maximum bound of the steering wheel (requires the definition of the wheelbase) [deactivate: zero]", - 0.0, 0.0, 50.0) + "EXPERIMENTAL: Maximum steering rate of a carlike robot (THIS SIGNIFICANTLY AFFECTS PERFORMANCE AT THE MOMENT) [deactivate: zero]", + 0.0, 0.0, 10.0) gen.add("wheelbase", double_t, 0, @@ -229,8 +229,8 @@ gen.add("weight_kinematics_turning_radius", double_t, 0, 1, 0, 1000) gen.add("weight_max_steering_rate", double_t, 0, - "Optimization weight for enforcing a maximum steering rate (carlike robots)", - 1, 0, 1000) + "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.5, 0, 100) gen.add("weight_optimaltime", double_t, 0, "Optimization weight for contracting the trajectory w.r.t transition time", diff --git a/include/teb_local_planner/teb_config.h b/include/teb_local_planner/teb_config.h index 22ca2de2..924f9668 100644 --- a/include/teb_local_planner/teb_config.h +++ b/include/teb_local_planner/teb_config.h @@ -95,7 +95,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; //!< Maximum bound of the steering wheel (requires the definition of the wheelbase) [deactivate: 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; //