Objects connected through fixed joints change orientation after simulation start

[axel.goedrich]

Hello,

I am currently writing an extension, that assembles a robot (robot + gripper + gripper fingers etc.) based on a configuration file.

The objects are defined as Xform Prims and are oriented and positioned using the following functions:

obj = world.scene.add(XFormPrim(scene_path, name = object_name))
obj.set_world_pose(new_pos, new_ori)
obj.set_default_state(nem_pos, new_ori)

I then connect the appropriate objects using fixed joints (not the objects directly, but the apropriate prims of the objects, for example the tool0 xform of the UR robot, with the main body of the gripper):

def create_joint(prim_path_1, prim_path_2, joint_path):
    stage = omni.usd.get_context().get_stage()
    joint = UsdPhysics.FixedJoint.Define(stage, joint_path)

    # define joint bodies
    joint.CreateBody0Rel().SetTargets(prim_path_1)
    joint.CreateBody1Rel().SetTargets(prim_path_2)

    # define joint positions
    joint.CreateLocalPos0Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
    joint.CreateLocalPos1Attr().Set(-Gf.Vec3f(0.0, 0.0, 0.0))
    joint.CreateLocalRot0Attr().Set(Gf.Quatf(1.0))
    joint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

But when the robot is loaded, the object orientations and positions initially do not match with the orientations and positions, specified in the code (although they still match directly after creating the joints). When I then start the simulation and stop it, the objects snap back to the positions and orientations specified in the code. When I then run the simulation, the objects change again. You can see the described behavior in this video:

Does anyone know what the cause of this may be?

[arudich]

The fixed joint is completely inactive while the timeline is stopped. You can move around the attached bodies relative to each other, and there will be no problem. Then when you click PLAY, the bodies will snap back together to satisfy the constraint.

The behavior of pressing STOP is for the asset to go back to the position that is written into its USD file (the position it would be loaded with). You tried to override the default position to be something else with obj.set_default_state() . That default state you are setting is only going to be used if you use world.reset() to control the timeline . It won’t be used if the STOP button is pressed.

If this is in an extension workflow, you can add a callback function for if the STOP timeline event happens and set the pose there.

[axel.goedrich]

That makes sense. But what I don’t understand, is why the object positions and/or orientations change at all.

I position the objects with the before mentioned functions and when I afterwards check the current position with the following code, they are at the specified position:

matrix: Gf.Matrix4d = omni.usd.get_world_transform_matrix(prim)
translate: Gf.Vec3d = matrix.ExtractTranslation()
rotate: Gf.Vec3d = matrix.ExtractRotationQuat()

After that, I create the fixed joints and don’t move the objects afterward or anything like that.

What I would expect for the fixed joints is to hold the relative positions that the objects had, right before I created the fixed joints. But instead, the position is different.

Do I have to initialize the joints differently or position the objects in a different way?

[vic-chen]

Hi @axel.goedrich ,
Try this,

def createJoint(self,stage, joint_type, from_prim, to_prim):
    # for single selection use to_prim
    if to_prim is None:
        to_prim = from_prim
        from_prim = None

    from_path = from_prim.GetPath().pathString if from_prim is not None and from_prim.IsValid() else ""
    to_path = to_prim.GetPath().pathString if to_prim is not None and to_prim.IsValid() else ""
    single_selection = from_path == "" or to_path == ""

    # to_path can be not writable as in case of instancing, find first writable path
    joint_base_path = to_path
    base_prim = stage.GetPrimAtPath(joint_base_path)
    while base_prim != stage.GetPseudoRoot():
        if base_prim.IsInMaster():
            base_prim = base_prim.GetParent()
        elif base_prim.IsInstanceProxy():
            base_prim = base_prim.GetParent()
        elif base_prim.IsInstanceable():
            base_prim = base_prim.GetParent()
        else:
            break
    joint_base_path = str(base_prim.GetPrimPath())
    if joint_base_path == '/':
        joint_base_path = ''

    #joint_name = "/" + create_unused_path(stage, joint_base_path, joint_type + "Joint")
    print("joint base path:",joint_base_path)
    joint_name = "/" + joint_type + "Joint"
    joint_path = joint_base_path + joint_name
    print("Joint path:",joint_path)
    if joint_type == "Fixed":
        component = UsdPhysics.FixedJoint.Define(stage, joint_path)
    elif joint_type == "Revolute":
        component = UsdPhysics.RevoluteJoint.Define(stage, joint_path)
        component.CreateAxisAttr("X")
    elif joint_type == "Prismatic":
        component = UsdPhysics.PrismaticJoint.Define(stage, joint_path)
        component.CreateAxisAttr("X")
    elif joint_type == "Spherical":
        component = UsdPhysics.SphericalJoint.Define(stage, joint_path)
        component.CreateAxisAttr("X")
    elif joint_type == "Distance":
        component = UsdPhysics.DistanceJoint.Define(stage, joint_path)
        component.CreateMinDistanceAttr(0.0)
        component.CreateMaxDistanceAttr(0.0)
    elif joint_type == "Gear":
        component = PhysxSchema.PhysxPhysicsGearJoint.Define(stage, joint_path)
    elif joint_type == "RackAndPinion":
        component = PhysxSchema.PhysxPhysicsRackAndPinionJoint.Define(stage, joint_path)
    else:
        component = UsdPhysics.Joint.Define(stage, joint_path)
        prim = component.GetPrim()
        for limit_name in ["transX", "transY", "transZ", "rotX", "rotY", "rotZ"]:
            limit_api = UsdPhysics.LimitAPI.Apply(prim, limit_name)
            limit_api.CreateLowAttr(1.0)
            limit_api.CreateHighAttr(-1.0)

    xfCache = UsdGeom.XformCache()

    if not single_selection:
        to_pose = xfCache.GetLocalToWorldTransform(to_prim)
        from_pose = xfCache.GetLocalToWorldTransform(from_prim)
        rel_pose = to_pose * from_pose.GetInverse()
        rel_pose = rel_pose.RemoveScaleShear()
        pos1 = Gf.Vec3f(rel_pose.ExtractTranslation())
        rot1 = Gf.Quatf(rel_pose.ExtractRotationQuat())

        component.CreateBody0Rel().SetTargets([Sdf.Path(from_path)])
        component.CreateBody1Rel().SetTargets([Sdf.Path(to_path)])
        component.CreateLocalPos0Attr().Set(pos1)
        component.CreateLocalRot0Attr().Set(rot1)
        component.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0))
        component.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))
    else:
        to_pose = xfCache.GetLocalToWorldTransform(to_prim)
        to_pose = to_pose.RemoveScaleShear()
        pos1 = Gf.Vec3f(to_pose.ExtractTranslation())
        rot1 = Gf.Quatf(to_pose.ExtractRotationQuat())

        component.CreateBody1Rel().SetTargets([Sdf.Path(to_path)])
        component.CreateLocalPos0Attr().Set(pos1)
        component.CreateLocalRot0Attr().Set(rot1)
        component.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0))
        component.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

    #component.CreateBreakForceAttr().Set(100000.0)
    #component.CreateBreakTorqueAttr().Set(100000.0)

    return stage.GetPrimAtPath(joint_base_path + joint_name)

[axel.goedrich]

Hi @vic-chen,
thank you very much, this solved the problem!