diff --git a/frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp b/frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp index d13b81f..6f7cd3b 100644 --- a/frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp +++ b/frenet_trajectory_planner/include/frenet_trajectory_planner/frenet_trajectory_planner.hpp @@ -26,6 +26,8 @@ #include #include +#include +#include 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); } diff --git a/ilqr_trajectory_tracker/CMakeLists.txt b/ilqr_trajectory_tracker/CMakeLists.txt index c95fb1c..f7e7e33 100644 --- a/ilqr_trajectory_tracker/CMakeLists.txt +++ b/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 diff --git a/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp b/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp index 6ddb26d..23ce814 100644 --- a/ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp +++ b/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 newton_optimizer; diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp index 96d73e2..bc94973 100644 --- a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp +++ b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp @@ -24,6 +24,8 @@ #include #include +#include + using namespace Eigen; namespace ilqr_trajectory_tracker @@ -65,7 +67,7 @@ class NewtonOptimizer : public Optimizer std::vector optimize( const typename RobotModel::StateT & x0, const std::vector & x_feasible, const Matrix3d & Q, - const Matrix2d & R, const double dt); + const Matrix3d & R, const double dt); double cost( const std::vector & x_tracked, @@ -183,7 +185,7 @@ template std::vector NewtonOptimizer::optimize( const typename RobotModel::StateT & x0, const std::vector & 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 NewtonOptimizer::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 NewtonOptimizer::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; } diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp index 111d3fc..d25b29e 100644 --- a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/base_model.hpp +++ b/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 Model::Model() { - input_limits_min_ << -std::numeric_limits::infinity(), -std::numeric_limits::infinity(); - input_limits_max_ << +std::numeric_limits::infinity(), +std::numeric_limits::infinity(); + input_limits_min_ << -std::numeric_limits::infinity(), -std::numeric_limits::infinity(), -std::numeric_limits::infinity(); + input_limits_max_ << +std::numeric_limits::infinity(), +std::numeric_limits::infinity(), +std::numeric_limits::infinity(); } template void Model::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; diff --git a/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/omni_robot_model.hpp b/ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/models/omni_robot_model.hpp new file mode 100644 index 0000000..ff6d40a --- /dev/null +++ b/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 . + +#pragma once + +#include + +#include +#include + +using namespace Eigen; + +namespace ilqr_trajectory_tracker +{ + +using OmniDriveRobotModelState = Vector3d; +using OmniDriveRobotModelInput = Vector3d; + +class OmniDriveRobotModel : public Model +{ +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); +}; + +} diff --git a/ilqr_trajectory_tracker/src/models/omni_robot_model.cpp b/ilqr_trajectory_tracker/src/models/omni_robot_model.cpp new file mode 100644 index 0000000..a408a44 --- /dev/null +++ b/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 . + +#include +#include +#include + +using namespace Eigen; + +namespace ilqr_trajectory_tracker +{ + +OmniDriveRobotModel::OmniDriveRobotModel() +: Model() +{ + +} + +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 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; +} + +} diff --git a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp b/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp index 3a40e5a..8570bb5 100644 --- a/nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp +++ b/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); diff --git a/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp b/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp index 367b11e..22d4fe1 100644 --- a/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp +++ b/nav2_frenet_ilqr_controller/src/frenet_ilqr_controller.cpp @@ -30,6 +30,8 @@ #include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include +#include + 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 X_feasible; + std::vector 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 newton_optimizer; - newton_optimizer.setIterationNumber(20); + ilqr_trajectory_tracker::NewtonOptimizer 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; } diff --git a/nav2_frenet_ilqr_controller/src/parameter_handler.cpp b/nav2_frenet_ilqr_controller/src/parameter_handler.cpp index 9c72219..2349bd0 100644 --- a/nav2_frenet_ilqr_controller/src/parameter_handler.cpp +++ b/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({0.0, -1.5}))); + node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_min", rclcpp::ParameterValue(std::vector({-0.5, -0.5, -0.3}))); declare_parameter_if_not_declared( - node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_max", rclcpp::ParameterValue(std::vector({1.0, 1.5}))); + node, plugin_name_ + ".ilqr_trajectory_tracker.input_limits_max", rclcpp::ParameterValue(std::vector({0.5, 0.5, 0.3}))); node->get_parameter(