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 << ".");