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
dynamic control seem to be applying the force at the center of mass of the uppermost rigid body in the tree.