how to calculate the gravity torque of a still legged robot?

[zdpisadaniu]

Dear Sir:
I am new guy to use the greate pinocchio library.I wanna get the 12 joints torque of my legged robot that stands still on the ground and the mass of my legged robot is 100Kg. So I design the python code like this

import pinocchio as pin
import numpy as np
import sys
import os

np.set_printoptions(suppress=True, precision=6)

def main():
urdf_filename = "t2.urdf"
if not os.path.exists(urdf_filename):
print(f"Error: URDF file '{urdf_filename}' not found.")
sys.exit(1)

model = pin.buildModelFromUrdf(urdf_filename, pin.JointModelFreeFlyer())
data = model.createData()

gravity = pin.Motion()
gravity.linear = np.array([0, 0, -9.81])
gravity.angular = np.zeros(3)
model.gravity = gravity

q = np.zeros(model.nq)
v = np.zeros(model.nv)
a = np.zeros(model.nv)

q[0:3] = [0.0, 0.0, 0.574855032086]  
quat = pin.Quaternion(np.cos(np.pi/2), 0, np.sin(np.pi/2), 0)  
q[3:7] = [quat.w, quat.x, quat.y, quat.z]

joint_angles = np.array([0.0, 0.85, -1.34,   # FL
                        -0.0, 0.85, -1.34,   # FR
                         0.0, 0.85, -1.34,   # RL
                        -0.0, 0.85, -1.34])  # RR
q[7:] = joint_angles
gravity_torque = pin.computeGeneralizedGravity(model, data, q)
joint_torques = gravity_torque[6:]
print('joint_tau',joint_torques)

However ,the output of these code is
joint_tau [ 5.206796 2.489272 -1.094445 5.206796 2.489272 -1.094445 -5.206796 2.489272 -1.094445 -5.206796 2.489272 -1.094445]
Apparently, the torque of the pin.computeGeneralizedGravity function is too small for my 100Kg robot.
So I wonder where did I go wrong? Should I use the rnea(model,data,q ,0,0,fext) insteadly? Or should I put some foot contact constraint to my legged robot so the pinocchio can confirm the legged robot is on the ground? It will be highly appreciated if anyone can offer some suggestions for my problem.It bothers me for a long time.

Best regards!

[ManifoldFR]

Hello @zdpisadaniu, and thanks for your interest in Pinocchio!

I took the liberty of moving your issue to the discussion tab of the repo to avoid gunking up the issue tracker.

If I understand correctly, what you want to do is compute the gravity compensation torque for the underactuated humanoid, correct?

The function you called is not that, not only because the Pinocchio model isn't aware of the foot contact constraint, but because the function you called computed the generalized gravity joint torque (the term $G(q)$ in the classical Lagrangian multibody equation, if you don't know what that is, look it up).

Furthermore you can't just get the motor torques because that is ill-posed, you'll need to compute the ground reaction forces along with that. Joint torques aren't enough to hold up your body on the ground, the reaction force the ground exerts on you is what stops you from clipping through it 😉

What you want to do probably is formulate bilateral contact constraints (you may use RigidConstraintModel to do so but this API is bound to change in the next release) and compute both the joint torque and ground reaction force, by solving a set of (under determined) linear equations using e.g. QR. Take a look at this function in my control library.

---

A small remark: you didn't need to redefine the model.gravity parameter actually. The value you set is already the default.

[zdpisadaniu]

Thank you very much for your prompt reply. I will study the suggestions you put forward carefully.
Wish you good health and smooth work!