Thanks to the both of you for the resources.
In the doc, it’s mentioned that forces are applied in Newtons. Is it true for all functions that apply forces, or only for the ones from dynamic control ? If I remember correctly, when I use the PhysX python bindings, I need 100 N to increase the velocity of a 1 kg object by 1 m/s per second. So I believe it’s not Newtons, but centi-Newtons.
To test that I am disabling the gravity for an object and apply an upward force onto it. I am then plotting the position and velocity against the time to check the units.
Here is the code I am using to test that:
import os
import omni
import carb
from pxr import Gf, Sdf
from omni.isaac.python_app import OmniKitHelper
from omni.isaac.utils.scripts.nucleus_utils import find_nucleus_server
from scipy.spatial.transform import Rotation as SSTR
import time
import numpy as np
CONFIG = {
"experience": f'{os.environ["EXP_PATH"]}/omni.isaac.sim.python.kit',
"renderer": "RayTracedLighting",
"headless": False,
}
def setup_cpu_physics(stage, physics_name, gravity=9.81, gravity_direction=Gf.Vec3f(0.0, 0.0, -1.0)):
from pxr import PhysicsSchemaTools, PhysxSchema
# Add physics scene
scene = UsdPhysics.Scene.Define(stage, Sdf.Path(physics_name))
# Set gravity vector
scene.CreateGravityDirectionAttr().Set(gravity_direction)
scene.CreateGravityMagnitudeAttr().Set(gravity*100)
# Set physics scene to use cpu physics
PhysxSchema.PhysxSceneAPI.Apply(stage.GetPrimAtPath(physics_name))
physxSceneAPI = PhysxSchema.PhysxSceneAPI.Get(stage,physics_name)
physxSceneAPI.CreateEnableCCDAttr(True)
physxSceneAPI.CreateEnableStabilizationAttr(True)
physxSceneAPI.CreateEnableGPUDynamicsAttr(False)
physxSceneAPI.CreateBroadphaseTypeAttr("MBP")
physxSceneAPI.CreateSolverTypeAttr("TGS")
def get_nucleus_server():
result, nucleus_server = find_nucleus_server()
if result is False:
carb.log_error(
"Could not find nucleus server. Stopping."
)
exit(1)
return nucleus_server
def Quaternion2RotationMatrix(quat):
# QX,QY,QZ,QW
R = SSTR.from_quat(quat)
return R.as_matrix()
def getLinearVel(rigidBodyAPI):
#return np.array([1,1,1])
vec = rigidBodyAPI.GetVelocityAttr().Get() # cm/s
return np.array([vec[0], vec[1], vec[2]]) * 0.01 # m/s
def getRelativeLinearVel(rigidBodyAPI, rotWR):
#return np.array([1,1,1])
world_velocity = getLinearVel(rigidBodyAPI)
robot_velocity = np.matmul(rotWR, world_velocity)
return robot_velocity # m/s
if __name__ == "__main__":
omniverse_kit = OmniKitHelper(CONFIG)
ext_manager = omni.kit.app.get_app().get_extension_manager()
omniverse_kit.set_setting("/app/window/drawMouse", True)
omniverse_kit.set_setting("/app/livestream/proto", "ws")
ext_manager.set_extension_enabled_immediate("omni.physx.bundle", True)
ext_manager.set_extension_enabled_immediate("omni.syntheticdata", True)
ext_manager.set_extension_enabled_immediate("omni.kit.livestream.core", True)
ext_manager.set_extension_enabled_immediate("omni.kit.livestream.native", True)
ext_manager.set_extension_enabled_immediate("omni.kit.window.stage", True)
ext_manager.set_extension_enabled_immediate("omni.kit.property.bundle", True)
from omni.physx import get_physx_interface
from omni.physx.scripts import utils
from pxr import UsdPhysics
nucleus_server = get_nucleus_server()
cube_asset_path = nucleus_server + "/Isaac/Props/Blocks/nvidia_cube.usd"
stage = omni.usd.get_context().get_stage()
setup_cpu_physics(stage, "/World/physicsScene")
physxIFace = get_physx_interface()
global t
global it
t = 0
it = 0
def physics_update(dt: float):
global t, it
transform = physxIFace.get_rigidbody_transformation(cube_path)
rot_mat = Quaternion2RotationMatrix(transform['rotation'])
rot_mat_inv = np.linalg.inv(rot_mat)
print('step:',it,'time:', it*1/120.0, 'speed:',getRelativeLinearVel(rigidbody,rot_mat_inv))
t += dt
it += 1
physxIFace.apply_force_at_pos(cube_path, carb._carb.Float3([0,0,100]), np.array(transform['position']))
physics_sub = omni.physx.acquire_physx_interface().subscribe_physics_step_events(physics_update)
position = Gf.Vec3d(0, 0, 0)
cube_path = omni.usd.get_stage_next_free_path(stage, '/World/nvidia_cube', False)
obj_prim = stage.DefinePrim(cube_path, "Xform")
obj_prim.GetReferences().AddReference(cube_asset_path)
utils.setRigidBody(obj_prim, "convexHull", False)
mass_api = UsdPhysics.MassAPI.Apply(obj_prim)
mass_api.CreateMassAttr(1.0)
rigidbody = UsdPhysics.RigidBodyAPI.Get(stage, cube_path)
print(rigidbody.GetSchemaAttributeNames())
omni.kit.commands.execute("ChangeProperty",prop_path="/World/nvidia_cube.physxRigidBody:disableGravity", value=True, prev=False)
while omniverse_kit.app.is_running():
omniverse_kit.update(1.0/30, physics_dt=1/120.0)
omniverse_kit.stop()
time.sleep(10)
omniverse_kit.shutdown()
Here is the output
step: 1 time: 0.008333333333333333 speed: [0. 0. 0.]
step: 2 time: 0.016666666666666666 speed: [0. 0. 0.00833333]
step: 3 time: 0.025 speed: [0. 0. 0.01666667]
step: 4 time: 0.03333333333333333 speed: [0. 0. 0.025]
step: 5 time: 0.041666666666666664 speed: [0. 0. 0.03333333]
step: 6 time: 0.05 speed: [0. 0. 0.04166667]
step: 7 time: 0.058333333333333334 speed: [0. 0. 0.05]
step: 8 time: 0.06666666666666667 speed: [0. 0. 0.05833334]
step: 9 time: 0.075 speed: [0. 0. 0.06666667]
step: 10 time: 0.08333333333333333 speed: [0. 0. 0.07500001]
step: 11 time: 0.09166666666666666 speed: [0. 0. 0.08333334]
step: 12 time: 0.1 speed: [0. 0. 0.09166667]
step: 13 time: 0.10833333333333334 speed: [0. 0. 0.1]
step: 14 time: 0.11666666666666667 speed: [0. 0. 0.10833333]
step: 15 time: 0.125 speed: [0. 0. 0.11666666]
step: 16 time: 0.13333333333333333 speed: [0. 0. 0.12499999]
step: 17 time: 0.14166666666666666 speed: [0. 0. 0.13333332]
step: 18 time: 0.15 speed: [0. 0. 0.14166665]
step: 19 time: 0.15833333333333333 speed: [0. 0. 0.14999998]
step: 20 time: 0.16666666666666666 speed: [0. 0. 0.15833331]
step: 21 time: 0.175 speed: [0. 0. 0.16666664]
step: 22 time: 0.18333333333333332 speed: [0. 0. 0.17499998]
step: 23 time: 0.19166666666666668 speed: [0. 0. 0.18333332]
step: 24 time: 0.2 speed: [0. 0. 0.19166666]
step: 25 time: 0.20833333333333334 speed: [0. 0. 0.2]
step: 26 time: 0.21666666666666667 speed: [0. 0. 0.20833334]
step: 27 time: 0.225 speed: [0. 0. 0.21666668]
step: 28 time: 0.23333333333333334 speed: [0. 0. 0.22500002]
step: 29 time: 0.24166666666666667 speed: [0. 0. 0.23333336]
step: 30 time: 0.25 speed: [0. 0. 0.2416667]
step: 31 time: 0.25833333333333336 speed: [0. 0. 0.25000004]
step: 32 time: 0.26666666666666666 speed: [0. 0. 0.25833338]
step: 33 time: 0.275 speed: [0. 0. 0.26666672]
step: 34 time: 0.2833333333333333 speed: [0. 0. 0.27500006]
step: 35 time: 0.2916666666666667 speed: [0. 0. 0.2833334]
step: 36 time: 0.3 speed: [0. 0. 0.29166674]
step: 37 time: 0.30833333333333335 speed: [0. 0. 0.30000008]
step: 38 time: 0.31666666666666665 speed: [0. 0. 0.30833342]
step: 39 time: 0.325 speed: [0. 0. 0.31666676]
step: 40 time: 0.3333333333333333 speed: [0. 0. 0.32500008]
step: 41 time: 0.3416666666666667 speed: [0. 0. 0.3333334]
step: 42 time: 0.35 speed: [0. 0. 0.34166672]
step: 43 time: 0.35833333333333334 speed: [0. 0. 0.35000004]
step: 44 time: 0.36666666666666664 speed: [0. 0. 0.35833336]
step: 45 time: 0.375 speed: [0. 0. 0.36666668]
step: 46 time: 0.38333333333333336 speed: [0. 0. 0.375]
step: 47 time: 0.39166666666666666 speed: [0. 0. 0.38333332]
step: 48 time: 0.4 speed: [0. 0. 0.39166664]
step: 49 time: 0.4083333333333333 speed: [0. 0. 0.39999996]
step: 50 time: 0.4166666666666667 speed: [0. 0. 0.40833328]
step: 51 time: 0.425 speed: [0. 0. 0.4166666]
step: 52 time: 0.43333333333333335 speed: [0. 0. 0.42499992]
step: 53 time: 0.44166666666666665 speed: [0. 0. 0.43333324]
step: 54 time: 0.45 speed: [0. 0. 0.44166656]
step: 55 time: 0.4583333333333333 speed: [0. 0. 0.44999989]
step: 56 time: 0.4666666666666667 speed: [0. 0. 0.45833321]
step: 57 time: 0.475 speed: [0. 0. 0.46666653]
step: 58 time: 0.48333333333333334 speed: [0. 0. 0.47499985]
step: 59 time: 0.49166666666666664 speed: [0. 0. 0.48333317]
step: 60 time: 0.5 speed: [0. 0. 0.49166649]
step: 61 time: 0.5083333333333333 speed: [0. 0. 0.49999981]
step: 62 time: 0.5166666666666667 speed: [0. 0. 0.50833313]
step: 63 time: 0.525 speed: [0. 0. 0.51666645]
step: 64 time: 0.5333333333333333 speed: [0. 0. 0.52499977]
step: 65 time: 0.5416666666666666 speed: [0. 0. 0.53333309]
step: 66 time: 0.55 speed: [0. 0. 0.54166641]
step: 67 time: 0.5583333333333333 speed: [0. 0. 0.54999973]
step: 68 time: 0.5666666666666667 speed: [0. 0. 0.55833305]
step: 69 time: 0.575 speed: [0. 0. 0.56666637]
step: 70 time: 0.5833333333333334 speed: [0. 0. 0.57499969]
step: 71 time: 0.5916666666666667 speed: [0. 0. 0.58333302]
step: 72 time: 0.6 speed: [0. 0. 0.59166634]
step: 73 time: 0.6083333333333333 speed: [0. 0. 0.59999966]
step: 74 time: 0.6166666666666667 speed: [0. 0. 0.60833298]
step: 75 time: 0.625 speed: [0. 0. 0.6166663]
step: 76 time: 0.6333333333333333 speed: [0. 0. 0.62499962]
step: 77 time: 0.6416666666666667 speed: [0. 0. 0.63333294]
step: 78 time: 0.65 speed: [0. 0. 0.64166626]
step: 79 time: 0.6583333333333333 speed: [0. 0. 0.64999962]
step: 80 time: 0.6666666666666666 speed: [0. 0. 0.65833298]
step: 81 time: 0.675 speed: [0. 0. 0.66666634]
step: 82 time: 0.6833333333333333 speed: [0. 0. 0.67499969]
step: 83 time: 0.6916666666666667 speed: [0. 0. 0.68333305]
step: 84 time: 0.7 speed: [0. 0. 0.69166641]
step: 85 time: 0.7083333333333334 speed: [0. 0. 0.69999977]
step: 86 time: 0.7166666666666667 speed: [0. 0. 0.70833313]
step: 87 time: 0.725 speed: [0. 0. 0.71666649]
step: 88 time: 0.7333333333333333 speed: [0. 0. 0.72499985]
step: 89 time: 0.7416666666666667 speed: [0. 0. 0.73333321]
step: 90 time: 0.75 speed: [0. 0. 0.74166656]
step: 91 time: 0.7583333333333333 speed: [0. 0. 0.74999992]
step: 92 time: 0.7666666666666667 speed: [0. 0. 0.75833328]
step: 93 time: 0.775 speed: [0. 0. 0.76666664]
step: 94 time: 0.7833333333333333 speed: [0. 0. 0.775]
step: 95 time: 0.7916666666666666 speed: [0. 0. 0.78333336]
step: 96 time: 0.8 speed: [0. 0. 0.79166672]
step: 97 time: 0.8083333333333333 speed: [0. 0. 0.80000008]
step: 98 time: 0.8166666666666667 speed: [0. 0. 0.80833344]
step: 99 time: 0.825 speed: [0. 0. 0.81666679]
step: 100 time: 0.8333333333333334 speed: [0. 0. 0.82500015]
step: 101 time: 0.8416666666666667 speed: [0. 0. 0.83333351]
step: 102 time: 0.85 speed: [0. 0. 0.84166687]
step: 103 time: 0.8583333333333333 speed: [0. 0. 0.85000023]
step: 104 time: 0.8666666666666667 speed: [0. 0. 0.85833359]
step: 105 time: 0.875 speed: [0. 0. 0.86666695]
step: 106 time: 0.8833333333333333 speed: [0. 0. 0.87500031]
step: 107 time: 0.8916666666666667 speed: [0. 0. 0.88333366]
step: 108 time: 0.9 speed: [0. 0. 0.89166702]
step: 109 time: 0.9083333333333333 speed: [0. 0. 0.90000038]
step: 110 time: 0.9166666666666666 speed: [0. 0. 0.90833374]
step: 111 time: 0.925 speed: [0. 0. 0.9166671]
step: 112 time: 0.9333333333333333 speed: [0. 0. 0.92500046]
step: 113 time: 0.9416666666666667 speed: [0. 0. 0.93333382]
step: 114 time: 0.95 speed: [0. 0. 0.94166718]
step: 115 time: 0.9583333333333334 speed: [0. 0. 0.95000053]
step: 116 time: 0.9666666666666667 speed: [0. 0. 0.95833389]
step: 117 time: 0.975 speed: [0. 0. 0.96666725]
step: 118 time: 0.9833333333333333 speed: [0. 0. 0.97500061]
step: 119 time: 0.9916666666666667 speed: [0. 0. 0.98333397]
step: 120 time: 1.0 speed: [0. 0. 0.99166733]
step: 121 time: 1.0083333333333333 speed: [0. 0. 1.00000069]
I am asking because the plugin I am writing to simulate hydrodynamics was behaving weirdly before I started multiplying my gains by 100 for forces, 10000 for torques, and 10000 for the inertia matrices. Without these gains, the simulation was highly unstable. With them, it’s not exactly like the original gazebo plugin, but it’s close.
