For an n-DOF rigid robot manipulator in joint coordinates \( \mathbf{q} \in \mathbb{R}^n \), you have already seen the standard rigid-body dynamics:

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} \]

where:

• \( \mathbf{M}(\mathbf{q}) \) is the symmetric, positive-definite inertia matrix.
• \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis and centripetal torques.
• \( \mathbf{g}(\mathbf{q}) \) is the gravity torque vector.
• \( \boldsymbol{\tau} \) is the actuator torque vector in joint space.

A key fact from robot dynamics is that the gravity vector is the gradient of the potential energy \( U(\mathbf{q}) \):

\[ U(\mathbf{q}) = \sum_{i=1}^n m_i g h_i(\mathbf{q}), \qquad \mathbf{g}(\mathbf{q}) = \frac{\partial U(\mathbf{q})}{\partial \mathbf{q}} \]

where \( h_i(\mathbf{q}) \) is the height of link center of mass \( i \) in the gravity field. Thus \( \mathbf{g}(\mathbf{q}) \) is fully determined by the robot geometry, masses, and the direction/magnitude of gravity and is independent of velocities or accelerations.

flowchart TD
  Q["Joint position q"] --> U["Compute potential energy U(q)"]
  U --> GRAD["Differentiate U wrt q"]
  GRAD --> G["Gravity torque vector g(q)"]
  G --> TAU["Required static torque tau = g(q)"]
        

Intuitively, if you want to hold the robot still at a configuration \( \mathbf{q}^\star \) against gravity, you must apply exactly the joint torques \( \boldsymbol{\tau} = \mathbf{g}(\mathbf{q}^\star) \). Gravity compensation is about automating this idea within a feedback controller.

Consider holding the robot at a constant desired configuration \( \mathbf{q}_d \) with zero velocity and acceleration: \( \dot{\mathbf{q}} = \ddot{\mathbf{q}} = \mathbf{0} \). Plugging this into the dynamics:

\[ \mathbf{M}(\mathbf{q}_d)\cdot \mathbf{0} + \mathbf{C}(\mathbf{q}_d,\mathbf{0})\cdot \mathbf{0} + \mathbf{g}(\mathbf{q}_d) = \boldsymbol{\tau}_{\text{equil}} \]

so the equilibrium torque is

\[ \boldsymbol{\tau}_{\text{equil}} = \mathbf{g}(\mathbf{q}_d). \]

If the controller were purely feedforward and simply set \( \boldsymbol{\tau} = \mathbf{g}(\mathbf{q}_d) \), then the configuration \( \mathbf{q} = \mathbf{q}_d \), \( \dot{\mathbf{q}} = \mathbf{0} \) is an equilibrium. However, it is open-loop: any disturbance or modeling error causes drift. Hence we combine gravity compensation with feedback.

A useful property of robot dynamics that we will need later for stability analysis is the skew-symmetry of \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \):

\[ \mathbf{x}^\top \big( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \big)\mathbf{x} = 0 \quad \forall \mathbf{x}\in\mathbb{R}^n. \]

This structural property is what will allow a simple Lyapunov proof for stability of PD control with gravity compensation.

From Lesson 2 you know the basic joint-space PD control (without gravity compensation):

\[ \boldsymbol{\tau} = \mathbf{K}_p (\mathbf{q}_d - \mathbf{q}) + \mathbf{K}_d (\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) \]

where \( \mathbf{K}_p, \mathbf{K}_d \) are positive definite gain matrices. This works well when gravity is small or the robot operates near a fixed configuration. To obtain configuration-independent behavior for posture control, we add gravity compensation:

\[ \boldsymbol{\tau} = \mathbf{K}_p (\mathbf{q}_d - \mathbf{q}) + \mathbf{K}_d (\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) + \mathbf{g}(\hat{\mathbf{q}}) \]

where \( \hat{\mathbf{q}} \) is the configuration used inside the gravitational model (typically the measured configuration, possibly filtered).

flowchart TD
  QD["Desired joints q_d, dq_d"] --> PD["PD block: Kp, Kd"]
  S["Sensors: q, dq"] --> PD
  S --> G["Gravity model: g(q_hat)"]
  PD --> SUM["Add gravity torque"]
  G --> SUM
  SUM --> ACT["Actuators apply tau"]
  ACT --> ROB["Robot joints"]
  ROB --> S
        

With a perfect gravity model, \( \hat{\mathbf{q}} = \mathbf{q} \) and \( \mathbf{g}(\hat{\mathbf{q}}) = \mathbf{g}(\mathbf{q}) \). For constant desired posture \( \mathbf{q}_d \) with \( \dot{\mathbf{q}}_d = \ddot{\mathbf{q}}_d = \mathbf{0} \), define the tracking errors \( \mathbf{e} = \mathbf{q}_d - \mathbf{q} \), \( \dot{\mathbf{e}} = -\dot{\mathbf{q}} \). Then substituting the control law into the dynamics, after some algebra, we obtain the closed-loop error dynamics:

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{e}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} = \mathbf{0}. \]

This resembles a set of coupled mass-spring-damper systems in joint coordinates, leading to spring-like behavior around \( \mathbf{q}_d \) that is nearly configuration independent when gravity is accurately canceled.

We now sketch a classical Lyapunov proof of global asymptotic stability of the equilibrium \( \mathbf{e} = \dot{\mathbf{e}} = \mathbf{0} \) for PD plus exact gravity compensation.

Consider the Lyapunov candidate

\[ V(\mathbf{e},\dot{\mathbf{e}}) = \tfrac{1}{2}\dot{\mathbf{e}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{e}} + \tfrac{1}{2}\mathbf{e}^\top \mathbf{K}_p \mathbf{e}. \]

Because \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{K}_p \) are positive definite for all \( \mathbf{q} \), the function \( V \) is positive definite and radially unbounded in \( (\mathbf{e},\dot{\mathbf{e}}) \).

Differentiating along closed-loop trajectories and using the dynamics from the previous section:

\[ \begin{aligned} \dot{V} &= \tfrac{1}{2}\dot{\mathbf{e}}^\top \dot{\mathbf{M}}(\mathbf{q}) \dot{\mathbf{e}} + \dot{\mathbf{e}}^\top \mathbf{M}(\mathbf{q}) \ddot{\mathbf{e}} + \dot{\mathbf{e}}^\top \mathbf{K}_p \mathbf{e} \\ &= \dot{\mathbf{e}}^\top \big(\mathbf{M}(\mathbf{q}) \ddot{\mathbf{e}} + \tfrac{1}{2}\dot{\mathbf{M}}(\mathbf{q}) \dot{\mathbf{e}}\big) + \dot{\mathbf{e}}^\top \mathbf{K}_p \mathbf{e}. \end{aligned} \]

Substituting the error dynamics, \( \mathbf{M}(\mathbf{q}) \ddot{\mathbf{e}} = -\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e} \):

\[ \begin{aligned} \dot{V} &= \dot{\mathbf{e}}^\top \big(-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{e}} - \mathbf{K}_d\dot{\mathbf{e}} - \mathbf{K}_p\mathbf{e} + \tfrac{1}{2}\dot{\mathbf{M}}(\mathbf{q}) \dot{\mathbf{e}}\big) + \dot{\mathbf{e}}^\top \mathbf{K}_p \mathbf{e} \\ &= \dot{\mathbf{e}}^\top \big(\tfrac{1}{2}\dot{\mathbf{M}}(\mathbf{q}) - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\big)\dot{\mathbf{e}} - \dot{\mathbf{e}}^\top \mathbf{K}_d \dot{\mathbf{e}}. \end{aligned} \]

Using the skew-symmetry property, \( \dot{\mathbf{M}}(\mathbf{q}) - 2\mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) is skew-symmetric, so the quadratic form in \( \dot{\mathbf{e}} \) is zero:

\[ \dot{\mathbf{e}}^\top \big(\tfrac{1}{2}\dot{\mathbf{M}}(\mathbf{q}) - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\big)\dot{\mathbf{e}} = 0. \]

Therefore,

\[ \dot{V} = -\dot{\mathbf{e}}^\top \mathbf{K}_d \dot{\mathbf{e}} \leq 0, \]

with equality only when \( \dot{\mathbf{e}} = \mathbf{0} \). Using standard Lyapunov and LaSalle arguments (and the fact that the only invariant set with \( \dot{V} = 0 \) is \( \mathbf{e} = \dot{\mathbf{e}} = \mathbf{0} \)), we conclude that the equilibrium \( \mathbf{q} = \mathbf{q}_d, \dot{\mathbf{q}} = \mathbf{0} \) is globally asymptotically stable under PD with exact gravity compensation.

Most robotics software stacks already provide an inverse dynamics function of the form

\[ \boldsymbol{\tau} = \text{ID}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) = \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}). \]

To obtain pure gravity torque, simply call inverse dynamics with zero velocities and accelerations:

\[ \mathbf{g}(\mathbf{q}) = \text{ID}(\mathbf{q}, \mathbf{0}, \mathbf{0}). \]

Gravity compensation in a real control loop then follows the pattern:

1. Read joint positions \( \mathbf{q} \) (and optionally velocities).
2. Compute \( \mathbf{g}(\mathbf{q}) \) via the model.
3. Compute PD torque based on desired \( \mathbf{q}_d, \dot{\mathbf{q}}_d \).
4. Send \( \boldsymbol{\tau} = \boldsymbol{\tau}_{PD} + \mathbf{g}(\mathbf{q}) \) to actuators.

flowchart TD
  SENS["1) Read q, dq from encoders"] --> GCOMP["2) Call inverse dynamics with dq=0, ddq=0 to get g(q)"]
  GCOMP --> PDBLOCK["3) Compute PD torque: tau_PD"]
  PDBLOCK --> SUM["4) tau = tau_PD + g(q)"]
  SUM --> SEND["5) Send tau to motor drivers"]
        

This approach reuses your existing dynamics implementation and is independent of how \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) are constructed internally (symbolic, URDF-based, or numerically).

In practice, gravity compensation is never exact: \( \hat{\mathbf{g}}(\mathbf{q}) = \mathbf{g}(\mathbf{q}) + \boldsymbol{\Delta}(\mathbf{q}) \), where \( \boldsymbol{\Delta}(\mathbf{q}) \) represents modeling errors (mass uncertainty, unmodeled payload, etc.).

With PD + imperfect gravity compensation, the torque is

\[ \boldsymbol{\tau} = \mathbf{K}_p (\mathbf{q}_d - \mathbf{q}) + \mathbf{K}_d (\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) + \hat{\mathbf{g}}(\mathbf{q}) = \mathbf{K}_p \mathbf{e} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{g}(\mathbf{q}) + \boldsymbol{\Delta}(\mathbf{q}). \]

The error term \( \boldsymbol{\Delta}(\mathbf{q}) \) acts as a disturbance. For reasonable gain choices, the effect is typically:

• Small steady-state posture error under constant gravity load.
• Additional torques demanded from actuators (important near joint limits).
• Residual coupling between joints for large configuration changes.

Later chapters (robust and adaptive control) will provide systematic ways to handle these uncertainties. For now, the main takeaway is that gravity compensation linearizes the effect of gravity around arbitrary postures, greatly simplifying gain tuning and improving performance of simple PD controllers.

We show a Python implementation using a standard rigid-body dynamics library (for example, pinocchio or roboticstoolbox). The exact API may differ, but the pattern is always the same: call inverse dynamics with dq = 0, ddq = 0.

import numpy as np

# Example: using pinocchio-style API (pseudo-code)
import pinocchio as pin

# Load robot model from URDF
model = pin.buildModelFromUrdf("my_robot.urdf")
data = model.createData()

def gravity_torque(q):
    """
    Return gravity compensation torque tau_g(q) for configuration q.
    q: numpy array of shape (n,)
    """
    dq = np.zeros(model.nv)
    ddq = np.zeros(model.nv)
    # Inverse dynamics: tau = M(q) ddq + C(q,dq) dq + g(q)
    tau = pin.rnea(model, data, q, dq, ddq)
    return tau

def pd_gravity_control(q, dq, q_d, dq_d, Kp, Kd):
    """
    Joint-space PD + gravity compensation.
    Kp, Kd: diagonal gain matrices (np.ndarray) or scalars.
    """
    e = q_d - q
    de = dq_d - dq
    tau_pd = Kp @ e + Kd @ de
    tau_g = gravity_torque(q)
    return tau_pd + tau_g

# Example usage inside a control loop
n = model.nq
Kp = np.diag([80.0] * n)
Kd = np.diag([15.0] * n)
q_d = np.zeros(n)
dq_d = np.zeros(n)

def control_step(q_meas, dq_meas):
    tau = pd_gravity_control(q_meas, dq_meas, q_d, dq_d, Kp, Kd)
    # send tau to actuators here
    return tau
      

In simulation (e.g., with PyBullet or Gazebo Python bindings), the same control law can be used by querying simulated joint states and sending torques back through the physics engine interface.

C++ libraries such as RBDL or pinocchio offer high-performance rigid-body dynamics. Below is a typical pattern (here using an RBDL-like API):

#include <rbdl/rbdl.h>
#include <vector>

using namespace RigidBodyDynamics;
using Eigen::VectorXd;

struct PDGravityController {
    Model model;
    VectorXd Kp;
    VectorXd Kd;

    PDGravityController(const Model& model_in,
                        const VectorXd& Kp_in,
                        const VectorXd& Kd_in)
        : model(model_in), Kp(Kp_in), Kd(Kd_in) {}

    VectorXd gravityTorque(const VectorXd& q) {
        VectorXd dq  = VectorXd::Zero(model.dof_count);
        VectorXd ddq = VectorXd::Zero(model.dof_count);
        VectorXd tau = VectorXd::Zero(model.dof_count);
        InverseDynamics(model, q, dq, ddq, tau);
        return tau; // tau = g(q)
    }

    VectorXd computeTorque(const VectorXd& q,
                           const VectorXd& dq,
                           const VectorXd& q_d,
                           const VectorXd& dq_d) {
        VectorXd e  = q_d - q;
        VectorXd de = dq_d - dq;
        VectorXd tau_pd = Kp.cwiseProduct(e) + Kd.cwiseProduct(de);
        VectorXd tau_g  = gravityTorque(q);
        return tau_pd + tau_g;
    }
};

// Example usage:
// 1. Load model from URDF into RBDL::Model model;
// 2. Set Kp, Kd; create controller;
// 3. In the real-time loop, call controller.computeTorque(q, dq, q_d, dq_d).
      

For real-time safety, the computation of \( \mathbf{g}(\mathbf{q}) \) should be bounded and executed at the same rate as the torque control loop. RBDL and similar libraries are designed with this use case in mind.

Java does not have as many standardized robotics dynamics libraries, but you can implement analytic expressions for simple manipulators and use a numerical linear algebra library (e.g. EJML) for matrices. Below is a 2-link planar arm gravity model:

For link masses \( m_1, m_2 \), lengths \( l_1, l_2 \), and gravitational acceleration \( g_0 \), the gravity torques are

\[ \begin{aligned} g_1(\mathbf{q}) &= \big(m_1 \tfrac{l_1}{2} + m_2 l_1\big) g_0 \cos(q_1) + m_2 \tfrac{l_2}{2} g_0 \cos(q_1 + q_2), \\ g_2(\mathbf{q}) &= m_2 \tfrac{l_2}{2} g_0 \cos(q_1 + q_2). \end{aligned} \]

public class GravityComp2Link {

    private final double m1, m2;
    private final double l1, l2;
    private final double g0;

    public GravityComp2Link(double m1, double m2,
                            double l1, double l2,
                            double g0) {
        this.m1 = m1;
        this.m2 = m2;
        this.l1 = l1;
        this.l2 = l2;
        this.g0 = g0;
    }

    public double[] gravityTorque(double q1, double q2) {
        double c1 = Math.cos(q1);
        double c12 = Math.cos(q1 + q2);

        double g1 = (m1 * l1 / 2.0 + m2 * l1) * g0 * c1
                  +  m2 * l2 / 2.0 * g0 * c12;
        double g2 =  m2 * l2 / 2.0 * g0 * c12;

        return new double[]{g1, g2};
    }

    public double[] pdGravityControl(double q1, double q2,
                                     double dq1, double dq2,
                                     double q1_d, double q2_d,
                                     double dq1_d, double dq2_d,
                                     double Kp1, double Kp2,
                                     double Kd1, double Kd2) {

        double e1  = q1_d  - q1;
        double e2  = q2_d  - q2;
        double de1 = dq1_d - dq1;
        double de2 = dq2_d - dq2;

        double tau_pd1 = Kp1 * e1 + Kd1 * de1;
        double tau_pd2 = Kp2 * e2 + Kd2 * de2;

        double[] g = gravityTorque(q1, q2);
        double tau1 = tau_pd1 + g[0];
        double tau2 = tau_pd2 + g[1];

        return new double[]{tau1, tau2};
    }
}
      

This example can be connected to a simulation environment written in Java or used to control a simple 2-DOF manipulator via a fieldbus interface that supports torque commands.

10.1 MATLAB (Robotics System Toolbox)

MATLAB's rigidBodyTree representation provides a direct function gravityTorque to compute \( \mathbf{g}(\mathbf{q}) \).

% Create robot from URDF
robot = importrobot("my_robot.urdf");
robot.DataFormat = "row";
robot.Gravity = [0 0 -9.81];

% PD + gravity compensation
Kp = diag([80 80 80]);  % example for 3-DOF
Kd = diag([15 15 15]);

function tau = pd_grav_control(robot, q, dq, qd, dqd, Kp, Kd)
    e  = qd - q;
    de = dqd - dq;
    tau_pd = (Kp * e.' + Kd * de.').';   % row format
    tau_g  = gravityTorque(robot, q);
    tau    = tau_pd + tau_g;
end

% Inside a control loop:
% [q, dq] measured from hardware
% qd, dqd desired; call pd_grav_control and send tau.
      

Simulink Block Diagram (Conceptual)

In Simulink, a typical posture control model with gravity compensation includes:

• A block providing q_d (desired joint angles) and dq_d (desired velocities, often zero).
• A PD block implementing \( \mathbf{K}_p \) and \( \mathbf{K}_d \).
• A MATLAB Function block calling gravityTorque(robot,q).
• A summation block adding tau_PD and tau_g to produce tau.
• A plant model (either a Simscape Multibody robot or a rigidBodyTree-based dynamics block).

The measured joint states q, dq are fed back into both the PD block and the gravity torque computation block.

10.2 Wolfram Mathematica (Symbolic and Numeric)

Mathematica is well suited for deriving and implementing gravity compensation from symbolic models. For a simple 2-link planar arm:

(* Define symbols *)
ClearAll[q1, q2, m1, m2, l1, l2, g0];
U1 = m1 * g0 * (l1/2 * Sin[q1]);
U2 = m2 * g0 * (l1 * Sin[q1] + l2/2 * Sin[q1 + q2]);
U  = U1 + U2;

(* Gravity vector g(q) = dU/dq *)
gq = Grad[U, {q1, q2}];
(* gq[[1]] and gq[[2]] are the symbolic torques g1(q), g2(q) *)

(* Numeric function for gravity torque *)
gravityTorque = Function[
  {q1val, q2val, m1val, m2val, l1val, l2val, gval},
  Module[{g1, g2},
    g1 = gq[[1]] /. {q1 -> q1val, q2 -> q2val,
                    m1 -> m1val, m2 -> m2val,
                    l1 -> l1val, l2 -> l2val,
                    g0 -> gval};
    g2 = gq[[2]] /. {q1 -> q1val, q2 -> q2val,
                    m1 -> m1val, m2 -> m2val,
                    l1 -> l1val, l2 -> l2val,
                    g0 -> gval};
    {g1, g2}
  ]
];

(* Example call *)
gravityTorque[0.5, 0.2, 5.0, 3.0, 0.5, 0.4, 9.81]
      

For more complex robots, you can construct the Lagrangian, compute \( \mathbf{M}(\mathbf{q}) \) and \( \mathbf{g}(\mathbf{q}) \) symbolically, and then export optimized C code for real-time use.

In this lesson we connected your knowledge of robot dynamics to a core building block of joint-space control: gravity compensation. Starting from the manipulator equation, we identified the gravity vector \( \mathbf{g}(\mathbf{q}) \) as the gradient of potential energy, explained how inverse dynamics functions can be reused to compute gravity torques, and combined this with PD control to obtain posture controllers with configuration-independent behavior.

We showed, via a Lyapunov argument, that PD plus exact gravity compensation yields global asymptotic stability for constant desired postures, and we discussed the effect of modeling errors. Finally, we implemented gravity compensation in multiple languages (Python, C++, Java, MATLAB/Simulink, and Mathematica), giving you a practical toolkit for real robot controllers and simulations. In the next lessons, we will refine gain tuning and then move toward fully model-based nonlinear control such as computed-torque.