Fixed the bug of assigning ilqr optimization start state (#39)

Author: CihatAltiparmak

ilqr_trajectory_tracker/demos/ilqr_optimizer_demo.cpp

@@ -56,7 +56,9 @@ int main(int argc, char ** argv)
   newton_optimizer.setAlpha(alpha);
 
   const auto start{std::chrono::steady_clock::now()};
-  auto u_optimal = newton_optimizer.optimize(x_feasible, Q, R, dt);
+  // 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);
   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

@@ -51,11 +51,13 @@ class NewtonOptimizer : public Optimizer
     const MatrixXd & P);
   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);
   double cost(
@@ -122,6 +124,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)
@@ -133,7 +136,7 @@ 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];
+  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)
@@ -165,6 +168,7 @@ 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)
 {
@@ -183,7 +187,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;

nav2_frenet_ilqr_controller/include/nav2_frenet_ilqr_controller/frenet_ilqr_controller.hpp

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

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 & c_state_robot,
   const frenet_trajectory_planner::CartesianTrajectory & robot_cartesian_trajectory)
 {
 
@@ -230,7 +230,7 @@ Vector2d FrenetILQRController::findOptimalInputForTrajectory(
   ilqr_trajectory_tracker::NewtonOptimizer<DiffDriveRobotModel> newton_optimizer;
   newton_optimizer.setIterationNumber(20);
   newton_optimizer.setAlpha(alpha);
-  auto U_optimal = newton_optimizer.optimize(X_feasible, Q, R, dt);
+  auto U_optimal = newton_optimizer.optimize(c_state_robot, X_feasible, Q, R, dt);
 
   if (U_optimal.empty()) {
     throw nav2_core::NoValidControl("Iterative LQR couldn't find any solution!");
@@ -301,13 +301,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;