# QP-based COG Trajectory

**Author:** Teng-Hu Cheng
**email:** [e-mail]
**Date:** Feb. 2024
**Website:** https://ncrl.lab.nycu.edu.tw/member/

# 目錄
[toc]

## Introduction

Quadruped robots are ideal for navigating uneven terrain due to their stability and mobility. A key concept in ensuring their balance is the Zero Moment Point (ZMP), which represents the point on the ground where the net moment from inertia and gravity is zero. For a robot to be stable, its ZMP must lie within the support polygon formed by the feet in contact with the ground. This is closely related to the robot's Center of Gravity (CoG); if the dynamic equation relating ZMP and CoG is satisfied and the ZMP remains inside the support area, the robot’s stability is guaranteed.

四足機器人因其良好的穩定性與機動性，特別適合在崎嶇地形中移動。確保其平衡的一個關鍵概念是零力矩點（Zero Moment Point, ZMP），它是地面上一個使慣性與重力所產生的合力矩為零的位置。當 ZMP 落在由著地腳形成的支撐多邊形內，機器人即處於穩定狀態。ZMP 的位置與重心（Center of Gravity, CoG）密切相關，只要兩者滿足特定的動態關係，且 ZMP 維持在支撐範圍內，系統便能保證穩定。
## Scenario

Imagine a quadruped robot navigating through rubble in a disaster zone. As it moves using a trot gait, its control system continuously calculates the CoG and ZMP. When the robot lifts a leg or shifts direction, it checks whether the ZMP stays within the support polygon. If the ZMP approaches the boundary, adjustments are made to gait or posture to maintain balance. This ZMP-CoG control strategy enables the robot to move safely and stably through challenging environments.

想像一台四足機器人在災區瓦礫中行走。當它以快步走（trot）方式移動時，控制系統會即時計算重心與 ZMP 的位置。每當機器人抬腳或轉向時，系統都會檢查 ZMP 是否仍在支撐多邊形內；若 ZMP 靠近邊緣，便會調整步態或姿態來維持平衡。透過這種 ZMP–CoG 控制策略，機器人能穩定且安全地應對複雜地形。

## Optimization Methods
[Reference](https://drive.google.com/file/d/11TrrTHkW1JymTmfVZXFfO51e1c3Ox4g8/view?usp=share_link)
Relationship of ZMP and CoG for stability. Given the equation is satisfied, where $X_{ZMP}$ is the ZMP position and $X_{CoG}$ is the CoG position in the 2D floor, if the ZMP is in the support polygon, the robot is quaranteed to be stable.

{%youtube ezhuYL01U4A%}

The governing equation for CoG to ensure stability
$$
X_{ZMP}=X_{CoG}-\frac{h}{g}\ddot{X}_{CoG},\quad (1)
$$where
$X_{ZMP} \in \mathbb{R}^2$ is defined as $$
X_{CoG}=\begin{bmatrix} x_{CoG}\\ y_{CoG} \end{bmatrix}$$
### Method (QP-based optimization):
$$\min_{\xi} \quad \frac{1}{2} \xi^T \mathbf{Q} \xi + \mathbf{c}^T\xi$$
$$\text{s.t.} \quad \mathbf{A_z} \xi \leq \mathbf{B}, \quad \mathbf{D} \xi= \mathbf{f}\quad(2)$$
- Cost (minimize the angular acceleration over time):
$$\int_{t_k}^{t_{k+1}} \ddot{\mathbf{P}}_c^2(t) \, dt = \boldsymbol{\xi}_k^T \mathbf{Q}_k \boldsymbol{\xi}_k
$$

where
$$Q =
\begin{bmatrix}
\frac{400}{7} T^7 & 40T^6 & \frac{120}{5}T^5 & 10T^4 & 0 & 0 \\
40T^6 & \frac{144}{5}T^5 & 18T^4 & 8T^3 & 0 & 0 \\
\frac{120}{5}T^5 & 18T^4 & 12T^3 & 6T^2 & 0 & 0 \\
10T^4 & 8T^3 & 6T^2 & 4T & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0
\end{bmatrix}$$
## Detailed Derivation
Note that the center of gravity (CoG) trajectory is composed of multiple sub-trajectories to enhance flexibility. Each sub-trajectory is strongly coupled with adjacent ones in terms of position and velocity at their junctions. To ensure stability, each sub-trajectory must satisfy the constraint given in Equation (1), while also maintaining continuity between successive sub-trajectories as required by Equation (2).

- **Step 1: Define the COG trajectory**
The trajectory is defined as a fifth order polynomial (in the x direction):
$$x_i(t)=\sum_{j=0}^{n} \sigma_{ij}t^j \in \mathbb{R}, \quad t_{i-1}\leq t <t_i, \quad \text{for} \: i \in {1,2,...,m.}$$$$\sigma_{i}=[\sigma_{in},\sigma_{i(n-1)},...,\:\sigma_{i0}]^T$$where
$x_i(t)$: denotes the $i^{th}$ segment of the trajectory defined as $x_i = [x, y]^T$, where $i$ can be defined as the $i^\text{th}$ leg.
$i$: denotes the index of the segments (up to $m$).
$j$: denotes the order of the polynomial (up to $n$).
$\sigma_{ij}$: denotes the coefficient of the polynomial $x_i(t)$.
**Note that in this quadruped robot application, a fifth order ($n=5$) of polynomial function is chosen.**
The trajectory in the y direction can be defined as another fifth order polynomial:
$$y_i(t)=\sum_{j=0}^{n} \beta_{ij}t^j\in \mathbb{R}, \quad t_{i-1}\leq t <t_i, \quad \text{for} \: i \in {1,2,...,m.}$$$$\beta_{i}=[\beta_{in},\beta_{i(n-1)},...,\:\beta_{i0}]^T$$
Then the remaining subsequent procedure is the same.
- **Step 2: Find the ZMP trajectory based on the CoG trajectory**
Taking the second time-derivative of the $i^{th}$ CoG trajectory yields,
$$\ddot{x}_i(t) = 20\sigma_{i5} t^3 + 12\sigma_{i4} t^2 + 6\sigma_{i3} t + 2\sigma_{i2}$$
For stability: $$x_i(t) = x_{i,ZMP} + \frac{1}{\omega^2} \ddot{x_i}(t),\quad (1)$$$\omega = \sqrt{\frac{g}{h}}$ is the natural frequency of the CoG motion (height-dependent).
Equivalently, (1) can be written as
$$(\sigma_{i5} t^5 +\sigma_{i4} t^4 +\sigma_{i3} t^3 + \sigma_{i2} t^2 + \sigma_{i1} t + \sigma_{i0})=x_{ZMP}+\frac{1}{\omega^2} (20\sigma_{i5} t^3 + 12\sigma_{i4} t^2 + 6\sigma_{i3} t + 2\sigma_{i2})\quad (2),$$
which can be use to find the ZMP position.
$$x_{i,ZMP}=\sigma_{i5} t^5 +\sigma_{i4} t^4+\sigma_{i3} t^3 + \sigma_{i2} t^2 + \sigma_{i1} t + \sigma_{i0}-(\frac{h}{g}(20\sigma_{i5} t^3 + 12\sigma_{i4} t^2 + 6\sigma_{i3} t + 2\sigma_{i2})),$$which is equivalent to
$$x_{i,ZMP}=\sigma_{i5} t^5 +\sigma_{i4} t^4+(\sigma_{i3}-\frac{h}{g}20\sigma_{i5}) t^3 + (\sigma_{i2}-\frac{h}{g}12\sigma_{i4}) t^2 + (\sigma_{i1}-\frac{h}{g} 6\sigma_{i3} ) t + \sigma_{i0}-2\frac{h}{g}\sigma_{i2},$$which can be expressed in matrix form as
$$x_{i,ZMP}=[(t^5-\frac{h}{g}20t^3),(t^4-\frac{h}{g}12t^2),(t^3-\frac{h}{g} 6t), (t^2-2\frac{h}{g}), t, 1)]\sigma_{i}$$
Similarly, in the y direction, the ZMP can be expressed as:
$$y_{i,ZMP}=\beta_{i5} t^5 +\beta_{i4} t^4+(\beta_{i3}-\frac{h}{g}20\beta_{i5}) t^3 + (\beta_{i2}-\frac{h}{g}12\beta_{i4}) t^2 + (\beta_{i1}-\frac{h}{g} 6\beta_{i3} ) t + \beta_{i0}-2\frac{h}{g}\beta_{i2}, $$or in matrix form:
$$y_{i,ZMP}=[(t^5-\frac{h}{g}20t^3),(t^4-\frac{h}{g}12t^2),(t^3-\frac{h}{g} 6t), (t^2-2\frac{h}{g}), t, 1)]\beta_{i},$$
where $\sigma_{i}$ and $\beta_{i}$ are defined in step 1.
- **Step 3: Continuity at the junction of two CoG trajectories**
1. Position Continuity ($t_i$ is **absolute time**):
$$x_i(t_i) = x_{i+1}(t_i-T)$$ $$y_i(t_i) = y_{i+1}(t_i-T)$$Ensures the CoG does not "jump," where $t_i$ denotes the time at the junction.
Given the leg swing sequence as FL->HR->FR->HL ($t_0$->$t_1$->$t_2$->$t_3$->$t_4$), and the duration are
$$T=t_1-t_0$$$$T=t_2-t_1$$$$T=t_3-t_2$$$$T=t_4-t_3.$$ $T_1-T_4$ are chosen based on the desired average velocity. For example, average velocity of CoG, $v_{c}=2*L_{step}/4T,$ where $L_{step}$ is the step length. During leg FL is in swing phase, the equality for continuity in the $x$ direction is:
$$x_i(t_i) = x_{i+1}(t_i-T)$$is equivalent to$$\sigma_{i5} t_i^5 +\sigma_{i4} t_i^4 +\sigma_{i3} t_i^3 + \sigma_{i2} t_i^2 + \sigma_{i1} t_i + \sigma_{i0}=\sigma_{(i+1)5} (t_i-T)^5 +\sigma_{(i+1)4} (t_i-T)^4 +\sigma_{(i+1)3} (t_i-T)^3 + \sigma_{(i+1)2} (t_i-T)^2 + \sigma_{(i+1)1} (t_i-T) + \sigma_{(i+1)0},$$ which can be expressed in matrix form as: $$[t_i^5, t_i^4, t_i^3, t_i^2, t_i ,1]\sigma_i-[(t_i-T)^5, (t_i-T)^4, (t_i-T)^3, (t_i-T)^2, (t_i-T), 1]\sigma_{i+1}=0$$
Similarly, the equality in the $y$ direction is
$$y_i(t_i) = y_{i+1}(t_i-T)$$ is equivalent to
$$\beta_{i5} t_i^5 +\beta_{i4} t_i^4 +\beta_{i3} t_i^3 + \beta_{i2} t_i^2 + \beta_{i1} t_i + \beta_{i0}=\beta_{(i+1)5} (t_i-T)^5 +\beta_{(i+1)4} (t_i-T)^4 +\beta_{(i+1)3} (t_i-T)^3 + \beta_{(i+1)2} (t_i-T)^2 + \beta_{(i+1)1} (t_i-T) + \beta_{(i+1)0},$$ which can be expressed matrix form as
$$[t_i^5, t_i^4, t_i^3, t_i^2, t_i ,1]\beta_i-[(t_i-T)^5, (t_i-T)^4, (t_i-T)^3, (t_i-T)^2, (t_i-T), 1]\beta_{i+1}=0$$
2. Velocity Continuity:
$$\dot{x}_i(t_i) = \dot{x}_{i+1}(t_i-T)$$ $$\dot{y}_i(t_i) = \dot{y}_{i+1}(t_i-T)$$ Ensures the CoG moves at the same speed at the junction.
For velocity continuity in the $x$ and $y$ directions:$$5\sigma_{i5} t_i^4 +4\sigma_{i4} t_i^3 +3\sigma_{i3} t_i^2 + 2\sigma_{i2} t_i + \sigma_{i1}=5\sigma_{(i+1)5} (t_i-T)^4 +4\sigma_{(i+1)4} (t_i-T)^3 +3\sigma_{(i+1)3} (t_i-T)^2 + 2\sigma_{(i+1)2} (t_i-T) + \sigma_{(i+1)1},$$ or in matrix form:
$$[5 t_i^4, 4 t_i^3, 3 t_i^2, 2 t_i, 1,0]\sigma_i-[5 (t_i-T)^4, 4 (t_i-T)^3, 3 (t_i-T)^2, 2 (t_i-T), 1, 0]\sigma_{i+1}=0,$$
$$5\beta_{i5} t_i^4 +4\beta_{i4} t_i^3 +3\beta_{i3} t_i^2 + 2\beta_{i2} t_i + \beta_{i1}=5\beta_{(i+1)5} (t_i-T)^4 +4\beta_{(i+1)4} (t_i-T)^3 +3\beta_{(i+1)3} (t_i-T)^2 + 2\beta_{(i+1)2} (t_i-T) + \beta_{(i+1)1},$$or in matrix form:
$$[5 t_i^4, 4 t_i^3, 3 t_i^2, 2 t_i, 1,0]\beta_i-[5 (t_i-T)^4, 4 (t_i-T)^3, 3 (t_i-T)^2, 2 (t_i-T), 1, 0]\beta_{i+1}=0,$$
~~3. Acceleration Continuity~~

- **Step 4: Let the ZMP to be in the Support Polygon**
Given the positions of the three stance legs, $^{I}r_{F,1}$, $^{I}r_{F,3}$, and $^{I}r_{F,4}$, the enclosed interior of the three lines (represented by the red triangle below) can be expressed as the set of points satisfying the three inequalities.

**Objective**: The inequality in the MATLAB function **"quadprog"** is expressed as $Ax \leq B.$


To this end, let's start with a line passing through any two points $(x_1, y_1)$ and $(x_2, y_2)$, and it gives the equation:$$(y - y_1) = m (x - x_1)$$ where the slope $m$ is:
$$m = \frac{y_2 - y_1}{x_2 - x_1}$$Rearrange into the standard form:
$$ax + by + c = 0$$where:
$$a = -(y_2 - y_1)$$$$b = (x_2 - x_1)$$$$c = -ax_1 - by_1$$The objective is to find the inequalities expressed as
Let's consider each line formulation given two scenarios according the sign of $(ax + by + c)$ given the centroid of the enclosed area at ($x_c,y_c$):
- If **sign$(ax + by + c)>0$**
Therefore, $ax + by + c>0$, which implies $ax + by>-c,$ which can be further expressed as $$-(ax + by)<c.$$
- Elseif **sign$(ax + by + c)<0$**
Therefore, $ax + by + c<0$, which implies $$ax + by<-c$$

Finally, any point that satisfies the inequalities is guaranteed to be within the interior region, which corresponds to the ZMP location we need to determine. Therefore, we must ensure that **the $x_{i,ZMP}$ and $y_{i,ZMP}$ computed in Step 2** satisfy all the three inequalities. Specifically:
$$A[x_{i,ZMP};y_{i,ZMP}] ≤ B \quad (3)$$where
$$A=[a_1 \quad b_1;a_2 \quad b_2;a_3 \quad b_3]\in \mathbb{R}^{3 \times 2}$$and$$B=[-c_1;\quad -c_2;\quad -c_3] \in \mathbb{R}^{3 \times 1}.$$Check the Appendix for more details.
By substituting the ZMP from step 2 into (3), we can extend (3) to the following ZMP inequality constraint inequality.$$A_z[\sigma_i;\beta_i]^T \leq B,$$where$$A_z=A\otimes t_z \in \mathbb{R}^{3 \times 12}$$$$[\sigma_i;\beta_i]^T \in \mathbb{R}^{12 \times 1}$$where $\otimes$ denotes the [Kronecker product](https://zh.wikipedia.org/zh-tw/克罗内克积)
$$t_z=[(t^5-\frac{h}{g}20t^3),(t^4-\frac{h}{g}12t^2),(t^3-\frac{h}{g} 6t), (t^2-2\frac{h}{g}), t, 1)]\in \mathbb{R}^{1 \times 6}.$$
- **Step 5: Solve for the QP constrained problem**
$$\min_{\xi} \quad \frac{1}{2} \xi^T \mathbf{Q} \xi + \mathbf{c}^T\xi$$
$$\text{s.t.} \quad \mathbf{A_z} \xi \leq \mathbf{B}, \quad \mathbf{D} \xi= \mathbf{f}$$
where $\sigma$ is the unknown polynomial vector to be computed and is defined as the stacked form of all the $\sigma_{ij},$ where$$Q = \begin{bmatrix}
Q_1 & \cdots & 0 \\
\vdots & \ddots & \vdots \\
0 & \cdots & Q_m
\end{bmatrix}$$$$\xi = \begin{bmatrix} \sigma_1^T, & \beta_1^T, & \sigma_2^T, & \beta_2^T, \cdots, & \sigma_m^T, & \beta_m^T \end{bmatrix}^T,$$$$\sigma_i = \begin{bmatrix} \sigma_{i1} & \cdots & \sigma_{in} \end{bmatrix}^T, \quad \beta_i = \begin{bmatrix} \beta_{i1} & \cdots & \beta_{in} \end{bmatrix}^T$$where $m$ is the number of CoG trajectory segments.
- **Step 6: Find the relative position vector from the COG to the foot of the stance leg**
To find the angular velocity for each leg, the relative position from the hip to the foot of the stance leg needs to be found. However, only the CoG trajectory is known after body trajectory planning. But fortunately, the relative position from the CoG to the hip joint is known, $^{I}r_{C,H}$. Therefore, to find the relative position $^{I}r_{HF}$, one can follow the derivation below.
$$^{I}r_{HF}=^{I}r_{F}-^{I}r_{H}$$$$^{I}r_{HF}=^{I}r_{F}-(^{I}r_{CoG}+^{I}r_{C,H})$$
where $^{I}r_{C,H}=^{I}R_{B}^{H}r_{H}-^{H}r_{CoG}$ denotes the relative position from the CoG to the hip position, and $^{H}r_{H}$ and $^{H}r_{CoG}$ are defined in the hip frame (same as body frame).
Since $^{I}r_{HF}$ is defined in the inertial frame, so it will then be transformed to hip frame to find the joint velocity since the IK is operated in the hip frame.
- **Step 7: Find the joint velocity by IK**
1. Convert the relaive position inertial fram to hip frame since the task space is defined in hip frame in solving Invere Kinematics.
$$^{H}r_{HF}=^{H}R_{I}* ^{I}r_{HF},$$
2. Solve $^{H}r_{HF}$ for the joint angles using IK.

---
# Appendix
Here’s a **MATLAB code** that give three point (2D) to find the set of positions ($x,y$) in the enclosed area:
$$a_1x + b_1y \leq -c_1$$$$a_2x + b_2y \leq -c_2$$$$a_3x + b_3y \leq -c_3.$$
- Objectives: The inequality in Matlab function "quadprog" is expressed as $Ax ≤ B$.
Let's consider each line formulation given two scenarios according the sign of $(ax + by + c)$ given the centroid of the enclosed area at ($x_c,y_c$):
- If **sign$(ax + by + c)>0$**
Therefore, $ax + by + c>0$, which implies $ax + by>-c,$ which can be further expressed as $$-(ax + by)<c.$$
- Elseif **sign$(ax + by + b)<0$**
Therefore, $ax + by + c<0$, which implies $$ax + by<-c$$

function [A, b] = compute_inequalities(triangle_points)
    % triangle_points: 3x2 matrix, each row is an (x, y) point
    % A: 3x2 matrix (Ax + By)
    % b: 3x1 vector (C)
    
    % Function to compute line equation in standard form Ax + By + C = 0
    function L = compute_line(p1, p2)
        if p1(1) == p2(1)  % Vertical line case
            A = 1;  % x = constant
            B = 0;
            C = -p1(1);
        else  % Regular case
            A = -(p2(2) - p1(2));
            B = (p2(1) - p1(1));
            C = - (A * p1(1) + B * p1(2));
        end
        L = [A, B, C];
    end

    % Compute the three lines defining the triangle
    L1 = compute_line(triangle_points(1,:), triangle_points(2,:)); % P1-P2
    L2 = compute_line(triangle_points(2,:), triangle_points(3,:)); % P2-P3
    L3 = compute_line(triangle_points(3,:), triangle_points(1,:)); % P3-P1

    % Compute the centroid of the triangle
    centroid = mean(triangle_points, 1);

    % Determine the correct inequality sign based on the centroid position
    sign_L=zeros(3,1);
    sign_L(1,:) = sign(L1(1) * centroid(1) + L1(2) * centroid(2) + L1(3));%sign_L1 
    sign_L(2,:) = sign(L2(1) * centroid(1) + L2(2) * centroid(2) + L2(3));%sign_L2 
    sign_L(3,:) = sign(L3(1) * centroid(1) + L3(2) * centroid(2) + L3(3));%sign_L3 
    A = [L1(1:2); L2(1:2); L3(1:2)];
    b = [L1(3); L2(3); L3(3)];

    for i=1:length(sign_L)
        if sign_L(i)>0
            A(i,:)=-A(i,:);
        else 
            b(i,:)=-b(i,:);
        end
    end
end