Center of mass of a robot composed of multiple rigid bodies

[antoine.richard]

Hey all,

Is there a way to get the center of mass of a robot composed of multiple rigid bodies?

I have a robot, and I want to know its center of mass.
This robot is composed of multiple elements that offset its center of mass from the “base_link”.
I would like to apply a force to the center of mass of the robot, but I don’t know how to get the center of mass of the whole robot.

I could compute it myself, but it’s a bit cumbersome, here is the code I am using to get the center of mass:

import omni
from pxr import Usd, UsdGeom, Gf, UsdPhysics
from omni.physx.scripts import utils
import math
import numpy as np
M = 0
CoM = np.array(Gf.Vec3d(0,0,0))
curr_prim = stage.GetPrimAtPath("/")
for prim in Usd.PrimRange(curr_prim):
	try:
		MassAPI = UsdPhysics.MassAPI.Get(stage, prim.GetPath())
		mass = MassAPI.GetMassAttr().Get()
		com = MassAPI.GetCenterOfMassAttr().Get()
		tr = prim.GetProperty('xformOp:translate').Get()
		if mass:
			print("RigidBody: "+str(prim.GetPath()) + ", mass: "+str(mass)+", translation: "+str(tr)+", center of mass: "+str(com))
			CoM += (com + tr)*mass
			M += mass
	except:
		pass
		
print("Robot total mass: " + str(M))
print("Robot center of mass: "+str(CoM/M))

This gives me something which doesn’t work when applied on my robot, but the mass is wrong as well, so I can’t really say what’s the problem…

I am assuming this information is available somewhere, as apply_body_force from dynamic control seem to be applying the force at the center of mass of the uppermost rigid body in the tree.

Thanks

[rgasoto]

Hello, thanks for the feedback!

The physics engine does have the rigid body COM, but it was not exposed to get the auto computed from simulation in the usd API or the Dynamic Control extension yet. that is already on our roadmap for features we want to implement.

If you have the correct Center of Mass and mass properties for the rigid body, you can override the autocomputed values with the Mass API on the rigid body.

[antoine.richard]

Thanks for the answer, I’m looking forward to that feature !