This repository contains an Adaptive Model Predictive Control (Adaptive MPC) approach designed specifically for quadrupedal robots operating under periodic disturbances.

First install raisim_ros_wrapper repository, which provides a ROS-based simulation environment using the RaiSim physics engine:

cd <path_to_ws>/catkin_ws
mkdir src
cd src
git clone https://gitlab.com/rl-unitree-a1/raisim_ros_wrapper.git
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

Then, clone this repository to <path_to_ws>/src:

git clone https://github.com/LizaP9/Periodic_Adaptive_MPC.git

Build and source workspace (Use the --jobs 2 flag if your CPU resources are limited to help avoid build errors.):

catkin build --jobs 2
source devel/setup.bash

In first terminal launch the visualizer. You can choose one of the following (opengl is easier for visualization):

# unity
roslaunch raisim unity.launch
# opengl
roslaunch raisim opengl.launch

In second terminal launch the Raisim server:

roslaunch raisim_unitree_ros_driver spawn.launch scene:=2

Arguments:

• scene - - indicates which objects will be created in the simulator scene.

In third terminal launch the controller for the simulator with the universal launch file:

roslaunch be2r_cmpc_unitree unitree_a1.launch sim:=true rviz:=false rqt_reconfigure:=true

Arguments:

• sim - (bool) whether running in simulation or on a real robot;
• rviz - (bool) whether to start RViz;
• rqt_reconfigure - (bool) whether to start rqt.

Launch the controller to control the real robot (separate launch file):

roslaunch be2r_cmpc_unitree unitree_real.launch

Launch the controller for the simulator with the universal launch file:

roslaunch be2r_cmpc_unitree unitree_a1.launch sim:=false rviz:=false rqt_reconfigure:=false

Arguments:

• sim - (bool) whether running in simulation or on a real robot;
• rviz - (bool) whether to start RViz;
• rqt_reconfigure - (bool) whether to start rqt.

You can exit from a gait state to the Passive state in two ways:

1. From any state directly. The motors switch to damping mode, and the robot smoothly goes to the ground.
2. Switch first to Balance_Stand, then Lay_Down, and finally Passive.

graph TD;
  Passive-->Stand_Up;
  Stand_Up-->Lay_Down;
  Lay_Down-->Stand_Up;
  Stand_Up-->Balance_Stand;
  Stand_Up-->MPC_Locomotion;
  Stand_Up-->Vision_Locomotion;
  Vision_Locomotion-->Balance_Stand;
  Balance_Stand-->Vision_Locomotion;
  Balance_Stand-->MPC_Locomotion;
  Balance_Stand-->Lay_Down;
  MPC_Locomotion-->Balance_Stand;

Control system architecture:

The discrete-time dynamics of a quadrupedal robot under periodic disturbances are represented as:

where:

where:

• $\theta$: Orientation of the robot's base
• $T(\theta)$: Transformation matrix from Euler angles to angular velocities
• $\Delta t$: Discrete time step
• $I$: Inertia matrix of the robot
• $[r_i]_{\times}$: Cross-product matrix associated with vector $r_i$
• $m$: Mass of the robot
• $g$: Gravitational acceleration

The external periodic disturbances are estimated through solving the quadratic optimization problem:

where:

• $\ x_{k}^{\text{real}}, x_{k+1}^{\text{real}} $: Real observed system states at steps (k) and (k+1)
• $\ A_d, B_d, Q_d, G_d $: Discrete-time system matrices
• $\ u_k $: Control inputs (ground reaction forces)
• $\ \xi $: Vector of unknown external disturbances to estimate
• $\ S \in \mathbb{R}^{6\times6} $: Weighting matrix penalizing the estimation errors

To add or modify an external disturbance (force) in the Raisim simulation environment, follow these steps:

1. Modify the Force Application Function

Edit the applyExternalForce() function located in:

raisim_unitree_ros_driver.cpp

This function controls how external forces are applied to the simulated robot.

2. Adjust External Force Parameters

Update the external force parameters defined in:

raisim_unitree_ros_driver.hpp

Here, you can set the magnitude and frequency of the disturbances applied to the robot.

Experimental simulations on a Unitree A1 quadrupedal platform demonstrate that Adaptive MPC with periodic disturbance estimation outperforms both baseline MPC and static disturbance compensation, achieving superior tracking accuracy across various disturbance scenarios.