update: MPC local planner

README.md

@@ -42,7 +42,29 @@ This repository provides the implementation of common **Motion Planning** algori
     sudo apt install git
     ```
 
-3. Other dependence.
+3. Install dependence
+
+    - OSQP
+    ```bash
+    git clone -b release-0.6.3 --recursive https://github.com/oxfordcontrol/osqp
+    cd osqp && mkdir build && cd build
+    cmake .. -DBUILD_SHARED_LIBS=ON
+    make -j6
+    sudo make install
+    sudo cp /usr/local/include/osqp/* /usr/local/include
+    ```
+
+    - OSQP-Eigen
+
+    ```bash
+    git clone https://github.com/robotology/osqp-eigen.git
+    cd osqp-eigen && mkdir build && cd build
+    cmake ..
+    make
+    sudo make install
+    ```
+
+    - Other dependence.
     ```bash
     sudo apt install python-is-python3 \
     ros-noetic-amcl \
@@ -180,7 +202,7 @@ For the efficient operation of the motion planning system, we provide a series o
 |   **APF**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/apf_planner/src/apf_planner.cpp)     | ![apf_ros.gif](assets/apf_ros.gif)|[Real-time obstacle avoidance for manipulators and mobile robots](https://ieeexplore.ieee.org/document/1087247)
 |   **RPP**   |     [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/rpp_planner/src/rpp_planner.cpp)     | ![rpp_ros.gif](assets/rpp_ros.gif)|[Regulated Pure Pursuit for Robot Path Tracking](https://arxiv.org/pdf/2305.20026.pdf)
 |   **TEB**   |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
-|   **MPC**   |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
+|   **MPC**   |                                                              [![Status](https://img.shields.io/badge/done-v1.0-brightgreen)](https://github.com/ai-winter/ros_motion_planning/blob/master/src/core/local_planner/mpc_planner/src/mpc_planner.cpp)     | ![mpc_ros.gif](assets/mpc_ros.gif) | -
 | **Lattice** |                                                                ![Status](https://img.shields.io/badge/develop-v1.0-red)                                                                 | ![Status](https://img.shields.io/badge/gif-none-yellow) |
 
 

src/core/local_planner/local_planner/src/local_planner.cpp

@@ -163,7 +163,7 @@ bool LocalPlanner::shouldRotateToGoal(const geometry_msgs::PoseStamped& cur, con
  */
 bool LocalPlanner::shouldRotateToPath(double angle_to_path, double tolerance)
 {
-  return (tolerance && (angle_to_path > tolerance)) || (angle_to_path > rotate_tol_);
+  return (tolerance && (angle_to_path > tolerance)) || (!tolerance && (angle_to_path > rotate_tol_));
 }
 
 /**
@@ -322,6 +322,7 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
     pprev_pose_it = std::prev(prev_pose_it);
     pt.point.x = goal_pose_it->pose.position.x;
     pt.point.y = goal_pose_it->pose.position.y;
+    kappa = 0;
   }
   else
   {
@@ -345,6 +346,23 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
 
     pt.point.x = i_points[0].first + rx;
     pt.point.y = i_points[0].second + ry;
+
+    double ax = pprev_pose_it->pose.position.x;
+    double ay = pprev_pose_it->pose.position.y;
+    double bx = prev_pose_it->pose.position.x;
+    double by = prev_pose_it->pose.position.y;
+    double cx = goal_pose_it->pose.position.x;
+    double cy = goal_pose_it->pose.position.y;
+
+    double a = std::hypot(bx - cx, by - cy);
+    double b = std::hypot(cx - ax, cy - ay);
+    double c = std::hypot(ax - bx, ay - by);
+
+    double cosB = (a * a + c * c - b * b) / (2 * a * c);
+    double sinB = std::sin(std::acos(cosB));
+
+    double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
+    kappa = std::copysign(2 * sinB / b, cross);
   }
 
   pt.header.frame_id = goal_pose_it->header.frame_id;
@@ -353,22 +371,6 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
   theta = atan2(goal_pose_it->pose.position.y - prev_pose_it->pose.position.y,
                 goal_pose_it->pose.position.x - prev_pose_it->pose.position.x);
 
-  double ax = pprev_pose_it->pose.position.x;
-  double ay = pprev_pose_it->pose.position.y;
-  double bx = prev_pose_it->pose.position.x;
-  double by = prev_pose_it->pose.position.y;
-  double cx = goal_pose_it->pose.position.x;
-  double cy = goal_pose_it->pose.position.y;
-
-  double a = std::hypot(bx - cx, by - cy);
-  double b = std::hypot(cx - ax, cy - ay);
-  double c = std::hypot(ax - bx, ay - by);
-
-  double cosB = (a * a + c * c - b * b) / (2 * a * c);
-  double sinB = std::sin(std::acos(cosB));
-
-  double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
-  kappa = std::copysign(2 * sinB / b, cross);
 }
 
 }  // namespace local_planner

src/core/local_planner/lqr_planner/include/lqr_planner.h

@@ -84,18 +84,16 @@ class LQRPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanne
   Eigen::Vector2d _lqrControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r);
 
 private:
-  bool initialized_;                       // initialized flag
-  bool goal_reached_;                      // goal reached flag
-  tf2_ros::Buffer* tf_;                    // transform buffer
-  // costmap_2d::Costmap2DROS* costmap_ros_;  // costmap(ROS wrapper)
+  bool initialized_;     // initialized flag
+  bool goal_reached_;    // goal reached flag
+  tf2_ros::Buffer* tf_;  // transform buffer
 
   double d_t_;         // control time interval
   Eigen::Matrix3d Q_;  // state error matrix
   Eigen::Matrix2d R_;  // control error matrix
   int max_iter_;       // maximum iteration for ricatti solution
   double eps_iter_;    // iteration ending threshold
 
-  // std::vector<geometry_msgs::PoseStamped> global_plan_;
   ros::Publisher target_pt_pub_, current_pose_pub_;
 
   // goal parameters

src/core/local_planner/lqr_planner/src/lqr_planner.cpp

@@ -23,22 +23,22 @@ PLUGINLIB_EXPORT_CLASS(lqr_planner::LQRPlanner, nav_core::BaseLocalPlanner)
 namespace lqr_planner
 {
 /**
- * @brief Construct a new RPP planner object
+ * @brief Construct a new LQR planner object
  */
 LQRPlanner::LQRPlanner() : initialized_(false), goal_reached_(false), tf_(nullptr)  //, costmap_ros_(nullptr)
 {
 }
 
 /**
- * @brief Construct a new RPP planner object
+ * @brief Construct a new LQR planner object
  */
 LQRPlanner::LQRPlanner(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) : LQRPlanner()
 {
   initialize(name, tf, costmap_ros);
 }
 
 /**
- * @brief Destroy the RPP planner object
+ * @brief Destroy the LQR planner object
  */
 LQRPlanner::~LQRPlanner()
 {
@@ -201,10 +201,8 @@ bool LQRPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 
   // get the particular point on the path at the lookahead distance
   geometry_msgs::PointStamped lookahead_pt;
-  double theta_dir, theta_trj, kappa;
+  double theta_trj, kappa;
   getLookAheadPoint(L, robot_pose_map, prune_plan, lookahead_pt, theta_trj, kappa);
-  theta_dir = atan2(lookahead_pt.point.y - robot_pose_map.pose.position.y,
-                    lookahead_pt.point.x - robot_pose_map.pose.position.x);
 
   // current angle
   double theta = tf2::getYaw(robot_pose_map.pose.orientation);  // [-pi, pi]
@@ -232,7 +230,7 @@ bool LQRPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
   {
     Eigen::Vector3d s(robot_pose_map.pose.position.x, robot_pose_map.pose.position.y, theta);  // current state
     Eigen::Vector3d s_d(lookahead_pt.point.x, lookahead_pt.point.y, theta_trj);                // desired state
-    Eigen::Vector2d u_r(vt, vt * kappa);                                                     // refered input
+    Eigen::Vector2d u_r(vt, vt * kappa);                                                       // refered input
     Eigen::Vector2d u = _lqrControl(s, s_d, u_r);
 
     cmd_vel.linear.x = linearRegularization(base_odom, u[0]);
@@ -287,6 +285,7 @@ Eigen::Vector2d LQRPlanner::_lqrControl(Eigen::Vector3d s, Eigen::Vector3d s_d,
   Eigen::MatrixXd K = -(R_ + B.transpose() * P_ * B).inverse() * B.transpose() * P_ * A;
 
   u = u_r + K * e;
+
   return u;
 }
 

src/core/local_planner/mpc_planner/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mpc_planner)
+
+find_package(catkin REQUIRED COMPONENTS
+  angles
+  costmap_2d
+  geometry_msgs
+  nav_core
+  nav_msgs
+  navfn
+  pluginlib
+  roscpp
+  tf2_geometry_msgs
+  tf2_ros
+  base_local_planner
+  local_planner
+)
+
+find_package(OsqpEigen REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS local_planner
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME}
+  src/mpc_planner.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  OsqpEigen
+)

src/core/local_planner/mpc_planner/include/mpc_planner.h

@@ -0,0 +1,112 @@
+/**
+ * *********************************************************
+ *
+ * @file: mpc_planner.h
+ * @brief: Contains the model predicted control (MPC) local planner class
+ * @author: Yang Haodong
+ * @date: 2024-01-31
+ * @version: 1.0
+ *
+ * Copyright (c) 2024, Yang Haodong.
+ * All rights reserved.
+ *
+ * --------------------------------------------------------
+ *
+ * ********************************************************
+ */
+#ifndef MPC_PLANNER_H
+#define MPC_PLANNER_H
+
+#include <geometry_msgs/PointStamped.h>
+#include <tf2/utils.h>
+
+#include "local_planner.h"
+
+namespace mpc_planner
+{
+/**
+ * @brief A class implementing a local planner using the MPC
+ */
+class MPCPlanner : public nav_core::BaseLocalPlanner, local_planner::LocalPlanner
+{
+public:
+  /**
+   * @brief Construct a new MPC planner object
+   */
+  MPCPlanner();
+
+  /**
+   * @brief Construct a new MPC planner object
+   */
+  MPCPlanner(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
+
+  /**
+   * @brief Destroy the MPC planner object
+   */
+  ~MPCPlanner();
+
+  /**
+   * @brief Initialization of the local planner
+   * @param name        the name to give this instance of the trajectory planner
+   * @param tf          a pointer to a transform listener
+   * @param costmap_ros the cost map to use for assigning costs to trajectories
+   */
+  void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
+
+  /**
+   * @brief Set the plan that the controller is following
+   * @param orig_global_plan the plan to pass to the controller
+   * @return true if the plan was updated successfully, else false
+   */
+  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
+
+  /**
+   * @brief Check if the goal pose has been achieved
+   * @return true if achieved, false otherwise
+   */
+  bool isGoalReached();
+
+  /**
+   * @brief Given the current position, orientation, and velocity of the robot, compute the velocity commands
+   * @param cmd_vel will be filled with the velocity command to be passed to the robot base
+   * @return true if a valid trajectory was found, else false
+   */
+  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
+
+private:
+  /**
+   * @brief Execute MPC control process
+   * @param s     current state
+   * @param s_d   desired state
+   * @param u_r   refered control
+   * @param du_p  previous control error
+   * @return u  control vector
+   */
+  Eigen::Vector2d _mpcControl(Eigen::Vector3d s, Eigen::Vector3d s_d, Eigen::Vector2d u_r, Eigen::Vector2d du_p);
+  /**
+   * @brief convert matrix A to its sparse view
+   * @param A     dense matrix
+   * @return A_s  sparse matrix
+   */
+  Eigen::SparseMatrix<double> _convertToSparseMatrix(Eigen::MatrixXd A);
+
+private:
+  bool initialized_;     // initialized flag
+  bool goal_reached_;    // goal reached flag
+  tf2_ros::Buffer* tf_;  // transform buffer
+
+  double d_t_;            // control time interval
+  Eigen::Matrix3d Q_;     // state error matrix
+  Eigen::Matrix2d R_;     // control error matrix
+  int p_;                 // predicting time domain
+  int m_;                 // control time domain
+  Eigen::Vector2d du_p_;  // previous control error
+
+  ros::Publisher target_pt_pub_, current_pose_pub_;
+
+  // goal parameters
+  double goal_x_, goal_y_;
+  Eigen::Vector3d goal_rpy_;
+};
+}  // namespace mpc_planner
+#endif

src/core/local_planner/mpc_planner/mpc_planner_plugin.xml

@@ -0,0 +1,8 @@
+<library path="lib/libmpc_planner">
+    <class name="mpc_planner/MPCPlanner" type="mpc_planner::MPCPlanner"
+        base_class_type="nav_core::BaseLocalPlanner">
+        <description>
+            A implementation of a local model predicted control (MPC) planner.
+        </description>
+    </class>
+</library>

src/core/local_planner/mpc_planner/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mpc_planner</name>
+  <version>0.0.0</version>
+  <description>The mpc_planner package</description>
+  <maintainer email="[email protected]">winter</maintainer>
+  <license>GPL3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>angles</depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>navfn</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>local_planner</depend>
+
+
+  <export>
+    <nav_core plugin="${prefix}/mpc_planner_plugin.xml" />
+
+  </export>
+</package>