static-contact-dynamics.py

Go to the documentation of this file.

import numpy as np
import pinocchio as pin
 
from os.path import dirname, join, abspath
 
np.set_printoptions(linewidth=np.inf)
 
# ----- PROBLEM STATEMENT ------
#
# We want to find the contact forces and torques required to stand still at a configuration 'q0'.
# We assume 3D contacts at each of the feet
#
# The dynamic equation would look like:
#
# M*q_ddot + g(q) + C(q, q_dot) = tau + J^T*lambda --> (for the static case) --> g(q) = tau + Jc^T*lambda (1)
 
# ----- SOLVING STRATEGY ------
 
# Split the equation between the base link (_bl) joint and the rest of the joints (_j). That is,
#
#  | g_bl |   |  0  |   | Jc__feet_bl.T |   | l1 |
#  | g_j  | = | tau | + | Jc__feet_j.T  | * | l2 |    (2)
#                                           | l3 |
#                                           | l4 |
 
# First, find the contact forces l1, l2, l3, l4 (these are 3 dimensional) by solving for the first 6 rows of (2).
# That is,
#
# g_bl   = Jc__feet_bl.T * | l1 |
#                          | l2 |    (3)
#                          | l3 |
#                          | l4 |
#
# Thus we find the contact froces by computing the jacobian pseudoinverse,
#
# | l1 | = pinv(Jc__feet_bl.T) * g_bl  (4)
# | l2 |
# | l3 |
# | l4 |
#
# Now, we can find the necessary torques using the bottom rows in (2). That is,
#
#                             | l1 |
#  tau = g_j - Jc__feet_j.T * | l2 |    (5)
#                             | l3 |
#                             | l4 |
 
# ----- SOLUTION ------
 
# 0. DATA
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
 
model_path = join(pinocchio_model_dir, "example-robot-data/robots")
mesh_dir = pinocchio_model_dir
urdf_filename = "solo12.urdf"
urdf_model_path = join(join(model_path, "solo_description/robots"), urdf_filename)
 
model, collision_model, visual_model = pin.buildModelsFromUrdf(
    urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
)
data = model.createData()
 
q0 = np.array(
    [
        0.0,
        0.0,
        0.235,
        0.0,
        0.0,
        0.0,
        1.0,
        0.0,
        0.8,
        -1.6,
        0.0,
        -0.8,
        1.6,
        0.0,
        0.8,
        -1.6,
        0.0,
        -0.8,
        1.6,
    ]
)
v0 = np.zeros(model.nv)
a0 = np.zeros(model.nv)
 
# 1. GRAVITY TERM
 
# We compute the gravity terms by using the ID at desired configuration q0, with velocity and acceleration being 0. I.e., ID with a = v = 0
g_grav = pin.rnea(model, data, q0, v0, a0)
 
g_bl = g_grav[:6]
g_j = g_grav[6:]
 
# 2. FIND CONTACTS
 
# First, we set the frame for our contacts. We assume the contacts are placed at the following 4 frames and they are 3D
feet_names = ["FL_FOOT", "FR_FOOT", "HL_FOOT", "HR_FOOT"]
feet_ids = [model.getFrameId(n) for n in feet_names]
bl_id = model.getFrameId("base_link")
ncontact = len(feet_names)
 
# Now, we need to find the contact Jacobians appearing in (1).
# These are the Jacobians that relate the joint velocity  to the velocity of each feet
Js__feet_q = [
    np.copy(pin.computeFrameJacobian(model, data, q0, id, pin.LOCAL)) for id in feet_ids
]
 
Js__feet_bl = [np.copy(J[:3, :6]) for J in Js__feet_q]
 
# Notice that we can write the equation above as an horizontal stack of Jacobians transposed and vertical stack of contact forces
Jc__feet_bl_T = np.zeros([6, 3 * ncontact])
Jc__feet_bl_T[:, :] = np.vstack(Js__feet_bl).T
 
# Now I only need to do the pinv to compute the contact forces
 
ls = np.linalg.pinv(Jc__feet_bl_T) @ g_bl  # This is (3)
 
# Contact forces at local coordinates (at each foot coordinate)
ls__f = np.split(ls, ncontact)
 
pin.framesForwardKinematics(model, data, q0)
 
# Contact forces at base link frame
ls__bl = []
for l__f, foot_id in zip(ls__f, feet_ids):
    l_sp__f = pin.Force(l__f, np.zeros(3))
    l_sp__bl = data.oMf[bl_id].actInv(data.oMf[foot_id].act(l_sp__f))
    ls__bl.append(np.copy(l_sp__bl.vector))
 
print("\n--- CONTACT FORCES ---")
for l__f, foot_id, name in zip(ls__bl, feet_ids, feet_names):
    print("Contact force at foot {} expressed at the BL is: {}".format(name, l__f))
 
# Notice that if we add all the contact forces are equal to the g_grav
print(
    "Error between contact forces and gravity at base link: {}".format(
        np.linalg.norm(g_bl - sum(ls__bl))
    )
)
 
# 3. FIND TAU
# Find Jc__feet_j
Js_feet_j = [np.copy(J[:3, 6:]) for J in Js__feet_q]
 
Jc__feet_j_T = np.zeros([12, 3 * ncontact])
Jc__feet_j_T[:, :] = np.vstack(Js_feet_j).T
 
# Apply (5)
tau = g_j - Jc__feet_j_T @ ls
 
# 4. CROSS CHECKS
 
# INVERSE DYNAMICS
# We can compare this torques with the ones one would obtain when computing the ID considering the external forces in ls
pin.framesForwardKinematics(model, data, q0)
 
joint_names = ["FL_KFE", "FR_KFE", "HL_KFE", "HR_KFE"]
joint_ids = [model.getJointId(n) for n in joint_names]
 
fs_ext = [pin.Force(np.zeros(6)) for _ in range(len(model.joints))]
for idx, joint in enumerate(model.joints):
    if joint.id in joint_ids:
        fext__bl = pin.Force(ls__bl[joint_ids.index(joint.id)])
        fs_ext[idx] = data.oMi[joint.id].actInv(data.oMf[bl_id].act(fext__bl))
 
tau_rnea = pin.rnea(model, data, q0, v0, a0, fs_ext)
 
print("\n--- ID: JOINT TORQUES ---")
print("Tau from RNEA:         {}".format(tau_rnea))
print("Tau computed manually: {}".format(np.append(np.zeros(6), tau)))
print("Tau error: {}".format(np.linalg.norm(np.append(np.zeros(6), tau) - tau_rnea)))
 
# FORWARD DYNAMICS
# We can also check the results using FD. FD with the tau we got, q0 and v0, should give 0 acceleration and the contact forces
Js_feet3d_q = [np.copy(J[:3, :]) for J in Js__feet_q]
acc = pin.forwardDynamics(
    model,
    data,
    q0,
    v0,
    np.append(np.zeros(6), tau),
    np.vstack(Js_feet3d_q),
    np.zeros(12),
)
 
print("\n--- FD: ACC. & CONTACT FORCES ---")
print("Norm of the FD acceleration: {}".format(np.linalg.norm(acc)))
print("Contact forces manually: {}".format(ls))
print("Contact forces FD: {}".format(data.lambda_c))
print("Contact forces error: {}".format(np.linalg.norm(data.lambda_c - ls)))