Questions regarding dynamic_control_.apply_body_force

Hey all,

I am trying to implement boats in Isaac. To do so I need to apply torques to objects. However, as far as I am aware, Isaac does not offer any means to apply torques directly. Alternatively, one can apply torque to an object by applying a force at one meter from the Center of Mass of the object. Hence I tried to use the apply_body_force method from the dynamic control extension to apply torque to my objects. However, when I try to apply torques to some cubes nothing happens. It is as if the force where applied at the center of the object and not the desired coordinates. Is there something I am doing wrong?

Please find bellow the code we use to test this behavior.

Best,

Antoine

import os
import omni
import carb
from pxr import Gf
from omni.isaac.python_app import OmniKitHelper

CONFIG = {
    "experience": f'{os.environ["EXP_PATH"]}/omni.isaac.sim.python.kit',
    "renderer": "RayTracedLighting",
    "headless": False,
}

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)
    import utils
    from omni.isaac.dynamic_control import _dynamic_control

    nucleus_server = utils.get_nucleus_server()
    asset_path = nucleus_server + "/Isaac/Props/Blocks/nvidia_cube.usd"

    stage = omni.usd.get_context().get_stage()
    utils.setup_cpu_physics(stage, "/World/physicsScene")
    utils.create_ground_plane(stage, "/World/ground_plane", size=100)

    dc = _dynamic_control.acquire_dynamic_control_interface()

    def physics_update(dt: float):
        rigid_body = dc.get_rigid_body(prims[-1])
        dc.apply_body_force(rigid_body, [0,0,981.1],[0,0,0])
        dc.apply_body_force(rigid_body, carb._carb.Float3(1000,0,0),carb._carb.Float3(0,100,35))
        dc.apply_body_force(rigid_body, carb._carb.Float3(-1000,0,0),carb._carb.Float3(0,-100,35))

    physics_sub = omni.physx.acquire_physx_interface().subscribe_physics_step_events(physics_update)

    prims = []

    position = Gf.Vec3d(0, 0, 35)
    prims = utils.createObject('/World/nvidia_cube',stage,asset_path,False,position=position, group=prims, density=1, scale=Gf.Vec3d(3,3,3))

    omni.usd.get_context().save_as_stage(nucleus_server + "/Users/test/saved.usd", None)
    while omniverse_kit.app.is_running():
        omniverse_kit.update(1.0/30.0)
    
    omniverse_kit.stop()
    omniverse_kit.shutdown()

Here is our utils file

import omni
import carb
from numpy import sin, cos
from pxr import UsdGeom, Gf, Sdf, UsdPhysics, UsdShade
from omni.isaac.utils.scripts.nucleus_utils import find_nucleus_server
from omni.physx.scripts import utils

def CreateBasicMaterial(stage):
    mtl_created_list = []
    omni.kit.commands.execute(
        "CreateAndBindMdlMaterialFromLibrary",
        mdl_name="OmniPBR.mdl",
        mtl_name="OmniPBR",
        mtl_created_list=mtl_created_list,
    )
    mtl_prim = stage.GetPrimAtPath(mtl_created_list[0])
    material = UsdShade.Material(mtl_prim)
    return material

def Euler2Quat(ypr):
    cypr = cos(ypr*0.5); sypr = sin(ypr*0.5)
    qw = cypr[0]*cypr[1]*cypr[1] + sypr[0]*sypr[1]*sypr[2]
    qx = sypr[0]*cypr[1]*cypr[1] - cypr[0]*sypr[1]*sypr[2]
    qy = cypr[0]*sypr[1]*cypr[1] + sypr[0]*cypr[1]*sypr[2]
    qz = cypr[0]*cypr[1]*sypr[1] - sypr[0]*sypr[1]*cypr[2]
    return Gf.Quaternion(qw, Gf.Vec3d(qx,qy,qz))

def createObject(prefix, stage, path, material, position=Gf.Vec3d(0, 0, 0), rotation=Gf.Quaternion(1, Gf.Vec3d(0,0,0)), group=[], allow_physics=True, density=1000, scale=Gf.Vec3d(1.0,1.0,1.0), is_instance=False):
    """! Creates a 3D object from a USD file and adds it to the stage

    @type prefix: str
    @param prefix: The name of the object in the world (does not have to be unique).
    @type stage: pxr.UsdStage
    @param stage: The name of the stage the objects belong to.
    @type path: str
    @param path: The path of to the USD file on the docker/drive.
    @type position: pxr.Gf.Vec3d
    @param position: The position of the object relatively to the stage.
    @type rotation: pxr.Gf.Quaternion
    @param rotation: The rotation of the object relatively to the stage.
    @type group: list
    @param group: The group of primitives this object belongs to.
    @type allow_physics: bool
    @param allow_physics: Allows physics if set to True.
    @type density: float
    @param density: The density of the object
    @type scale: pxr.Gf.Vec3d
    @param scale: The scale of the object in X,Y, and Z.

    @rtype: list
    @return: The group of primitives this object belongs to.
    """
    prim_path = omni.usd.get_stage_next_free_path(stage, prefix, False)
    group.append(prim_path)
    obj_prim = stage.DefinePrim(prim_path, "Xform")
    obj_prim.GetReferences().AddReference(path)
    if is_instance:
        obj_prim.SetInstanceable(True)
    xform = UsdGeom.Xformable(obj_prim)

    xform = setScale(xform, scale)
    xform = setTransform(xform, rotation, position)
    
    if material:
        UsdShade.MaterialBindingAPI(obj_prim).Bind(material, UsdShade.Tokens.strongerThanDescendants)

    if allow_physics:
        utils.setRigidBody(obj_prim, "convexHull", False)
        mass_api = UsdPhysics.MassAPI.Apply(obj_prim)
        mass_api.CreateMassAttr(density)
    return group

def setScale(xform, scale):
    """! Set the scale of an object

    @type xform: pxr.UsdGeomXform
    @param xform: the Xform related to the object.
    @type scale: pxr.Gf.Vec3d
    @param scale: The scaling factor to be applied onto the object.
    
    @rtype: pxr.UsdGeomXform
    @return: the Xform related to the object.
    """
    scale_op = None
    for xformOp in xform.GetOrderedXformOps():
        if xformOp.GetOpType() == UsdGeom.XformOp.TypeScale:
            scale_op = xformOp
    if scale_op:
        xform_op = scale_op
    else:
        xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeScale, UsdGeom.XformOp.PrecisionDouble, "")
    xform_op.Set(scale)
    return xform

def setTranslate(xform, translation):
    """! Translates an object

    @type xform: pxr.UsdGeomXform
    @param xform: the Xform related to the object.
    @type translation: pxr.Gf.Vec3d
    @param translation: The translation to be applied onto the object.
    
    @rtype: pxr.UsdGeomXform
    @return: the Xform related to the object.
    """
    translate_op = None
    for xformOp in xform.GetOrderedXformOps():
        if xformOp.GetOpType() == UsdGeom.XformOp.TypeTranslate:
            translate_op = xformOp
    if translate_op:
        xform_op = translate_op
    else:
        xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeTranslate, UsdGeom.XformOp.PrecisionDouble, "")
    xform_op.Set(translation)
    return xform

def setRotateXYZ(xform, rotation):
    """! Rotates an object using the XYZ convention

    @type xform: pxr.UsdGeomXform
    @param xform: the Xform related to the object.
    @type rotation: pxr.Gf.Vec3d
    @param rotation: The rotation to be applied onto the object.
    
    @rtype: pxr.UsdGeomXform
    @return: the Xform related to the object.
    """
    rotation_op = None
    for xformOp in xform.GetOrderedXformOps():
        if xformOp.GetOpType() == UsdGeom.XformOp.TypeRotationXYZ:
            rotation_op = xformOp
    if rotation_op:
        xform_op = rotation_op
    else:
        xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeRotationXYZ, UsdGeom.XformOp.PrecisionDouble, "")
    xform_op.Set(rotation)
    return xform

def setTransform(xform, rotation, position):
    """! Applies a transformation matrix onto an object

    @type xform: pxr.UsdGeomXform
    @param xform: the Xform related to the object.
    @type rotation: pxr.Gf.Quaternion
    @param rotation: The rotation to be applied onto the object.
    @type position: pxr.Gf.Vec3d
    @param position: The rotation to be applied onto the object.
    
    @rtype: pxr.UsdGeomXform
    @return: the Xform related to the object.
    """

    mat = Gf.Matrix4d().SetTranslate(position)
    mat.SetRotateOnly(Gf.Rotation(rotation))

    transform_op = None
    for xformOp in xform.GetOrderedXformOps():
        if xformOp.GetOpType() == UsdGeom.XformOp.TypeTransform:
            transform_op = xformOp
    if transform_op:
        xform_op = transform_op
    else:
        xform_op = xform.AddXformOp(UsdGeom.XformOp.TypeTransform, UsdGeom.XformOp.PrecisionDouble, "")
    xform_op.Set(mat)
    return xform


def create_ground_plane(stage, plane_name, size=10, up_direction="Z", location=Gf.Vec3f(0,0,0), unknown=Gf.Vec3f(1.0), visible=True):
    from pxr import PhysicsSchemaTools, PhysxSchema
    """
    Creates a ground plane.
    Inputs:
        stage: The name of the world the objects belong to.
        plane_name: The name of the ground plane.
        size: The size of the ground plane in meters.
        up_direction: The direction normal to the plane.
        location: The position of the plane.
        unknown: An unknown parameter.
        visible: Should the ground plane be visible.
    """
    PhysicsSchemaTools.addGroundPlane(stage, plane_name, up_direction, size*10, location, unknown)
    if not visible:
        imageable = UsdGeom.Imageable(stage.GetPrimAtPath(plane_name))
        if imageable:
            imageable.MakeInvisible()

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

Hi Antoine,
I attempt to run your code and the apply_body_force seems to be applying the forces. Are you not getting any movement from the cube or not the intended movement?

Make sure to click on “Play” to enable the physics simulation, otherwise you won’t get to the physics_update method. You can either click on “Play” in the UI or do it programmatically with omniverse_kit.play() (in your code).
Once I do this, I start seeing the cube being moved.

I think the Physics Inspection (Isaac Utils → Inspect Physics) tool can be helpful to inspect what is going on with your cube as well. You can check its Position, Mass, Velocities, etc
To further debug parts of your code (like force intensity, position, effects of mass) you can also just run parts of it with the Script Editor (Window → Script Editor) on-demand vs having to re-run the script to change the physics_update method all the time.

Hi Teresa,

I am getting movement, but not the one expected. I’ll double check with the Physics Inspection tool. In my simulation the cube slowly lifts up, but is not rotating around z when it should be rotating around z.

So this is being applied:

dc.apply_body_force(rigid_body, [0,0,981.1],[0,0,0])

But it looks like these are cancelling each other as if they were applied at the same point:

dc.apply_body_force(rigid_body, carb._carb.Float3(1000,0,0),carb._carb.Float3(0,100,35))
dc.apply_body_force(rigid_body, carb._carb.Float3(-1000,0,0),carb._carb.Float3(0,-100,35))

Here is what it should look like:

Update, using the Physx API, the object has the expected behavior:

    def physics_update(dt: float):
        physxIFace = get_physx_interface()
        physxIFace.apply_force_at_pos(prims[-1], carb._carb.Float3([0,0,1000]),carb._carb.Float3([0,0,0]))
        physxIFace.apply_force_at_pos(prims[-1], carb._carb.Float3([1000,0,0]),carb._carb.Float3([0,100,0]))
        physxIFace.apply_force_at_pos(prims[-1], carb._carb.Float3([-1000,0,0]),carb._carb.Float3([0,-100,0]))
1 Like