Added kinematic bicycle model with velocity input

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:

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 <https://www.gnu.org/licenses/>.
+ *
+ *  Authors: Christoph Rösmann
+ *********************************************************************/
+
+#ifndef SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_
+#define SYSTEMS_KINEMATIC_BICYCLE_MODEL_H_
+
+#include <mpc_local_planner/systems/base_robot_se2.h>
+
+#include <cmath>
+
+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<KinematicBicycleModelVelocityInput>(); }
+
+    // implements interface method
+    int getInputDimension() const override { return 2; }
+
+    // implements interface method
+    void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> 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<const Eigen::VectorXd>& 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_

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
+ * @see SimpleCarFrontWheelDrivingModel KinematicBicycleModelVelocityInput
- *      corbo::SystemDynamicsInterface
+ *      BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface
  *
  * @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
  */

mpc_local_planner/src/controller.cpp

@@ -36,6 +36,7 @@
 #include <mpc_local_planner/optimal_control/min_time_via_points_cost.h>
 #include <mpc_local_planner/optimal_control/quadratic_cost_se2.h>
 #include <mpc_local_planner/optimal_control/stage_inequality_se2.h>
+#include <mpc_local_planner/systems/kinematic_bicycle_model.h>
 #include <mpc_local_planner/systems/simple_car.h>
 #include <mpc_local_planner/systems/unicycle_robot.h>
 #include <mpc_local_planner/utils/time_series_se2.h>
@@ -359,6 +360,14 @@ RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHa
         else
             return std::make_shared<SimpleCarModel>(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<KinematicBicycleModelVelocityInput>(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 << ".");