Add experimental omni drive robot support

frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp

@@ -26,6 +26,8 @@
 #include <frenet_trajectory_planner/policies/acceleration_policy.hpp>
 
 #include <memory>
+#include <iostream>
+#include <cmath>
 
 namespace frenet_trajectory_planner
 {
@@ -107,6 +109,11 @@ CartesianTrajectory FrenetTrajectoryPlanner::planByWaypoint(
     if (!best_frenet_trajectory_optional.has_value()) {
       break;
     }
+
+    // for (auto miku : best_frenet_trajectory_optional.value()) {
+    //   std::cout << "MIKU : " << miku[1] << " | " << miku[4] << " | " << std::hypot(miku[1], miku[4]) << std::endl;
+    // }
+
     auto best_frenet_trajectory = best_frenet_trajectory_optional.value();
     if (best_frenet_trajectory.size() <= 1) {
       break;
@@ -120,6 +127,8 @@ CartesianTrajectory FrenetTrajectoryPlanner::planByWaypoint(
       planned_frenet_trajectory.size();
   }
 
+  // std::cout << "ADMIRABLE MIKu" << std::endl;
+
   return frenet_frame_converter->convertFrenet2Cartesian(planned_frenet_trajectory);
 }
 

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/omni_robot_model.cpp)
 
 target_include_directories(
   ilqr_trajectory_tracker_lib

ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp

@@ -48,7 +48,7 @@ int main(int argc, char ** argv)
   }
 
   Matrix3d Q = Matrix3d::Identity() * 10;
-  Matrix2d R = Matrix2d::Identity() * 0.1;
+  Matrix3d R = Matrix3d::Identity() * 0.1;
   double alpha = 1;
 
   ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp

@@ -24,6 +24,8 @@
 #include <algorithm>
 #include <limits>
 
+#include <iostream>
+
 using namespace Eigen;
 
 namespace ilqr_trajectory_tracker
@@ -65,7 +67,7 @@ class NewtonOptimizer : public Optimizer
   std::vector<typename RobotModel::InputT> optimize(
     const typename RobotModel::StateT & x0,
     const std::vector<typename RobotModel::StateT> & x_feasible, const Matrix3d & Q,
-    const Matrix2d & R, const double dt);
+    const Matrix3d & R, const double dt);
 
   double cost(
     const std::vector<typename RobotModel::StateT> & x_tracked,
@@ -183,7 +185,7 @@ template<typename RobotModel>
 std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
   const typename RobotModel::StateT & x0,
   const std::vector<typename RobotModel::StateT> & x_trajectory, const Matrix3d & Q,
-  const Matrix2d & R, const double dt)
+  const Matrix3d & R, const double dt)
 {
   // assert trajectory_size > 0
 
@@ -206,6 +208,8 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
     u_optimized = u_tracked;
 
     double trajectory_cost = this->cost(x_tracked, x_trajectory);
+
+    // std::cout << "traj cost : " << trajectory_cost << std::endl;
     if (trajectory_cost < best_trajectory_cost) {
       previous_best_trajectory_cost = best_trajectory_cost;
       best_trajectory_cost = trajectory_cost;
@@ -221,6 +225,13 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
       alpha /= 0.7;
     }
   }
+  // std::cout << "------------------------" << std::endl;
+
+  // for (auto x : x_best_trajectory) {
+  //   std::cout << "[" << x[0] << ", " << x[1] << ", " << x[2] << "]," << std::endl;
+  // }
+  // std::cout << "------------------------" << std::endl;
+
 
   return u_best_trajectory;
 }

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp

@@ -34,7 +34,7 @@ class Model
 
   virtual InputT applyLimits(const InputT & u) = 0;
 
-  void setLimits(const Vector2d & input_limits_min, const Vector2d & input_limits_max);
+  void setLimits(const InputT & input_limits_min, const InputT & input_limits_max);
 
   virtual MatrixXd getStateMatrix(const StateT & x_eq, const InputT & u_eq, const double dt) = 0;
 
@@ -50,14 +50,14 @@ class Model
 template<typename StateT, typename InputT>
 Model<StateT, InputT>::Model()
 {
-  input_limits_min_ << -std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity();
-  input_limits_max_ << +std::numeric_limits<double>::infinity(), +std::numeric_limits<double>::infinity();
+  input_limits_min_ << -std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity();
+  input_limits_max_ << +std::numeric_limits<double>::infinity(), +std::numeric_limits<double>::infinity(), +std::numeric_limits<double>::infinity();
 }
 
 template<typename StateT, typename InputT>
 void Model<StateT, InputT>::setLimits(
-  const Vector2d & input_limits_min,
-  const Vector2d & input_limits_max) {
+  const InputT & input_limits_min,
+  const InputT & input_limits_max) {
 
   input_limits_min_ = input_limits_min;
   input_limits_max_ = input_limits_max;

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/omni_robot_model.hpp

@@ -0,0 +1,45 @@
+// 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 OmniDriveRobotModelState = Vector3d;
+using OmniDriveRobotModelInput = Vector3d;
+
+class OmniDriveRobotModel : public Model<OmniDriveRobotModelState, OmniDriveRobotModelInput>
+{
+public:
+  using StateT = OmniDriveRobotModelState;
+  using InputT = OmniDriveRobotModelInput;
+  OmniDriveRobotModel();
+  OmniDriveRobotModelState 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/omni_robot_model.cpp

@@ -0,0 +1,76 @@
+// 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/omni_robot_model.hpp>
+#include <Eigen/Dense>
+#include <cmath>
+
+using namespace Eigen;
+
+namespace ilqr_trajectory_tracker
+{
+
+OmniDriveRobotModel::OmniDriveRobotModel()
+: Model<OmniDriveRobotModelState, OmniDriveRobotModelInput>()
+{
+
+}
+
+OmniDriveRobotModelState OmniDriveRobotModel::applySystemDynamics(
+  const OmniDriveRobotModelState & x, const OmniDriveRobotModelInput & u,
+  const double dt)
+{
+  OmniDriveRobotModelState x_final;
+  x_final <<
+    x[0] + u[0] * std::cos(x[2]) * dt - u[1] * std::sin(x[2]) * dt,
+    x[1] + u[0] * std::sin(x[2]) * dt + u[1] * std::cos(x[2]) * dt,
+    x[2] + u[2] * dt;
+
+  return x_final;
+}
+
+OmniDriveRobotModelInput OmniDriveRobotModel::applyLimits(const OmniDriveRobotModelInput & u) {
+  return u.cwiseMin(input_limits_max_).cwiseMax(input_limits_min_);
+}
+
+MatrixXd OmniDriveRobotModel::getStateMatrix(
+  const OmniDriveRobotModelState & x_eq, const OmniDriveRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix3d state_matrix;
+  state_matrix << 
+    1, 0, -u_eq[0] * std::sin(x_eq[2]) * dt - u_eq[1] * std::cos(x_eq[2]) * dt,
+    0, 1, +u_eq[0] * std::cos(x_eq[2]) * dt - u_eq[1] * std::sin(x_eq[2]) * dt,
+    0, 0, 1;
+
+  return state_matrix;
+}
+
+MatrixXd OmniDriveRobotModel::getControlMatrix(
+  const OmniDriveRobotModelState & x_eq, const OmniDriveRobotModelInput & u_eq,
+  const double dt)
+{
+  Matrix<double, 3, 3> control_matrix;
+  control_matrix << 
+    std::cos(x_eq[2]) * dt, -std::sin(x_eq[2]) * dt, 0,
+    std::sin(x_eq[2]) * dt,  std::cos(x_eq[2]) * dt, 0,
+    0, 0, dt;
+
+  return 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/omni_robot_model.hpp"
 #include "ilqr_trajectory_tracker/ilqr_optimizer.hpp"
 
 namespace nav2_frenet_ilqr_controller
@@ -108,7 +109,7 @@ class FrenetILQRController : public nav2_core::Controller
     const geometry_msgs::msg::Twist & velocity,
     nav2_core::GoalChecker * /*goal_checker*/) override;
 
-  Vector2d findOptimalInputForTrajectory(
+  Vector3d findOptimalInputForTrajectory(
     const Vector3d & c_state_robot,
     const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory);
 

nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp

@@ -30,6 +30,8 @@
 #include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 #include <pluginlib/class_loader.hpp>
 
+#include <iostream>
+
 using std::hypot;
 using std::min;
 using std::max;
@@ -205,30 +207,36 @@ nav_msgs::msg::Path FrenetILQRController::truncateGlobalPlanWithLookAheadDist(
   return truncated_path;
 }
 
-Vector2d FrenetILQRController::findOptimalInputForTrajectory(
+Vector3d FrenetILQRController::findOptimalInputForTrajectory(
   const Vector3d & c_state_robot,
   const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory)
 {
 
-  using ilqr_trajectory_tracker::DiffDriveRobotModel;
-  using ilqr_trajectory_tracker::DiffDriveRobotModelState;
-  using ilqr_trajectory_tracker::DiffDriveRobotModelInput;
+  using ilqr_trajectory_tracker::OmniDriveRobotModel;
+  using ilqr_trajectory_tracker::OmniDriveRobotModelState;
+  using ilqr_trajectory_tracker::OmniDriveRobotModelInput;
 
-  std::vector<DiffDriveRobotModelState> X_feasible;
+  std::vector<OmniDriveRobotModelState> X_feasible;
+  // std::cout << "[" << c_state_robot[0] << ", " << c_state_robot[1] << ", " << c_state_robot[2] << "]," << std::endl;
   for (auto cartesian_state : robot_cartesian_trajectory) {
-    DiffDriveRobotModelState x;
+    OmniDriveRobotModelState x;
     x << cartesian_state[0],
       cartesian_state[3],
       cartesian_state[6];
     X_feasible.push_back(x);
+
+    // std::cout << "[" << x[0] << ", " << x[1] << ", " << x[2] << "]," << std::endl;
   }
+  // std::cout << "******************" << std::endl;
+  X_feasible.erase(X_feasible.begin());
+  // X_feasible.erase(X_feasible.begin());
 
-  Matrix3d Q = Matrix3d::Identity() * 10;
-  Matrix2d R = Matrix2d::Identity() * 2;
+  Matrix3d Q = Matrix3d::Identity() * 1;
+  Matrix3d R = Matrix3d::Identity() * 0.1;
   double alpha = 1;
   double dt = 0.05;
-  ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
-  newton_optimizer.setIterationNumber(20);
+  ilqr_trajectory_tracker::NewtonOptimizer<OmniDriveRobotModel> newton_optimizer;
+  newton_optimizer.setIterationNumber(40);
   newton_optimizer.setAlpha(alpha);
   newton_optimizer.setInputConstraints(params_->input_limits_min, params_->input_limits_max);
   auto U_optimal = newton_optimizer.optimize(c_state_robot, X_feasible, Q, R, dt);
@@ -278,14 +286,18 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
   frenet_trajectory_planner::CartesianState robot_cartesian_state =
     frenet_trajectory_planner::CartesianState::Zero();
   double robot_yaw = tf2::getYaw(robot_pose.pose.orientation);
-  double linear_speed = speed.linear.x;
-  if (linear_speed == 0) {
-    linear_speed = 0.001;
-  }
+
+  // (void) speed;
+  // (void) robot_yaw;
   robot_cartesian_state[0] = robot_pose.pose.position.x;
-  robot_cartesian_state[1] = linear_speed * std::cos(robot_yaw);
+  robot_cartesian_state[1] = speed.linear.x * std::cos(robot_yaw) - speed.linear.y * std::sin(robot_yaw);
   robot_cartesian_state[3] = robot_pose.pose.position.y;
-  robot_cartesian_state[4] = linear_speed * std::sin(robot_yaw);
+  robot_cartesian_state[4] = speed.linear.x * std::sin(robot_yaw) + speed.linear.y * std::cos(robot_yaw);
+
+  if (std::hypot(speed.linear.x, speed.linear.y) < 0.01 ) {
+    robot_cartesian_state[2] = 2.0 * std::cos(robot_yaw);
+    robot_cartesian_state[5] = 2.0 * std::sin(robot_yaw);
+  } 
 
   frenet_trajectory_planner_.setFrenetTrajectoryPlannerConfig(
     params_->frenet_trajectory_planner_config);
@@ -317,7 +329,8 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
   geometry_msgs::msg::TwistStamped cmd_vel;
   cmd_vel.header = pose.header;
   cmd_vel.twist.linear.x = u_opt[0];
-  cmd_vel.twist.angular.z = u_opt[1];
+  cmd_vel.twist.linear.y = u_opt[1];
+  cmd_vel.twist.angular.z = u_opt[2];
   return cmd_vel;
 }
 

nav2_frenet_ilqr_controller/src/parameter_handler.cpp

@@ -81,10 +81,10 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".ilqr_trajectory_tracker.alpha", rclcpp::ParameterValue(1.0));
 
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_min", rclcpp::ParameterValue(std::vector<double>({0.0, -1.5})));
+    node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_min", rclcpp::ParameterValue(std::vector<double>({-0.5, -0.5, -0.3})));
 
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_max", rclcpp::ParameterValue(std::vector<double>({1.0, 1.5})));
+    node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_max", rclcpp::ParameterValue(std::vector<double>({0.5, 0.5, 0.3})));
 
 
   node->get_parameter(