This repository implements Model Predictive Control (MPC) and a Disturbance Observer (DOB) for a 6-DOF manipulator (Piper Robotic Arm). It is designed to validate control algorithms within the ROS2 Humble and Gazebo simulation environments.
Rigid body dynamics are calculated using the Pinocchio library, and the MPC optimization problem is solved using CasADi (supporting both SQP and IPOPT solvers). This project serves as a baseline implementation for researchers and developers aiming to test custom MPC and DOB algorithms.
- OS: Ubuntu 22.04 (Jammy Jellyfish)
- ROS Version: ROS2 Humble
- Simulator: Gazebo Classic
Ensure that your ROS2 environment is sourced before running the following commands.
sudo apt update
sudo apt install ros-humble-gazebo-ros2-control ros-humble-gazebo-ros2-control-demos
sudo apt install ros-humble-ros2-control ros-humble-controller-manager
sudo apt install ros-humble-gazebo-ros ros-humble-joint-state-publisher
sudo apt install ros-humble-gazebo-ros-pkgs
This project requires Pinocchio for dynamics and CasADi for optimization.
Install Pinocchio via robotpkg:
sudo apt install -qqy lsb-release curl
sudo mkdir -p /etc/apt/keyrings
curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \
| sudo tee /etc/apt/keyrings/robotpkg.asc
echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \
| sudo tee /etc/apt/sources.list.d/robotpkg.list
sudo apt update
sudo apt install -qqy robotpkg-py3*-pinocchio
Install CasADi:
pip install casadi
Open a terminal and launch the Gazebo simulation environment.
ros2 launch piper_gazebo piper_gazebo.launch.py
Open a new terminal and run the MPC node. You can choose between the SQP or IPOPT solver.
To run with the SQP solver:
ros2 run piper_controller run_piper_mpc_sqp
To run with the IPOPT solver:
ros2 run piper_controller run_piper_mpc_ipopt
The controller supports four distinct modes to evaluate performance. You can switch modes dynamically using keyboard input in the terminal where the controller node is running.
s(Stance Mode): The robot maintains its initial pose using a PD controller. This is the default state upon startup.t(Tracking Mode): The robot follows a predefined trajectory using Model Predictive Control (MPC). No external disturbance is applied.d(Disturbance Injection): While tracking the trajectory, external disturbance torques are injected into specific joints to simulate environmental forces or modeling errors.c(Compensation Mode): The Disturbance Observer (DOB) estimates the injected disturbance and adds a compensation torque to the control input, mitigating the tracking error caused by the disturbance.
The internal parameters for the MPC (horizon, weights, constraints) and the DOB (bandwidth, gains) can be tuned directly in the source code. The codebase is structured to provide a clear and standard implementation, making it suitable for further development or integration of custom control strategies.