From ec6a0c4406c3ff58e3d57ae2612c3e9e3da9e9d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20R=C3=B6smann?= Date: Sun, 24 May 2020 09:50:41 +0200 Subject: [PATCH] Added kinematic bicycle model with velocity input --- .../cfg/test_mpc_optim_node.yaml | 9 ++ .../systems/kinematic_bicycle_model.h | 110 ++++++++++++++++++ .../mpc_local_planner/systems/simple_car.h | 4 +- mpc_local_planner/src/controller.cpp | 46 ++++++++ 4 files changed, 167 insertions(+), 2 deletions(-) create mode 100644 mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h diff --git a/mpc_local_planner/cfg/test_mpc_optim_node.yaml b/mpc_local_planner/cfg/test_mpc_optim_node.yaml index 1080587..1df3b8d 100644 --- a/mpc_local_planner/cfg/test_mpc_optim_node.yaml +++ b/mpc_local_planner/cfg/test_mpc_optim_node.yaml @@ -17,6 +17,15 @@ robot: acc_lim_x: 0.0 # deactive bounds with zero dec_lim_x: 0.0 # deactive bounds with zero max_steering_rate: 0.5 # deactive bounds with zero + kinematic_bicycle_vel_input: + length_rear: 1.0 + length_front: 1.0 + max_vel_x: 0.4 + max_vel_x_backwards: 0.2 + max_steering_angle: 1.4 + acc_lim_x: 0.0 # deactive bounds with zero + dec_lim_x: 0.0 # deactive bounds with zero + max_steering_rate: 0.5 # deactive bounds with zero ## Footprint model for collision avoidance footprint_model: diff --git a/mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h b/mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h new file mode 100644 index 0000000..0ad1b74 --- /dev/null +++ b/mpc_local_planner/include/mpc_local_planner/systems/kinematic_bicycle_model.h @@ -0,0 +1,110 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ +#define SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ + +#include + +#include + +namespace mpc_local_planner { + +/** + * @brief Kinematic Bicycle Model with Velocity Input + * + * This class implements the dynamics for a kinematic bicycle model. + * Note, in this model the robot coordinate system is located at the center of gravity + * which is between the rear and the front axle. + * In case you want to define the coordinate system at the rear axle, + * please refer to the simplified equations simple_car.h (SimpleCarModel). + * + * [1] R. Rajamani, Vehicle Dynamics and Control, Springer, 2012. + * [2] J. Kong et al., Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design, IV, 2015. + * + * @see SimpleCarModel SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface + * corbo::SystemDynamicsInterface + * + * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) + */ +class KinematicBicycleModelVelocityInput : public BaseRobotSE2 +{ + public: + //! Default constructor + KinematicBicycleModelVelocityInput() = default; + + //! Constructs model with given wheelbase + KinematicBicycleModelVelocityInput(double lr, double lf) : _lr(lr), _lf(lf) {} + + // implements interface method + SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared(); } + + // implements interface method + int getInputDimension() const override { return 2; } + + // implements interface method + void dynamics(const Eigen::Ref& x, const Eigen::Ref& u, Eigen::Ref f) const override + { + assert(x.size() == getStateDimension()); + assert(u.size() == getInputDimension()); + assert(x.size() == f.size() && + "KinematicBicycleModelVelocityInput::dynamics(): x and f are not of the same size, do not forget to pre-allocate f."); + + double beta = std::atan(_lr / (_lf + _lr) * std::tan(u[1])); + + f[0] = u[0] * std::cos(x[2] + beta); + f[1] = u[0] * std::sin(x[2] + beta); + f[2] = u[0] * std::sin(beta) / _lr; + } + + // implements interface method + bool getTwistFromControl(const Eigen::Ref& u, geometry_msgs::Twist& twist) const override + { + assert(u.size() == getInputDimension()); + twist.linear.x = u[0]; + twist.linear.y = twist.linear.z = 0; + + twist.angular.z = u[1]; // warning, this is the angle and not the angular vel + twist.angular.x = twist.angular.y = 0; + + return true; + } + + //! Set parameters + void setParameters(double lr, double lf) + { + _lr = lr; + _lf = lf; + } + //! Get length between COG and front axle + double getLengthFront() const { return _lf; } + //! Get length between COG and rear axle + double getLengthRear() const { return _lr; } + + protected: + double _lr = 1.0; + double _lf = 1.0; +}; + +} // namespace mpc_local_planner + +#endif // SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_ diff --git a/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h b/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h index 83f0ba8..cb97c16 100644 --- a/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h +++ b/mpc_local_planner/include/mpc_local_planner/systems/simple_car.h @@ -44,8 +44,8 @@ namespace mpc_local_planner { * in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998. * (https://homepages.laas.fr/jpl/promotion/chap4.pdf) * - * @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface - * corbo::SystemDynamicsInterface + * @see SimpleCarFrontWheelDrivingModel KinematicBicycleModelVelocityInput + * BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface * * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de) */ diff --git a/mpc_local_planner/src/controller.cpp b/mpc_local_planner/src/controller.cpp index a9d4534..964a0c1 100644 --- a/mpc_local_planner/src/controller.cpp +++ b/mpc_local_planner/src/controller.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -359,6 +360,14 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa else return std::make_shared(wheelbase); } + else if (_robot_type == "kinematic_bicycle_vel_input") + { + double length_rear = 1.0; + nh.param("robot/kinematic_bicycle_vel_input/length_rear", length_rear, length_rear); + double length_front = 1.0; + nh.param("robot/kinematic_bicycle_vel_input/length_front", length_front, length_front); + return std::make_shared(length_rear, length_front); + } else { ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified."); @@ -516,6 +525,22 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle)); } + else if (_robot_type == "kinematic_bicycle_vel_input") + { + double max_vel_x = 0.4; + nh.param("robot/kinematic_bicycle_vel_input/max_vel_x", max_vel_x, max_vel_x); + double max_vel_x_backwards = 0.2; + nh.param("robot/kinematic_bicycle_vel_input/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards); + if (max_vel_x_backwards < 0) + { + ROS_WARN("max_vel_x_backwards must be >= 0"); + max_vel_x_backwards *= -1; + } + double max_steering_angle = 1.5; + nh.param("robot/kinematic_bicycle_vel_input/max_steering_angle", max_steering_angle, max_steering_angle); + + ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle)); + } else { ROS_ERROR_STREAM("Cannot configure OCP for unknown robot type " << _robot_type << "."); @@ -724,6 +749,27 @@ corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros:: Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate); _inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub); } + else if (_robot_type == "kinematic_bicycle_vel_input") + { + double acc_lim_x = 0.0; + nh.param("robot/kinematic_bicycle_vel_input/acc_lim_x", acc_lim_x, acc_lim_x); + double dec_lim_x = 0.0; + nh.param("robot/kinematic_bicycle_vel_input/dec_lim_x", dec_lim_x, dec_lim_x); + if (dec_lim_x < 0) + { + ROS_WARN("dec_lim_x must be >= 0"); + dec_lim_x *= -1; + } + double max_steering_rate = 0.0; + nh.param("robot/kinematic_bicycle_vel_input/max_steering_rate", max_steering_rate, max_steering_rate); + + if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL; + if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL; + if (max_steering_rate <= 0) max_steering_rate = corbo::CORBO_INF_DBL; + Eigen::Vector2d ud_lb(-dec_lim_x, -max_steering_rate); + Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate); + _inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub); + } else { ROS_ERROR_STREAM("Cannot configure control deviation bounds for unknown robot type " << _robot_type << ".");