Support for ackermann robots and propose a solution of zero linear velocity problem

ilqr_trajectory_tracker/CMakeLists.txt

@@ -16,7 +16,10 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 
-add_library(ilqr_trajectory_tracker_lib SHARED src/models/diff_robot_model.cpp)
+add_library(
+  ilqr_trajectory_tracker_lib SHARED 
+    src/models/diff_robot_model.cpp
+    src/models/ackermann_robot_model.cpp)
 
 target_include_directories(
   ilqr_trajectory_tracker_lib

ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp

@@ -49,13 +49,12 @@ int main(int argc, char ** argv)
 
   Matrix3d Q = Matrix3d::Identity() * 10;
   Matrix2d R = Matrix2d::Identity() * 0.1;
-  double alpha = 1;
 
   ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
   newton_optimizer.setIterationNumber(10);
-  newton_optimizer.setAlpha(alpha);
 
   const auto start{std::chrono::steady_clock::now()};
+
   // TODO: Fix demo later, we should pass the actual robot pose instead of
   // the beginning pose of the feasible trajectory
   auto u_optimal = newton_optimizer.optimize(x_feasible[0], x_feasible, Q, R, dt);

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp

@@ -19,8 +19,10 @@
 
 #include <Eigen/Dense>
 #include <cmath>
+#include <memory>
 #include <tuple>
 #include <algorithm>
+#include <limits>
 
 using namespace Eigen;
 
@@ -40,15 +42,19 @@ template<typename RobotModel>
 class NewtonOptimizer : public Optimizer
 {
 public:
-  NewtonOptimizer();
+  template<typename ... RobotModelParams>
+  NewtonOptimizer(const RobotModelParams ... model_params);
+
   std::vector<MatrixXd> backwardPass(
     const std::vector<typename RobotModel::StateT> & x_feasible,
     const std::vector<typename RobotModel::InputT> & u_feasible,
     const MatrixXd & Q, const MatrixXd & R, const double dt);
+
   std::tuple<MatrixXd, MatrixXd> solveDiscreteLQRProblem(
     const MatrixXd & A, const MatrixXd & B,
     const MatrixXd & Q, const MatrixXd & R,
     const MatrixXd & P);
+
   std::tuple<std::vector<typename RobotModel::StateT>,
     std::vector<typename RobotModel::InputT>> forwardPass(
     const typename RobotModel::StateT & x0,
@@ -60,23 +66,28 @@ class NewtonOptimizer : public Optimizer
     const typename RobotModel::StateT & x0,
     const std::vector<typename RobotModel::StateT> & x_feasible, const Matrix3d & Q,
     const Matrix2d & R, const double dt);
+
   double cost(
     const std::vector<typename RobotModel::StateT> & x_tracked,
     const std::vector<typename RobotModel::StateT> & x_trajectory);
 
   void setIterationNumber(const size_t iteration_number);
-  void setAlpha(const double alpha);
+
+  void setInputConstraints(
+    typename RobotModel::InputT input_limits_min,
+    typename RobotModel::InputT input_limits_max);
 
 private:
-  RobotModel robot_model_;
-  double alpha_;
+  std::unique_ptr<RobotModel> robot_model_;
   size_t iteration_number_;
 };
 
 template<typename RobotModel>
-NewtonOptimizer<RobotModel>::NewtonOptimizer()
+template<typename ... RobotModelParams>
+NewtonOptimizer<RobotModel>::NewtonOptimizer(const RobotModelParams ... model_params)
 : Optimizer()
 {
+  robot_model_ = std::make_unique<RobotModel>(model_params...);
 }
 
 template<typename RobotModel>
@@ -97,15 +108,15 @@ std::vector<MatrixXd> NewtonOptimizer<RobotModel>::backwardPass(
 
   for (auto i = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 2)); i >= 0; i--) {
     auto x_offset =
-      robot_model_.applySystemDynamics(x_feasible.at(i), u_feasible[i], dt) - x_feasible[i + 1];
+      robot_model_->applySystemDynamics(x_feasible.at(i), u_feasible[i], dt) - x_feasible[i + 1];
 
     MatrixXd A_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
-    auto A = robot_model_.getStateMatrix(x_feasible[i], u_feasible[i], dt);
+    auto A = robot_model_->getStateMatrix(x_feasible[i], u_feasible[i], dt);
     A_tilda.topLeftCorner(state_dimension, state_dimension) = A;
     A_tilda.topRightCorner(state_dimension, 1) = x_offset;
 
     MatrixXd B_tilda = MatrixXd::Zero(state_dimension + 1, input_dimension);
-    auto B = robot_model_.getControlMatrix(x_feasible[i], u_feasible[i], dt);
+    auto B = robot_model_->getControlMatrix(x_feasible[i], u_feasible[i], dt);
     B_tilda.topLeftCorner(state_dimension, input_dimension) = B;
 
     MatrixXd Q_tilda = MatrixXd::Identity(state_dimension + 1, state_dimension + 1);
@@ -137,16 +148,15 @@ std::tuple<std::vector<typename RobotModel::StateT>,
 
   // assert trajectory_size > 0
   x_tracked[0] = x0;
-  for (typename std::vector<typename RobotModel::StateT>::difference_type i = 0,
-    max_difference = std::distance(x_feasible.begin(), std::prev(x_feasible.end(), 1));
-    i < max_difference; ++i)
+  for (size_t i = 0; i < x_feasible.size() - 1; ++i)
   {
     auto x_error = x_tracked[i] - x_feasible[i];
     Vector<double, 4> z_error;
     z_error << x_error, alpha;
 
     u_applied[i] = u_feasible[i] + K_gains[i] * z_error;
-    x_tracked[i + 1] = robot_model_.applySystemDynamics(x_tracked[i], u_applied[i], dt);
+    u_applied[i] = robot_model_->applyLimits(u_applied[i]);
+    x_tracked[i + 1] = robot_model_->applySystemDynamics(x_tracked[i], u_applied[i], dt);
   }
 
   return {x_tracked, u_applied};
@@ -174,7 +184,7 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
 {
   // assert trajectory_size > 0
 
-  double alpha = alpha_;
+  double alpha = 1.0;
 
   std::vector<typename RobotModel::StateT> x_best_trajectory;
   std::vector<typename RobotModel::InputT> u_best_trajectory;
@@ -234,9 +244,11 @@ void NewtonOptimizer<RobotModel>::setIterationNumber(const size_t iteration_numb
 }
 
 template<typename RobotModel>
-void NewtonOptimizer<RobotModel>::setAlpha(const double alpha)
+void NewtonOptimizer<RobotModel>::setInputConstraints(
+  typename RobotModel::InputT input_limits_min,
+  typename RobotModel::InputT input_limits_max)
 {
-  alpha_ = alpha;
+  robot_model_->setLimits(input_limits_min, input_limits_max);
 }
 
 }

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/ackermann_robot_model.hpp

@@ -0,0 +1,47 @@
+// Copyright (C) 2024 Cihat Kurtuluş Altıparmak
+// Copyright (C) 2024 Prof. Tufan Kumbasar, Istanbul Technical University Artificial Intelligence and Intelligent Systems (AI2S) Laboratory
+// Copyright (C) 2024 Prof. Behçet Uğur Töreyin
+//
+// 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/>.
+
+#pragma once
+
+#include <ilqr_trajectory_tracker/models/base_model.hpp>
+
+#include <Eigen/Dense>
+#include <cmath>
+
+using namespace Eigen;
+
+namespace ilqr_trajectory_tracker
+{
+
+using AckermannRobotModelState = Vector3d;
+using AckermannRobotModelInput = Vector2d;
+
+class AckermannRobotModel : public Model<AckermannRobotModelState, AckermannRobotModelInput>
+{
+public:
+  using StateT = AckermannRobotModelState;
+  using InputT = AckermannRobotModelInput;
+  AckermannRobotModel(const double wheelbase);
+  StateT applySystemDynamics(const StateT & x, const InputT & u, const double dt) override;
+  InputT applyLimits(const InputT & u) override;
+  MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+  MatrixXd getControlMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+private:
+  double wheelbase_;
+};
+
+}

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp

@@ -31,15 +31,28 @@ class Model
 public:
   Model();
   virtual StateT applySystemDynamics(const StateT & x, const InputT & u, const double dt) = 0;
+  virtual InputT applyLimits(const InputT & u) = 0;
+  void setLimits(const Vector2d & input_limits_min, const Vector2d & input_limits_max);
   virtual MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt) = 0;
   virtual MatrixXd getControlMatrix(
     const StateT & x_eq, const InputT & u_eq,
     const double dt) = 0;
+protected:
+  InputT input_limits_min_;
+  InputT input_limits_max_;
 };
 
 template<typename StateT, typename InputT>
 Model<StateT, InputT>::Model()
 {
 }
 
+template<typename StateT, typename InputT>
+void Model<StateT, InputT>::setLimits(
+  const Vector2d & input_limits_min,
+  const Vector2d & input_limits_max) {
+
+  input_limits_min_ = input_limits_min;
+  input_limits_max_ = input_limits_max;
+}
 }

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/diff_robot_model.hpp

@@ -36,9 +36,10 @@ class DiffDriveRobotModel : public Model<DiffDriveRobotModelState, DiffDriveRobo
   using StateT = DiffDriveRobotModelState;
   using InputT = DiffDriveRobotModelInput;
   DiffDriveRobotModel();
-  Vector3d applySystemDynamics(const Vector3d & x, const Vector2d & u, const double dt) override;
-  MatrixXd getStateMatrix(const Vector3d & x_eq, const Vector2d & u_eq, const double dt);
-  MatrixXd getControlMatrix(const Vector3d & x_eq, const Vector2d & u_eq, const double dt);
+  DiffDriveRobotModelState applySystemDynamics(const StateT & x, const InputT & u, const double dt) override;
+  InputT applyLimits(const InputT & u) override;
+  MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
+  MatrixXd getControlMatrix(const StateT & x_eq, const InputT & u_eq, const double dt);
 };
 
 }

ilqr_trajectory_tracker/src/models/ackermann_robot_model.cpp

@@ -0,0 +1,74 @@
+// Copyright (C) 2024 Cihat Kurtuluş Altıparmak
+// Copyright (C) 2024 Prof. Tufan Kumbasar, Istanbul Technical University Artificial Intelligence and Intelligent Systems (AI2S) Laboratory
+// Copyright (C) 2024 Prof. Behçet Uğur Töreyin
+//
+// 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/>.
+
+#include <ilqr_trajectory_tracker/models/ackermann_robot_model.hpp>
+#include <Eigen/Dense>
+#include <cmath>
+
+using namespace Eigen;
+
+namespace ilqr_trajectory_tracker
+{
+
+AckermannRobotModel::AckermannRobotModel(const double wheelbase)
+: Model<AckermannRobotModelState, AckermannRobotModelInput>(), wheelbase_(wheelbase)
+{
+
+}
+
+AckermannRobotModelState AckermannRobotModel::applySystemDynamics(
+  const AckermannRobotModelState & x, const AckermannRobotModelInput & u,
+  const double dt)
+{
+  AckermannRobotModelState x_final;
+  x_final <<
+    x[0] + u[0] * std::cos(x[2]) * dt,
+    x[1] + u[0] * std::sin(x[2]) * dt,
+    x[2] + (u[0] / wheelbase_) * std::tan(u[1]) * dt;
+
+  return x_final;
+}
+
+AckermannRobotModelInput AckermannRobotModel::applyLimits(const AckermannRobotModelInput & u) {
+  return u.cwiseMin(input_limits_max_).cwiseMax(input_limits_min_);
+}
+
+MatrixXd AckermannRobotModel::getStateMatrix(
+  const AckermannRobotModelState & x_eq, const AckermannRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix3d state_matrix;
+  state_matrix << 1, 0, -u_eq[0] * std::sin(x_eq[2]) * dt,
+    0, 1, u_eq[0] * std::cos(x_eq[2]) * dt,
+    0, 0, 1;
+
+  return state_matrix;
+}
+
+MatrixXd AckermannRobotModel::getControlMatrix(
+  const AckermannRobotModelState & x_eq, const AckermannRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix<double, 3, 2> control_matrix;
+  control_matrix << std::cos(x_eq[2]) * dt, 0,
+    std::sin(x_eq[2]) * dt, 0,
+    std::tan(u_eq[1]) / wheelbase_ * dt, (u_eq[0] / wheelbase_) * (1 / (std::cos(u_eq[1]) * std::cos(u_eq[1]))) * dt;
+
+  return control_matrix;
+}
+
+}

ilqr_trajectory_tracker/src/models/diff_robot_model.cpp

@@ -30,11 +30,11 @@ DiffDriveRobotModel::DiffDriveRobotModel()
 
 }
 
-Vector3d DiffDriveRobotModel::applySystemDynamics(
-  const Vector3d & x, const Vector2d & u,
+DiffDriveRobotModelState DiffDriveRobotModel::applySystemDynamics(
+  const DiffDriveRobotModelState & x, const DiffDriveRobotModelInput & u,
   const double dt)
 {
-  Vector3d x_final;
+  DiffDriveRobotModelState x_final;
   x_final <<
     x[0] + u[0] * std::cos(x[2] + u[1] * dt) * dt,
     x[1] + u[0] * std::sin(x[2] + u[1] * dt) * dt,
@@ -43,8 +43,12 @@ Vector3d DiffDriveRobotModel::applySystemDynamics(
   return x_final;
 }
 
+DiffDriveRobotModelInput DiffDriveRobotModel::applyLimits(const DiffDriveRobotModelInput & u) {
+  return u.cwiseMin(input_limits_max_).cwiseMax(input_limits_min_);
+}
+
 MatrixXd DiffDriveRobotModel::getStateMatrix(
-  const Vector3d & x_eq, const Vector2d & u_eq,
+  const DiffDriveRobotModelState & x_eq, const DiffDriveRobotModelInput & u_eq,
   const double dt)
 {
   Matrix3d state_matrix;
@@ -56,7 +60,7 @@ MatrixXd DiffDriveRobotModel::getStateMatrix(
 }
 
 MatrixXd DiffDriveRobotModel::getControlMatrix(
-  const Vector3d & x_eq, const Vector2d & /*u_eq*/,
+  const DiffDriveRobotModelState & x_eq, const DiffDriveRobotModelInput & u_eq,
   const double dt)
 {
   Matrix<double, 3, 2> control_matrix;

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp

@@ -37,6 +37,7 @@
 #include "frenet_trajectory_planner/type_definitions.hpp"
 #include "frenet_trajectory_planner/frenet_trajectory_planner.hpp"
 #include "ilqr_trajectory_tracker/models/diff_robot_model.hpp"
+#include "ilqr_trajectory_tracker/models/ackermann_robot_model.hpp"
 #include "ilqr_trajectory_tracker/ilqr_optimizer.hpp"
 
 namespace nav2_frenet_ilqr_controller

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/parameter_handler.hpp

@@ -37,7 +37,8 @@ struct Parameters
   double transform_tolerance;
   double time_discretization;
   int iteration_number;
-  double alpha;
+  std::string robot_type;
+  double wheelbase;
   frenet_trajectory_planner::FrenetTrajectoryPlannerConfig frenet_trajectory_planner_config;
 };
 

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/policies/rclcpp_node_policy.hpp

@@ -33,7 +33,7 @@ namespace policies
 class RclcppNodePolicy : public Policy
 {
 public:
-  RclcppNodePolicy();
+  RclcppNodePolicy() : Policy() {}
   virtual void initialize(
     const std::string & policy_plugin_name,
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
@@ -55,11 +55,6 @@ class RclcppNodePolicy : public Policy
   nav2_costmap_2d::Costmap2D * costmap_{nullptr};
 };
 
-RclcppNodePolicy::RclcppNodePolicy()
-: Policy()
-{
-}
-
 }
 }