Debug and additional fix for ilqr optimization

Author: CihatAltiparmak

ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp

@@ -49,14 +49,14 @@ 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()};
-  auto u_optimal = newton_optimizer.optimize(x_feasible, Q, R, dt);
+  // TODO: pass parameter of actual robot pose instead of 
+  // passing the first state of feasible trajectory
+  auto u_optimal = newton_optimizer.optimize(x_feasible[0], x_feasible, Q, R, dt);
   const auto end{std::chrono::steady_clock::now()};
 
   const std::chrono::duration<double> elapsed_seconds{end - start};

ilqr_trajectory_tracker/include/ilqr_trajectory_tracker/ilqr_optimizer.hpp

@@ -57,11 +57,13 @@ class NewtonOptimizer : public Optimizer
 
   std::tuple<std::vector<typename RobotModel::StateT>,
     std::vector<typename RobotModel::InputT>> forwardPass(
+    const typename RobotModel::StateT x0,
     const std::vector<typename RobotModel::StateT> & x_feasible,
     const std::vector<typename RobotModel::InputT> & u_feasible,
     const std::vector<MatrixXd> & K_gains, const double dt, const double alpha);
 
   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);
 
@@ -71,15 +73,12 @@ class NewtonOptimizer : public Optimizer
 
   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:
   std::unique_ptr<RobotModel> robot_model_;
-  double alpha_;
   size_t iteration_number_;
 };
 
@@ -136,6 +135,7 @@ std::vector<MatrixXd> NewtonOptimizer<RobotModel>::backwardPass(
 template<typename RobotModel>
 std::tuple<std::vector<typename RobotModel::StateT>,
   std::vector<typename RobotModel::InputT>> NewtonOptimizer<RobotModel>::forwardPass(
+  const typename RobotModel::StateT x0,
   const std::vector<typename RobotModel::StateT> & x_feasible,
   const std::vector<typename RobotModel::InputT> & u_feasible,
   const std::vector<MatrixXd> & K_gains, const double dt, const double alpha)
@@ -147,10 +147,8 @@ std::tuple<std::vector<typename RobotModel::StateT>,
   std::vector<typename RobotModel::InputT> u_applied(input_size);
 
   // assert trajectory_size > 0
-  x_tracked[0] = x_feasible[0];
-  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)
+  x_tracked[0] = x0;
+  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;
@@ -180,12 +178,13 @@ std::tuple<MatrixXd, MatrixXd> NewtonOptimizer<RobotModel>::solveDiscreteLQRProb
 
 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)
 {
   // 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;
@@ -198,7 +197,7 @@ std::vector<typename RobotModel::InputT> NewtonOptimizer<RobotModel>::optimize(
   double previous_best_trajectory_cost = best_trajectory_cost;
   for (size_t i = 0; i < iteration_number_; ++i) {
     auto K_gain_list = this->backwardPass(x_trajectory, u_optimized, Q, R, dt);
-    auto [x_tracked, u_tracked] = this->forwardPass(
+    auto [x_tracked, u_tracked] = this->forwardPass(x0,
       x_trajectory, u_optimized, K_gain_list, dt,
       alpha);
     u_optimized = u_tracked;
@@ -244,12 +243,6 @@ void NewtonOptimizer<RobotModel>::setIterationNumber(const size_t iteration_numb
   iteration_number_ = iteration_number;
 }
 
-template<typename RobotModel>
-void NewtonOptimizer<RobotModel>::setAlpha(const double alpha)
-{
-  alpha_ = alpha;
-}
-
 template<typename RobotModel>
 void NewtonOptimizer<RobotModel>::setInputConstraints(
   typename RobotModel::InputT input_limits_min,

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp

@@ -110,7 +110,7 @@ class FrenetILQRController : public nav2_core::Controller
     nav2_core::GoalChecker * /*goal_checker*/) override;
 
   Vector2d findOptimalInputForTrajectory(
-    const geometry_msgs::msg::PoseStamped & robot_pose,
+    const Vector3d & x0,
     const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory);
 
   bool cancel() override;

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/src/frenet_ilqr_controller.cpp

@@ -206,7 +206,7 @@ nav_msgs::msg::Path FrenetILQRController::truncateGlobalPlanWithLookAheadDist(
 }
 
 Vector2d FrenetILQRController::findOptimalInputForTrajectory(
-  const geometry_msgs::msg::PoseStamped & /*robot_pose*/,
+  const Vector3d & x0,
   const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory)
 {
 
@@ -219,7 +219,8 @@ Vector2d FrenetILQRController::findOptimalInputForTrajectory(
   using ilqr_trajectory_tracker::AckermannRobotModelInput;
 
   std::vector<AckermannRobotModelState> X_feasible;
-  for (auto cartesian_state : robot_cartesian_trajectory) {
+  for (size_t i = 1; i < robot_cartesian_trajectory.size(); ++i) {
+    auto cartesian_state = robot_cartesian_trajectory[i];
     AckermannRobotModelState x;
     x << cartesian_state[0],
       cartesian_state[3],
@@ -229,26 +230,62 @@ Vector2d FrenetILQRController::findOptimalInputForTrajectory(
 
   Matrix3d Q = Matrix3d::Identity() * 1;
   Matrix2d R = Matrix2d::Identity() * 0.2;
-  double alpha = 1;
-  double dt = 0.05;
-  ilqr_trajectory_tracker::NewtonOptimizer<AckermannRobotModel> newton_optimizer(0.5);
 
-  AckermannRobotModelInput input_limits_min;
-  input_limits_min << -0.5, -2.5;
+  if (params_->robot_type == "ackermann") {
+    using ilqr_trajectory_tracker::AckermannRobotModel;
+    ilqr_trajectory_tracker::NewtonOptimizer<AckermannRobotModel> newton_optimizer(params_->wheelbase);
 
-  AckermannRobotModelInput input_limits_max;
-  input_limits_max << 0.5, 2.5;
-  newton_optimizer.setInputConstraints(input_limits_min, input_limits_max);
+    Vector2d input_limits_min;
+    input_limits_min << -0.5, -0.523599;
 
-  newton_optimizer.setIterationNumber(20);
-  newton_optimizer.setAlpha(alpha);
-  auto U_optimal = newton_optimizer.optimize(X_feasible, Q, R, dt);
+    Vector2d input_limits_max;
+    input_limits_max << 0.5, 0.523599;
+    newton_optimizer.setInputConstraints(input_limits_min, input_limits_max);
 
-  if (U_optimal.empty()) {
-    throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
-  }
+    newton_optimizer.setIterationNumber(20);
+    auto U_optimal = newton_optimizer.optimize(x0, X_feasible, Q, R, params_->time_discretization);
+
+    RCLCPP_INFO(logger_, "--------------------");
+    for (auto cstate : X_feasible) {
+      RCLCPP_INFO(logger_, "[%f, %f, %f]", cstate[0], cstate[1], cstate[2]);
+    }
+    RCLCPP_INFO(logger_, "********************");
+
+    if (U_optimal.empty()) {
+      throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
+    }
+
+    Vector2d u_ackermann;
+    u_ackermann << U_optimal[0][0], (U_optimal[0][0] / params_->wheelbase) * std::tan(U_optimal[0][1]);
+    RCLCPP_INFO(logger_, "--------------------");
+    for (auto u : U_optimal) {
+      RCLCPP_INFO(logger_, "[%f, %f]", u[0], u[1]);
+    }
+    RCLCPP_INFO(logger_, "********************");
+
+    return u_ackermann;
+  } else if (params_->robot_type == "diffdrive") {
+    using ilqr_trajectory_tracker::DiffDriveRobotModel;
+    ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
 
-  return U_optimal[0];
+    Vector2d input_limits_min;
+    input_limits_min << -0.5, -2.5;
+
+    Vector2d input_limits_max;
+    input_limits_max << 0.5, 2.5;
+    newton_optimizer.setInputConstraints(input_limits_min, input_limits_max);
+
+    newton_optimizer.setIterationNumber(20);
+    auto U_optimal = newton_optimizer.optimize(x0, X_feasible, Q, R, params_->time_discretization);
+
+    if (U_optimal.empty()) {
+      throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
+    }
+
+    return U_optimal[0];
+  } else {
+    throw nav2_core::NoValidControl("Unknown robot type! Possible robot types : ackermann, diffdrive");
+  }
 }
 
 geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
@@ -289,19 +326,19 @@ 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;
+  double linear_speed = std::abs(speed.linear.x);
 
   // if (std::abs(linear_speed) <= 0.5) {
   //   params_->frenet_trajectory_planner_config.time_interval = 10;
   // }
-  double max_curvature = 2.5;
-  linear_speed = 1 / max_curvature;
+  // double max_curvature = 2.5;
+  // linear_speed = 1 / max_curvature;
   robot_cartesian_state[0] = robot_pose.pose.position.x;
   robot_cartesian_state[1] = linear_speed * std::cos(robot_yaw);
-  robot_cartesian_state[2] = -linear_speed * std::sin(robot_yaw);
+  // robot_cartesian_state[2] = -linear_speed * 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[5] = linear_speed * std::cos(robot_yaw);
+  // robot_cartesian_state[5] = linear_speed * std::cos(robot_yaw);
   robot_cartesian_state[6] = robot_yaw;
 
   frenet_trajectory_planner_.setFrenetTrajectoryPlannerConfig(
@@ -319,13 +356,16 @@ geometry_msgs::msg::TwistStamped FrenetILQRController::computeVelocityCommands(
     transformed_plan.header.frame_id,
     planned_cartesian_trajectory);
   frenet_plan = path_handler_->transformPath(costmap_ros_->getBaseFrameID(), frenet_plan);
-
   planned_cartesian_trajectory = convertToCartesianTrajectory(frenet_plan);
 
   truncated_path_pub_->publish(frenet_plan);
   robot_pose_pub_->publish(robot_pose);
 
-  auto u_opt = findOptimalInputForTrajectory(robot_pose, planned_cartesian_trajectory);
+  path_handler_->transformPose(costmap_ros_->getBaseFrameID(), robot_pose, robot_pose);
+  Vector3d c_state_robot;
+  c_state_robot << robot_pose.pose.position.x, robot_pose.pose.position.y,
+    tf2::getYaw(robot_pose.pose.orientation);
+  auto u_opt = findOptimalInputForTrajectory(c_state_robot, planned_cartesian_trajectory);
 
   // populate and return message
   geometry_msgs::msg::TwistStamped cmd_vel;

nav2_frenet_ilqr_controller/src/parameter_handler.cpp

@@ -78,8 +78,9 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".ilqr_trajectory_tracker.iteration_number", rclcpp::ParameterValue(20));
 
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".ilqr_trajectory_tracker.alpha", rclcpp::ParameterValue(1.0));
-
+    node, plugin_name_ + ".robot_type", rclcpp::ParameterValue("diff_drive"));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".wheelbase", rclcpp::ParameterValue(2.5));
 
   node->get_parameter(
     plugin_name_ + ".interpolate_curvature_after_goal",
@@ -122,7 +123,9 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".ilqr_trajectory_tracker.iteration_number",
     params_.iteration_number);
-  node->get_parameter(plugin_name_ + ".ilqr_trajectory_tracker.alpha", params_.alpha);
+
+  node->get_parameter(plugin_name_ + ".robot_type", params_.robot_type);
+  node->get_parameter(plugin_name_ + ".wheelbase", params_.wheelbase);
 
 
   dynamic_params_handler_ = node->add_on_set_parameters_callback(
@@ -184,8 +187,10 @@ ParameterHandler::dynamicParametersCallback(
       } else {
         params_.iteration_number = parameter.as_int();
       }
-    } else if (name == plugin_name_ + ".ilqr_trajectory_tracker.alpha") {
-      params_.alpha = parameter.as_double();
+    } else if (name == plugin_name_ + ".robot_type") {
+      params_.robot_type = parameter.as_string();
+    } else if (name == plugin_name_ + ".wheelbase") {
+      params_.wheelbase = parameter.as_double();
     }
   }