Gravity compensation is a fundamental technique in robotics that allows a robot arm to be freely moved by hand while maintaining its position when released. This page will explain how you can create a simple gravity compensation algorithm for the SO-100 robot arm.You can enable gravity compensation in phosphobot by going to Leader arm control and clicking on the Enable gravity compensation button.
At its core, gravity compensation counteracts the effect of gravity on a robot’s joints. Without compensation, a robot arm would fall due to gravity when the motors are not actively holding position. With proper compensation, you can move the robot by hand, and it will stay in place when released.
Inverse dynamics calculates the joint torques (τ) required to achieve a desired motion, given the current positions, velocities, and accelerations of the joints. In our case, we use it to find the gravity torques (τg).The equation for inverse dynamics can be expressed as:τ=M(q)q¨+C(q,q˙)q˙+G(q)τWhere:
M(q) is the mass matrix
C(q,q˙) represents Coriolis and centrifugal forces
G(q) is the gravity vector
q,q˙,q¨ are joint positions, velocities, and accelerations respectively
We want to solve for a specific case where the robot is at rest.
When we set velocities and accelerations to zero, we get just the gravity term:
τg=G(q)\tau_g = G(q)τg=G(q)
Our robot is an SO-100 with STS3215 motors, which don’t natively support torque control. Instead, they use position control.To achieve gravity compensation, we need to simulate the effect of torque control using position control.Instead of directly applying the calculated torques (which would require torque control), we create small virtual displacements in the direction that would counteract gravity.For each joint, we calculate a desired position that’s slightly offset from the current position:θdes=θcurrent+α⋅τgθdesWhere:
θdes is the desired joint position
θcurrent is the current joint position
α is a scaling factor that determines how much the joint should move
To make the robot arm more compliant during gravity compensation, we adjust the PID gains of the motors. The default gains are optimized for position control, but we need different gains for gravity compensation.
Tuning Note
These will depend on the motors you have, 6V or 12V.
Play around with the values to find the best settings for your robot.
These are lower than the default gains, making the robot more responsive to external forces while still maintaining enough stiffness to hold position against gravity.
The main gravity compensation loop needs to run at a high frequency to provide smooth motion. You should aim for at least 50 Hz to 100 Hz.Here’s a simplified version of the loop:
Read current joint positions
Update the robot state in the physics simulator (Mujoco, Genesis, PyBullet, etc.)
Calculate gravity torques using inverse dynamics
Compute desired positions with the virtual displacement formula
The virtual displacement method can be understood as a form of impedance control. In traditional impedance control, the relationship between force and position is:F=K(xdes−x)+Bx˙FWhere:
xdes is the desired position
x is the current position
K is stiffness
B is damping
Our approach inverts this relationship:xdes=x+K−1FxdesIn our case: