Joints#

Joints govern the relative motion of pairs of rigid bodies. For example, a hinge joint that permits a car door to swing open and shut, but holds the car and the door with a fixed relationship in every other way.

A joints couples a dynamic rigid body to:

  • another dynamic rigid body

  • a kinematic rigid body (see section Kinematic Rigid Body for dynamic and kinematic rigid bodies)

  • a static coordinate frame

If two jointed rigid bodies are part of the same articulation, the joint is instantiated as an articulation joint. Articulation joints are more restrictive in their features than regular joints but offer more precision. See Articulations and Articulation Limitations and Differences for a detailed discussion on the limitations of articulation joints.

Meta Joints control the relative behavior of two joints.

A video introduction to joints may be found here: Rigging a Desk Lamp.

Joint Frames#

Joints involve poses (each composed of a position and a rotation), in a variety of coordinate frames. Let body0 and body1 be two bodies referenced by a joint. body0 and body1 have a pose in the world frame. Let those poses be referred to as G_0 and G_1 respectively. A joint will associate an additional pose with each referenced body. Let those poses be called L_0 and L_1 respectively. L_0 and L_1 are poses in the local frame of body0 and body1 respectively. The joint itself develops a pose J arising from its position along and rotation around the three axes (X, Y, Z). Each rigid body referenced by a joint has:

  • a pose in the world frame.

  • an associated local pose specified in the rigid body’s frame.

The joint itself develops a pose arising from its position along and rotation around the three axes (X, Y, Z).

Each joint enforces the rule:

G0L0J=G1L1

Where G0, G1 are the pose of body0 and body1 in the world frame; L0, L1 are joint local frames associated with body0 and body1, and J is the joint pose arising from joint positions along (X, Y, Z) and joint angles around (X, Y, Z). The poses L0, L1 may not be modified by the simulation and are therefore constant across each simulation step.

If the joint connects to a static frame instead of a rigid body, then the corresponding frame G0 or G1 is constant across each simulation step.

The solver aims to produce body and joint velocities that satisfy the joint equation, while also enforcing the rules applied to J that are particular to each joint type. For example, if body1 is coupled to the world frame with a fixed joint, then the solver attempts to produce a velocity on body1 such that its pose :math:G_1 after integration satisfies the rule G1=G0L0L11.

Rules on the Form of J#

Each joint type enforces a different rule on the form of J. For example:

  • A fixed joint permits J to be only the identity pose; that is, the pose that has the position (0, 0, 0) and identity rotation.

  • A prismatic joint enforces the rule that J has identity rotation and allows a non-zero position along a single axis.

  • The revolute joint type is the complement of the prismatic joint in that it enforces a linear position of (0, 0, 0), but limits rotation around a single axis.

  • The spherical joint enforces the rule that J has position (0, 0, 0), but allows for any rotation.

For more information on the different joint types and the rules they impose on the form of J, see section Joint Types.

Joint Drive#

Joint drives apply a force to the joint in order to maintain a position and/or a velocity. The equation governing the joint drive force is as follows:

driveForce=stiffness(positiontargetPosition)+damping(velocitytargetVelocity)

Joint drives may be configured as pure position drive by setting stiffness non-zero and setting damping to zero. Joint drives may be configured as pure velocity drive by setting stiffness to zero and damping non-zero.

Damped harmonic motion may be achieved by setting both stiffness and damping to non-zero and selecting a target velocity of zero.

Note

Rigid bodies are subject to a variety of forces that will influence joint velocity and there is no guarantee that the drive will achieve its target unless the stiffness and/or damping values are chosen so that the drive force dominates all other forces acting on the jointed body pair.

Force and Acceleration Drives#

Drives may be configured as force or acceleration drives.

Consider a drive that falls along a single linear axis with the joint coupling a dynamic rigid body to the world frame. If a force drive is selected, the acceleration arising from the drive force will be drive_force/mass with mass denoting the mass of the dynamic rigid body. Acceleration drive produces the outcome as if the dynamic rigid body had unit mass.

For more examples of force and acceleration drives, see sections Prismatic Joint and Revolute Joint.

Mathematically, the effective inertia of the joint is a function of body masses, inertias, joint frames, and relative pose. Force drives require inertia to translate joint force to joint acceleration. For a single degree of freedom, it is possible to compute an effective inertia of the joint by considering the outcome of a unit impulse applied to that degree of freedom. The effective inertia of the joint is the reciprocal of the change in joint velocity arising from the unit impulse.

Section D6 Joint introduces soft limits modelled as springs that drive the joint from a position that violates the limit back towards the limit.

Joint Limit#

Joints may be configured with limits that impose an upper and lower bound on angular or linear motion. Limits are applied to the joint pose J introduced in section Joint Frames. For example, a prismatic joint that permits motion along the y-axis only, could be limited so that J must have a position in-between (0, lowerLimit, 0) and (0, upperLimit, 0). Limits may also be applied to the rotation of J in the same way. More examples of joint limit are in sections Prismatic Joint and Revolute Joint.

Disabling Joints#

Joints may be disabled by removing the joint from the stage or by specifying a breaking force attribute that effectively disables the joint if the joint exerts a force greater than a specified threshold. An example of the breaking force attribute can be found in section Revolute Joint.

Note

The ability to remove or disable joints during simulation is limited for articulation joints. The breakForce attribute of UsdPhysics.Joint is, for example, ignored for articulation joints. See Articulation Limitations and Differences.

Joint Types#

Joints implement many different relationships between pairs of rigid bodies. Table Joint Categories illustrates different types of supported relationships and their corresponding joint types. Each joint type imposes its own rule on the joint pose J introduced in section Joint Frames.

Type

Description

D6 Joint

The D6 joint can be configured to allow from zero up to six degrees of freedom for relative motion - three translational and three rotational.

Distance Joint

The distance joint allows any relative motion but limits the distance between the connected rigid bodies.

Fixed Joint

The fixed joint allows no relative motion. It is particularly useful when creating fixed base articulations (see section Create a Fixed-Base Articulation).

Prismatic Joint

The prismatic joint allows for linear motion along a single axis.

Revolute Joint

The revolute joint allows for rotation about a single axis.

Spherical Joint

The spherical joint allows for rotation about three axes; equivalent to a ball-and-socket joint.

Table Joint Support table sets out the types of coupling supported by regular joints and articulation joints on CPU and GPU.

Type

Joint Support (cpu/gpu)

Articulation Joint Support (cpu/gpu)

D6 Joint

yes/yes

yes/yes (provided all linear axes are locked)

Distance Joint

yes/no

no/no

Fixed Joint

yes/yes

yes/yes

Prismatic Joint

yes/yes

yes/yes

Revolute Joint

yes/yes

yes/yes

Spherical Joint

yes/yes

yes/yes

D6 Joint#

The D6 joint allows each axis of the position and rotation of J to be configured as free, limited or locked. A D6 joint may, for example, be configured as a revolute joint or as a prismatic joint. A wide variety of outcomes are possible.

The D6 joint does not support limit cones, instead each pair of angular limits forms a pyramid. See section Spherical Joint for more information on the limit cone arising from angular limits applied to two swing axes.

Note

Special rules apply for which degrees of freedom can be restricted in the case of articulation joints, see Articulation Limitations and Differences.

The following Python snippet serves as an introduction to the D6 joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]
d6JointPath = "/World/d6Joint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

for i in range(2):

    # Create the rigid body
    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)

# Create a D6 joint between the two prims
d6Joint = UsdPhysics.Joint.Define(stage, d6JointPath)
d6Joint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
d6Joint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])
d6JointPrim = d6Joint.GetPrim()

# Configure the local frames of the joint.
d6Joint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
d6Joint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
d6Joint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
d6Joint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

# Configure the joint as a prismatic joint with limited motion along the y-axis.
# All axes except for transY will be locked.
# Axis transY will be limited and driven.
lockedAxes = [
    UsdPhysics.Tokens.transX, UsdPhysics.Tokens.transZ,
    UsdPhysics.Tokens.rotX, UsdPhysics.Tokens.rotY, UsdPhysics.Tokens.rotZ
]
limitAxis = UsdPhysics.Tokens.transY
for i in range(5):
    limitAPI = UsdPhysics.LimitAPI.Apply(d6JointPrim, lockedAxes[i])
    limitAPI.CreateLowAttr(1.0)
    limitAPI.CreateHighAttr(-1.0)
# Axis transY will be limited.
limitAPI = UsdPhysics.LimitAPI.Apply(d6JointPrim, limitAxis)
limitAPI.CreateLowAttr(-5.0)
limitAPI.CreateHighAttr(5.0)
# Drive along the y-axis to a target velocity.
linearDriveAPI = UsdPhysics.DriveAPI.Apply(d6JointPrim, limitAxis)
linearDriveAPI.CreateTypeAttr(UsdPhysics.Tokens.acceleration)
linearDriveAPI.CreateStiffnessAttr(0.0)
linearDriveAPI.CreateTargetPositionAttr(0.0)
linearDriveAPI.CreateTargetVelocityAttr(10.0)
linearDriveAPI.CreateDampingAttr(1000.0)

The Python snippet couples a dynamic and kinematic rigid body using a joint that permits linear motion along a single axis. With reference to the joint equation set out in section Joint Frames, the joint local frames L0, L1 are stored in the arrays jointLocalPositions and jointLocalRotations. The scene begins with body poses G0, G1 having the values stored in the arrays rigidBodyStartPositions and rigidBodyStartRotations. It may be verified that the initial body poses and the joint local frames do not satisfy the joint equation for a prismatic joint. It is the job of the solver to enforce the joint equation with J supporting linear motion along only a single axis.

You can set a limit per joint axis. The limit restricts the joint movement between a low and a high value along that axis. Each axis of the D6 joint may be locked by creating a limit such that the lower limit is higher than the upper limit. Setting the lower limit to a smaller value than the upper limit will result in limited motion along the corresponding axis.

Drives are specified per axis. Stiffness and damping values may be individually specified for each linear axis. For the angular axes UsdPhysics.Tokens.rotY and UsdPhysics.Tokens.rotZ, it is recommended to use the same pair of stiffness/damping values, because setting different values results in undefined behavior. Axis UsdPhysics.Tokens.rotX can be configured with its own individual stiffness and damping values.

The spring parameters for an angular drive around the y and z axis have to be the same. For the other axis you can have, for example, a very soft spring that slowly gets you to the x-coordinate of your target position, but at the same time you can have a stiff spring that gets you quickly to the y-coordinate of your target position. For angular (rotational) drive around y and z you need to use the “same” spring.

It is possible to extend the feature set of a limit by applying PhysxSchema.PhysxLimitAPI. For example, the following snippet introduces a bounce to the limit, so that the rigid body bounces off the limit rather than experiencing a hard stop at the limit. The bounce has a restitution of 1.0 when joint velocity along the specified axis is greater than 0.1 distance units per second:

physxLimitAPI = PhysxSchema.PhysxLimitAPI.Apply(d6JointPrim, limitAxis);
physxLimitAPI.CreateRestitutionAttr(1.0)
physxLimitAPI.CreateBounceThresholdAttr(0.1)

PhysxSchema.PhysxLimitAPI may also be used to configure the joint as a soft limit, by specifying stiffness and damping attributes that take effect for as long as the limit is violated:

physxLimitAPI.CreateStiffnessAttr(100.0)
physxLimitAPI.CreateDampingAttr(10.0)

The stiffness and damping attributes work as outlined in the spring equation in section Joint Drive, but with targetPosition now denoting the position of the joint at the limit. Moreover, the value of targetVelocity will be 0.0 for a hard limit in the event that the restitution attribute is 0.0 or if the joint velocity along the specified axis is less than the value of the bounce threshold attribute. If the bounce threshold attribute is specified as non-zero and the joint velocity exceeds the value of the bounce threshold attribute, then :targetVelocity will account for the restitution.

Note

In the context of articulations, PhysxSchema.PhysxLimitAPI is not supported unless the joint is marked as excluded from the articulation, see Articulation Limitations and Differences.

Distance Limit#

Another feature of D6 joints is the distance limit. The snippet above may be amended to apply a distance limit as follows:

limitAPI = UsdPhysics.LimitAPI.Apply(d6JointPrim, UsdPhysics.Tokens.distance)
limitAPI.CreateHighAttr(5.0)

The distance limit applies an upper limit to the distance between the origins of the two constraint frames G0L0 and G1L1, as defined in section Joint Frames. The distance is computed only along axes that have limited or free motion. Depending on the number of axes involved in the distance computation, the outcome is linear, circular, or spherical motion. The behavior is similar to the distance joint (see section Distance Joint), however the D6 joint does not support a lower bound on the computed distance.

Distance Joint#

The distance joint sets an upper and lower bound on the distance between the origins of the two constraint frames G0L0 and G1L1, as defined in section Joint Frames.

Note

In the context of articulations, distance joints are not supported, unless marked as excluded from the articulation, see Joint Support.

The following Python snippet serves as an introduction to the distance joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]
distanceJointPath = "/path/to/distanceJoint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

for i in range(2):

    # Create the rigid dynamic body
    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)

# Create a distance joint between the two prims
distanceJoint = UsdPhysics.DistanceJoint.Define(stage, distanceJointPath)
distanceJoint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
distanceJoint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])
distanceJointPrim = distanceJoint.GetPrim()

# Configure the local frames of the joint.
distanceJoint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
distanceJoint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
distanceJoint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
distanceJoint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

distanceJoint.CreateMinDistanceAttr(10)
distanceJoint.CreateMaxDistanceAttr(50)

The snippet creates a distance joint that couples a kinematic rigid body to a dynamic rigid body. The joint is configured so that the distance between the origins of the two constraint frames G0L0 and G1L1 must be in between 10 and 50 distance units. It is possible to create only a minimum (or maximum) distance requirement by specifying either minimum (or maximum) distance alone. If neither is specified, the solver will have no rule to enforce and the two bodies of the joint will be free to have any relative translation. It is not recommended to set the minimum distance to be greater than the maximum distance. This will result in undefined behavior.

The snippet implements limits as hard constraints.

Soft Constraints#

Limits do not have to abruptly stop or invert joint motion. They can also be configured as soft constraints using spring attributes that drive the joint back towards the limit. The following addition will configure the limits as soft constraints:

physxDistanceJointAPI = PhysxSchema.PhysxPhysicsDistanceJointAPI.Apply(
    distanceJointPrim
)
physxDistanceJointAPI.CreateSpringEnabledAttr(True)
physxDistanceJointAPI.CreateSpringStiffnessAttr(10.0)
physxDistanceJointAPI.CreateSpringDampingAttr(10.0)

The stiffness and damping attributes introduced, above, will take effect for as long as no limit is violated and will work as outlined in the spring equation in section Joint Drive.

The parameter targetPosition represents the separation of the joint frames at the violated limit, while position is their current separation. The value of targetVelocity will be 0.0.

Fixed Joint#

The fixed joint imposes the rule that the joint pose J is the identity pose. An identity pose is defined by a zero position and identity rotation. For more information on J, see section Joint Frames.

The following Python snippet serves as an introduction to the fixed joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]
fixedJointPath = "/World/fixedJoint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

# Create one kinematic rigid body and one dynamic rigid body.
# Each rigid body has cube geometry.
for i in range(2):

    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)


# Create a fixed joint between the two prims.
# This joint has zero degrees of freedom.
fixedJoint = UsdPhysics.FixedJoint.Define(stage, fixedJointPath)
fixedJointPrim = fixedJoint.GetPrim()
fixedJoint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
fixedJoint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])

# Configure the local frames of the joint.
fixedJoint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
fixedJoint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
fixedJoint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
fixedJoint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

The snippet creates a fixed joint that couples a kinematic rigid body to a dynamic rigid body. With reference to the joint equation set out in section Joint Frames, the joint local frames L0, L1 are stored in the arrays jointLocalPositions and jointLocalRotations. The scene begins with body poses G0, G1 having the values stored in the arrays rigidBodyStartPositions and rigidBodyStartRotations. It may be verified that the initial body poses and the fixed joint local frames do not satisfy the joint equation for a fixed joint. It is the job of the solver to enforce the joint equation with J having identity value.

Prismatic Joint#

The prismatic joint imposes the rule that the joint pose J (see section Joint Frames) should have identity rotation and a position that is zero on two of the three linear axes. The only motion permitted by the joint, therefore, is linear motion along a single axis.

The following Python snippet serves as an introduction to the prismatic joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]

prismaticJointPath = "/World/prismaticJoint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

# Create one kinematic rigid body and one dynamic rigid body.
# Each rigid body has cube geometry.
for i in range(2):

    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)

# Create a prismatic joint between the two prims
# This joint has a single degree of linear freedom.
prismaticJoint = UsdPhysics.PrismaticJoint.Define(stage, prismaticJointPath)
prismaticJoint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
prismaticJoint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])
prismaticJointPrim = prismaticJoint.GetPrim()

# Configure the local frames of the joint.
prismaticJoint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
prismaticJoint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
prismaticJoint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
prismaticJoint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

# The prismatic joint permits motion along only a single axis.
# The Y axis is chosen for this joint.
prismaticJoint.CreateAxisAttr(UsdPhysics.Tokens.y)

# Set a drive to drive towards a target joint position with an acceleration drive
linearDriveAPI = UsdPhysics.DriveAPI.Apply(prismaticJointPrim, UsdPhysics.Tokens.linear)
linearDriveAPI.CreateTypeAttr(UsdPhysics.Tokens.acceleration)
linearDriveAPI.CreateStiffnessAttr(200.0)
linearDriveAPI.CreateTargetPositionAttr(20.0)
linearDriveAPI.CreateTargetVelocityAttr(0.0)
linearDriveAPI.CreateDampingAttr(0.0)

# Set the range of the joint's degree of freedom to be from -5 to 5
# The drive will attempt to drive towards a joint position of 20.0 but
# the limit will overpower the drive when the joint position reaches 5.0
prismaticJoint.CreateLowerLimitAttr(-5.0)
prismaticJoint.CreateUpperLimitAttr(5.0)

The snippet creates a prismatic joint that couples a kinematic rigid body to a dynamic rigid body. With reference to the joint equation set out in section Joint Frames, the joint local frames L0, L1 are stored in the arrays jointLocalPositions and jointLocalRotations. The scene begins with body poses G0, G1 having the values stored in the arrays rigidBodyStartPositions and rigidBodyStartRotations. It may be verified that the initial body poses and the prismatic joint local frames do not satisfy the joint equation for a prismatic joint. It is the job of the solver to enforce the joint equation with J supporting motion only along a single linear axis. In the Python snippet, permitted values of J will have identity rotation and position (0, y, 0) with y being any value arising from the sum of forces acting on the body.

The Python snippet introduces a drive. Drives are discussed in more detail in section Joint Drive. In the snippet, the drive is a position drive that will drive the position of J to (0, 20, 0) starting from position (0, 0, 0). The drive is configured to be an acceleration drive with a stiffness of 200. It would be possible to create a force drive by swapping UsdPhysics.Tokens.acceleration for UsdPhysics.Tokens.force.

Section Joint Drive mentions that drives are not guaranteed to reach their target. This is true of the snippet above due to the applied limit. The drive attempts to drive J to position (0, 20, 0) but this is outside the limits which permit motion only between (0, -5, 0) and (0, 5, 0). Limits take priority over drive so the motion of the joint will halt at (0, 5, 0).

Revolute Joint#

The revolute joint imposes the rule that the joint pose J (see section Joint Frames) should have position (0, 0, 0) and a rotation that represents a non-zero angle around a single axis. The only motion permitted by the joint, therefore, is angular motion around a single axis.

The following Python snippet serves as an introduction to the revolute joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]

revoluteJointPath = "/World/revoluteJoint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

for i in range(2):

    # Create the rigid body
    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)

# Create a revolute joint between the two prims
# This joint has a single degree of rotational freedom.
revoluteJoint = UsdPhysics.RevoluteJoint.Define(stage, revoluteJointPath)
revoluteJoint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
revoluteJoint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])
revoluteJointPrim = revoluteJoint.GetPrim()

# Configure the local frames of the joint.
revoluteJoint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
revoluteJoint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
revoluteJoint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
revoluteJoint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

# The dynamic body will rotate around the Y axis
revoluteJoint.CreateAxisAttr(UsdPhysics.Tokens.y)

# Set the maximum force that the joint may exert without breaking the joint.
# The joint will be permanently redundant in the event that the breaking
# force is exceeded.
revoluteJoint.CreateBreakForceAttr(10.0)

# Set a drive to drive towards a target joint velocity with a force drive.
angularDriveAPI = UsdPhysics.DriveAPI.Apply(revoluteJointPrim,
    UsdPhysics.Tokens.angular
)
angularDriveAPI.CreateTypeAttr(UsdPhysics.Tokens.force)
angularDriveAPI.CreateStiffnessAttr(0.0)
angularDriveAPI.CreateTargetPositionAttr(0.0)
angularDriveAPI.CreateDampingAttr(100.0)
angularDriveAPI.CreateTargetVelocityAttr(200.0)

# Set the range of the joint's degree of freedom to be from -90 to +90 degrees
# The drive will attempt to drive towards a joint position of 200.0 but
# the limit will overpower the drive when the joint position reaches 90.0
revoluteJoint.CreateLowerLimitAttr(-90.0)
revoluteJoint.CreateUpperLimitAttr(90.0)

The snippet creates a revolute joint that couples a kinematic rigid body to a dynamic rigid body. With reference to the joint equation set out in section Joint Frames, the joint local frames L0, L1 are stored in the arrays jointLocalPositions and jointLocalRotations. The scene begins with body poses G0, G1 having the values stored in the arrays rigidBodyStartPositions and rigidBodyStartRotations. It may be verified that the initial body poses and the revolute joint local frames do not satisfy the joint equation for a revolute joint. It is the job of the solver to enforce the joint equation with J supporting angular motion around a single angular axis. In the Python snippet, permitted values of J will have position (0, 0, 0) and a rotation equivalent to Gf.Rotation(Gf.Vec3d(0, 1, 0), y) with y denoting the angle around the y-axis.

The Python snippet introduces a drive. Drives are discussed in more detail in section Joint Drive. In the snippet, the drive is a velocity drive that will drive the angular velocity of the joint to (0, 200, 0); that is, 200 degrees per second around the y-axis.

Spherical Joint#

The spherical joint imposes the rule that the joint pose J (see section Joint Frames) should have position (0, 0, 0) and a rotation that permits angular motion around all axes. The spherical joint allows limits to be applied around two axes to form a limit cone.

Note

  • The spherical joint does not permit limits to be applied to motion around the principal axis of the limit cone.

  • The spherical joint does not support drive.

The following Python snippet serves as an introduction to the spherical joint:

from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

rigidBodyPaths = ["/World/rigidBody0", "/World/rigidBody1"]
rigidBodyBoxPaths = ["/World/rigidBody0/box", "/World/rigidBody1/box"]
sphericalJointPath = "/World/sphericalJoint"

# One kinematic rigid body and one dynamic rigid body
rigidBodyDynamic = [False, True]

# Set the start pose of the dynamic rigid body so that the joint has to perform
# work to align the bodies to satisfy the constraints enforced by the joint
rot_z_45 = Gf.Quatf(Gf.Rotation(Gf.Vec3d(0.0, 0.0, 1.0), 45.0).GetQuat())
rigidBodyStartPositions = [Gf.Vec3f(0, 0, 0), Gf.Vec3f(10, 10, 0)]
rigidBodyStartRotations = [Gf.Quatf(1.0), rot_z_45]
jointLocalPositions = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 0.0, 0.0)]
jointLocalRotations = [Gf.Quatf(1.0), Gf.Quatf(1.0)]

# Properties of the geometry
cubePosition = Gf.Vec3f(0.0, 0.0, 0.0)
cubeOrientation = Gf.Quatf(1.0)
cubeScale = Gf.Vec3f(1.0, 1.0, 1.0)
cubeSizes = [4.0, 2.0]

for i in range(2):

    # Create the rigid body
    rigidBodyXform = UsdGeom.Xform.Define(stage, rigidBodyPaths[i])
    rigidBodyXform.AddTranslateOp().Set(rigidBodyStartPositions[i])
    rigidBodyXform.AddOrientOp().Set(rigidBodyStartRotations[i])
    rigidBodyPrim = rigidBodyXform.GetPrim()
    rigidBodyAPI = UsdPhysics.RigidBodyAPI.Apply(rigidBodyPrim)
    rigidBodyAPI.CreateKinematicEnabledAttr(not rigidBodyDynamic[i])
    massAPI = UsdPhysics.MassAPI.Apply(rigidBodyPrim)
    massAPI.CreateDensityAttr(1.0)

    # Create a cube as a child of the rigid body prim
    cubeGeom = UsdGeom.Cube.Define(stage, rigidBodyBoxPaths[i])
    cubeGeom.CreateSizeAttr(cubeSizes[i])
    cubeHalfExtent = cubeSizes[i]/2.0
    cubeGeom.CreateExtentAttr([Gf.Vec3f(-cubeHalfExtent), Gf.Vec3f(cubeHalfExtent)])
    cubeGeom.AddTranslateOp().Set(cubePosition)
    cubeGeom.AddOrientOp().Set(cubeOrientation)
    cubeGeom.AddScaleOp().Set(cubeScale)
    cubePrim = cubeGeom.GetPrim()
    UsdPhysics.CollisionAPI.Apply(cubePrim)

# Create a spherical joint between the two prims
sphericalJoint = UsdPhysics.SphericalJoint.Define(stage, sphericalJointPath)
sphericalJoint.CreateBody0Rel().SetTargets([Sdf.Path(rigidBodyPaths[0])])
sphericalJoint.CreateBody1Rel().SetTargets([Sdf.Path(rigidBodyPaths[1])])
sphericalJointPrim = sphericalJoint.GetPrim()

# Configure the local frames of the joint.
sphericalJoint.CreateLocalPos0Attr().Set(jointLocalPositions[0])
sphericalJoint.CreateLocalRot0Attr().Set(jointLocalRotations[0])
sphericalJoint.CreateLocalPos1Attr().Set(jointLocalPositions[1])
sphericalJoint.CreateLocalRot1Attr().Set(jointLocalRotations[1])

# Add a cone limit which constrains the angle between the y-axes of the two constraint
# frames. The cone limit may also be configured to constrain the angle between the
# x-axes and z-axes of the two constraint frames but in this snippet the y-axis has
# been chosen. The y-axis of the constraint frame of body1 is constrained by a
# limit cone whose principal axis is the y-axis of the constraint frame of body0.
# The allowed limit values are the maximum rotation around the z- and x-axes of that
# frame. Different values for the z- and x-axes may be specified, in which case the
# limit takes the form of an elliptical angular cone. The principal axis of the limit
# cone is chosen with the axis attribute (CreateAxisAttr()).
# The following table sets out the meaning of ConeAngle0 and ConeAngle1 for different
# choices of principal axis:
#   principal axis = x-axis -> ConeAngle0 = y-axis, ConeAngle1 = z-axis
#   principal axis = y-axis -> ConeAngle0 = z-axis, ConeAngle1 = x-axis
#   principal axis = z-axis -> ConeAngle0 = x-axis, ConeAngle1 = y-axis
sphericalJoint.CreateAxisAttr(UsdPhysics.Tokens.y)
sphericalJoint.CreateAxisAttr(UsdPhysics.Tokens.x)
sphericalJoint.CreateConeAngle0LimitAttr().Set(30.0)
sphericalJoint.CreateConeAngle1LimitAttr().Set(45.0)

The snippet creates a spherical joint that couples a kinematic rigid body to a dynamic rigid body. With reference to the joint equation set out in section Joint Frames, the joint local frames L0, L1 are stored in the arrays jointLocalPositions and jointLocalRotations. The scene begins with body poses G0, G1 having the values stored in the arrays rigidBodyStartPositions and rigidBodyStartRotations. It may be verified that the initial body poses and the spherical joint local frames do not satisfy the joint equation for a spherical joint. It is the job of the solver to enforce the joint equation with J supporting zero relative linear motion.

In the absence of limits, the spherical joint allows the rotation of J to have any value. The snippet, however, does apply a limit. This constrains the relative rotation of the two constraint frames G0L0 and G1L1. More specifically, the limits of a spherical joint form a cone limit. The snippet chooses the y-axis as the principal axis of the limit cone. The rule that follows this choice is that the y-axis of the constraint frame of body1 must lie inside a cone whose principal axis is the y-axis of the constraint frame of body0. Two further limit angles around the z-axis and x-axis specify the extent of the cone.

Note

The snippet above could, instead, have chosen the x-axis (or z-axis) to be the principal axis of the limit cone. Such a choice would have led to the rule that the x-axis (or z-axis) of the constraint frame of body1 should lie inside the limit cone.

A permutation rule governs the role of the two limit angles (CreateConeAngle0LimitAttr() and CreateConeAngle1LimitAttr()).

  • If the x-axis is chosen as the principal axis of the limit cone then CreateConeAngle0LimitAttr() governs the limit angle around the y-axis and CreateConeAngle1LimitAttr() governs the limit angle around the z-axis.

  • If the y-axis is chosen instead, then CreateConeAngle0LimitAttr() governs the limit angle around the z-axis and CreateConeAngle1LimitAttr() governs the limit around the x-axis.

  • If the z-axis is chosen instead then CreateConeAngle0LimitAttr() governs the limit angle around the x-axis and CreateConeAngle1LimitAttr() governs the limit around the y-axis.

Each axis of the limit cone may be completely locked by setting CreateConeAngle0LimitAttr() or CreateConeAngle1LimitAttr() to a negative value.

Note

At first glance, spherical joints might look like the perfect fit for scenarios where all rotational joint motion should be allowed. However, it has to be noted that the spherical joint comes with a set of limitations (as listed further below). If any of those limitations are hit, then the recommendation is to use a D6 Joint instead and lock all linear degrees of freedom (with the caveat that limits will be pyramidal and not elliptical).

  • Motion around the principal axis of the limit cone cannot be limited.

  • The lower limit value cannot be asymmetric relative to the upper limit value.

  • Soft limits are not supported (note that on articulation joints soft limits are generally not supported, see Articulation Limitations and Differences).

  • Drive/Actuation is not supported.

Meta Joints#

Meta joints are joints that impose a rule on pairs of joints. For example:

  • Gear joints couple two revolute joints.

  • Rack-and-pinion joints couple revolute joints with prismatic joints.

Type

Description

Gear Joints

The gear joint couples the rotations of two revolute joints with a gear ratio.

Rack And Pinion Joints

The rack-and-pinion joint couples the rotation of a revolute joint to the travel of a prismatic joint by a gear ratio (rotation/distance).

Table Meta Joint Support table sets out the types of coupling supported by meta joints and articulation meta joints on CPU and GPU.

Type

Meta Joint Support (cpu/gpu)

Articulation Meta Joint Support (cpu/gpu)

Gear Joints

yes/no

no/no

Rack And Pinion Joints

yes/no

no/no

Note

  • Gear and rack-and-pinion joints can be used with GPU simulation, but in that case these specific joints will run through the CPU pipeline.

  • The Mimic Joint implements gear and rack-and-pinion joints functionality specialized for articulations and has native GPU acceleration support.

Gear Joints#

A gear joint connects two existing revolute joints and constrains the angular velocities of involved rigid bodies in this way:

|ω1|=|ω0|R

where ω0 is the angular velocity of the first rigid body around the revolute rotation axis, ω1 is the angular velocity of the second rigid body around the revolute rotation axis, and R is the gear ratio. Note that the equation is valid for the absolute values of angular velocities. The angular velocities may otherwise have opposite signs, depending on the orientations of the joint frames.

The following Python snippet serves as an introduction to the gear joint:

from pxr import Usd, UsdGeom, Sdf, Gf, UsdPhysics, PhysxSchema
import omni.usd
import math

def create_gear(stage, nbShapes, gearPos, scale, gearIndex):
    gearPath = "/World/gear" + str(gearIndex)

    rigidXform = UsdGeom.Xform.Define(stage, gearPath)
    rigidPrim = stage.GetPrimAtPath(gearPath)

    rigidXform.AddTranslateOp().Set(gearPos)
    rigidXform.AddOrientOp().Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0))

    physicsAPI = UsdPhysics.RigidBodyAPI.Apply(rigidPrim)
    UsdPhysics.MassAPI.Apply(rigidPrim)

    shapePos = Gf.Vec3f(0.0, 0.0, 0.0)

    for i in range(nbShapes):
        collisionShapePath = gearPath + "/physicsBoxShape" + str(i)

        coeff = i/nbShapes
        angle = math.pi*0.5*coeff
        w = math.cos((math.pi / 2.0 - angle) / 2.0)
        y = -math.sin((math.pi / 2.0 - angle) / 2.0)
        shapeQuat = Gf.Quatf(w, Gf.Vec3f(0.0, y, 0.0))

        cubeGeom = UsdGeom.Cube.Define(stage, collisionShapePath)
        cubePrim = stage.GetPrimAtPath(collisionShapePath)
        cubeGeom.CreateSizeAttr(1.0)
        cubeGeom.AddTranslateOp().Set(shapePos)
        cubeGeom.AddOrientOp().Set(shapeQuat)
        cubeGeom.AddScaleOp().Set(scale)

        UsdPhysics.CollisionAPI.Apply(cubePrim)

    # Create revolute joint that attaches the gear object to the world
    revoluteJoint = UsdPhysics.RevoluteJoint.Define(stage,
        "/World/revoluteJoint" + str(gearIndex)
    )

    revoluteJoint.CreateAxisAttr("Y")

    revoluteJoint.CreateBody1Rel().SetTargets([gearPath])

    revoluteJoint.CreateLocalPos0Attr().Set(gearPos)
    revoluteJoint.CreateLocalRot0Attr().Set(Gf.Quatf(1.0))

    revoluteJoint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
    revoluteJoint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

gearDepth = 10.0

# Create first gear object
gear0Pos = Gf.Vec3f(0.0, 0.0, 100.0)
radius0 = 80.0
scale = Gf.Vec3f(radius0, gearDepth*2.0, radius0)
nbShapes = int(radius0/10.0)
create_gear(stage, nbShapes, gear0Pos, scale, 0)

# Create second gear object
gear1Pos = Gf.Vec3f(74.0, 0.0, 100.0)
radius1 = 30.0
scale = Gf.Vec3f(radius1, gearDepth, radius1)
nbShapes = int(radius1/10.0)
create_gear(stage, nbShapes, gear1Pos, scale, 1)

# Create gear joint that constrains the gears' revolute joints
gearJoint = PhysxSchema.PhysxPhysicsGearJoint.Define(stage, "/World/gearJoint")

gearJoint.CreateBody0Rel().SetTargets(["/World/gear0"])
gearJoint.CreateBody1Rel().SetTargets(["/World/gear1"])

gearJoint.CreateGearRatioAttr(radius0/radius1)
gearJoint.CreateHinge0Rel().SetTargets(["/World/revoluteJoint0"])
gearJoint.CreateHinge1Rel().SetTargets(["/World/revoluteJoint1"])

The snippet first creates two independent gear objects, each as a dynamic rigid body connected to the world by a revolute joint. Then it creates a gear joint that couples the two revolute joints themselves, so that their rotations are not independent anymore.

The gear joint is configured so that the teeth of the gears do not overlap. To achieve this the gear ratio must be derived from either the number of teeth in each gear, or as in this snippet from the radius of each gear.

Rack And Pinion Joints#

A rack-and-pinion joint connects an existing revolute joint to an existing prismatic joint, and constrains the angular and linear velocities of involved rigid bodies this way:

|ω|=|V|R

where ω is the angular velocity of the gear rigid body around the revolute rotation axis, V is the linear velocity of the rack rigid body along the prismatic joint axis, and R is the ratio. Note that the equation is valid for the absolute values of velocities. The velocities may otherwise have opposite signs, depending on the orientations of the joint frames.

The following Python snippet serves as an introduction to the rack-and-pinion joint:

from pxr import Usd, UsdGeom, Sdf, Gf, UsdPhysics, PhysxSchema
import omni.usd
import math

def create_gear(stage, nbShapes, gearPos, scale, gearIndex):
    gearPath = "/World/gear" + str(gearIndex)

    rigidXform = UsdGeom.Xform.Define(stage, gearPath)
    rigidPrim = stage.GetPrimAtPath(gearPath)

    rigidXform.AddTranslateOp().Set(gearPos)
    rigidXform.AddOrientOp().Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0))

    physicsAPI = UsdPhysics.RigidBodyAPI.Apply(rigidPrim)
    UsdPhysics.MassAPI.Apply(rigidPrim)

    shapePos = Gf.Vec3f(0.0, 0.0, 0.0)

    for i in range(nbShapes):
        collisionShapePath = gearPath + "/physicsBoxShape" + str(i)

        coeff = i/nbShapes
        angle = math.pi*0.5*coeff
        w = math.cos((math.pi / 2.0 - angle) / 2.0)
        y = -math.sin((math.pi / 2.0 - angle) / 2.0)
        shapeQuat = Gf.Quatf(w, Gf.Vec3f(0.0, y, 0.0))

        cubeGeom = UsdGeom.Cube.Define(stage, collisionShapePath)
        cubePrim = stage.GetPrimAtPath(collisionShapePath)
        cubeGeom.CreateSizeAttr(1.0)
        cubeGeom.AddTranslateOp().Set(shapePos)
        cubeGeom.AddOrientOp().Set(shapeQuat)
        cubeGeom.AddScaleOp().Set(scale)

        UsdPhysics.CollisionAPI.Apply(cubePrim)

    # Create revolute joint that attaches the gear object to the world
    revoluteJoint = UsdPhysics.RevoluteJoint.Define(stage,
        "/World/revoluteJoint" + str(gearIndex)
    )

    revoluteJoint.CreateAxisAttr("Y")

    revoluteJoint.CreateBody1Rel().SetTargets([gearPath])

    revoluteJoint.CreateLocalPos0Attr().Set(gearPos)
    revoluteJoint.CreateLocalRot0Attr().Set(Gf.Quatf(1.0))

    revoluteJoint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
    revoluteJoint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

def create_rack(stage, nbShapes, rackPos, scale, rackIndex, rackLength):
    rackPath = "/World/rack" + str(rackIndex)

    rigidXform = UsdGeom.Xform.Define(stage, rackPath)
    rigidPrim = stage.GetPrimAtPath(rackPath)

    rigidXform.AddTranslateOp().Set(rackPos)
    rigidXform.AddOrientOp().Set(Gf.Quatf(1.0, 0.0, 0.0, 0.0))

    UsdPhysics.RigidBodyAPI.Apply(rigidPrim)

    angle = math.pi * 0.25
    w = math.cos((math.pi / 2.0 - angle) / 2.0)
    y = -math.sin((math.pi / 2.0 - angle) / 2.0)
    shapeQuat = Gf.Quatf(w, Gf.Vec3f(0.0, y, 0.0))

    for i in range(nbShapes):
        collisionShapePath = rackPath + "/physicsBoxShape" + str(i)

        coeff = i / nbShapes

        shapePos = Gf.Vec3f(coeff * rackLength - rackLength * 0.5, 0.0, 0.0)

        cubeGeom = UsdGeom.Cube.Define(stage, collisionShapePath)
        cubePrim = stage.GetPrimAtPath(collisionShapePath)
        cubeGeom.CreateSizeAttr(1.0)
        cubeGeom.AddTranslateOp().Set(shapePos)
        cubeGeom.AddOrientOp().Set(shapeQuat)
        cubeGeom.AddScaleOp().Set(scale)

        UsdPhysics.CollisionAPI.Apply(cubePrim)

    # Create prismatic joint that attaches the rack object to the world
    prismaticJoint = UsdPhysics.PrismaticJoint.Define(stage,
        "/World/prismaticJoint" + str(rackIndex)
    )

    prismaticJoint.CreateAxisAttr("X")

    prismaticJoint.CreateBody1Rel().SetTargets([rackPath])

    prismaticJoint.CreateLocalPos0Attr().Set(rackPos)
    prismaticJoint.CreateLocalRot0Attr().Set(Gf.Quatf(1.0))

    prismaticJoint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))
    prismaticJoint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

gearDepth = 10.0

# Create gear object
gear0Pos = Gf.Vec3f(0.0, 0.0, 100.0)
radius0 = 80.0
scale = Gf.Vec3f(radius0, gearDepth * 2.0, radius0)
nbShapes0 = int(radius0 / 10.0)
create_gear(stage, nbShapes0, gear0Pos, scale, 0)

# Create rack object
gear1Pos = Gf.Vec3f(0.0, 0.0, 174.0)
rackExtent = 30.0
rackLength = 310.0
scale = Gf.Vec3f(rackExtent, gearDepth, rackExtent)
nbShapes1 = int(rackLength / 10.0)
create_rack(stage, nbShapes1, gear1Pos, scale, 0, rackLength)

# Create rack joint that constrains the previously created revolute and prismatic joints
rackJoint = PhysxSchema.PhysxPhysicsRackAndPinionJoint.Define(stage, "/World/rackJoint")

rackJoint.CreateBody0Rel().SetTargets(["/World/gear0"])
rackJoint.CreateBody1Rel().SetTargets(["/World/rack0"])

# for the rack, each box shape contributes 1 tooth
# for the gear, each box shape contributes 4 teeth
nbRackTeeth = nbShapes1
nbPinionTeeth = nbShapes0 * 4
ratio = (360.0 * nbRackTeeth) / (rackLength * nbPinionTeeth)
rackJoint.CreateRatioAttr(ratio)
rackJoint.CreateHingeRel().SetTargets(["/World/revoluteJoint0"])
rackJoint.CreatePrismaticRel().SetTargets(["/World/prismaticJoint0"])

The snippet first creates two independent objects. The first one is a gear object created as a dynamic rigid body connected to the world by a revolute joint. The second one is a rack object created as a dynamic rigid body connected to the world by a prismatic joint. Then the snippet creates a rack-and-pinion joint that couples the revolute and prismatic joints themselves, so that their motions are not independent anymore.

The rack-and-pinion joint is configured so that the teeth of the gear and the teeth of the rack do not overlap. To achieve this the ratio attribute must be derived from the formula:

ratio = (PI * 2 * nbRackTeeth)/(rackLength * nbPinionTeeth)

Joint Instancing#

Joint instancing is the counterpart of rigid body point instancing, as discussed in section Point Instancing. A key point here is that instanced joints may only reference point instanced bodies. Scenegraph instancing is not supported for joints.

The following python snippet is a simple example of joint instancing:

from pxr import Usd, UsdGeom, UsdPhysics, Sdf, Gf, PhysxSchema
import omni.usd

# Create a stage
omni.usd.get_context().new_stage()
stage = omni.usd.get_context().get_stage()

# Define the root Xform (transformable object)
rootxform = UsdGeom.Xform.Define(stage, "/World")

def createInstancedRigidBodies(stage, pointInstancerPath):

    pointInstancer = UsdGeom.PointInstancer.Define(stage, pointInstancerPath)

    # Create a rigid dynamic body with mass 3.
    # and a sphere collider with radius of 2 distance units.
    prototypeRigidBodyPath = pointInstancerPath + "/rigidBody"
    prototypeRigidBody = UsdGeom.Xform.Define(stage, prototypeRigidBodyPath)
    prototypeRigidBody.AddTranslateOp().Set(Gf.Vec3f(0.0))
    prototypeRigidBody.AddOrientOp().Set(Gf.Quatf(1.0))
    prototypeRigidBodyPrim = prototypeRigidBody.GetPrim()
    UsdPhysics.RigidBodyAPI.Apply(prototypeRigidBodyPrim)
    massAPI = UsdPhysics.MassAPI.Apply(prototypeRigidBodyPrim)
    massAPI.CreateMassAttr(3.0)
    # Create a collision sphere with radius 2.
    spherePath = "/World/pointinstancer/rigidBody/sphere"
    sphereGeom = UsdGeom.Sphere.Define(stage, spherePath)
    sphereGeom.CreateRadiusAttr(2.0)
    sphereGeom.AddTranslateOp().Set(Gf.Vec3f(0.0))
    sphereGeom.AddOrientOp().Set(Gf.Quatf(1.0))
    sphereGeom.AddScaleOp().Set(Gf.Vec3f(1.0))
    UsdPhysics.CollisionAPI.Apply(sphereGeom.GetPrim())

    # Create 3 instances of the rigid body
    rigidBodyPrototypeList = pointInstancer.GetPrototypesRel()
    rigidBodyPrototypeList.AddTarget(prototypeRigidBodyPath)
    prototypeIndices = [0, 0, 0]
    rigidBodyStartPositions = [Gf.Vec3f(0), Gf.Vec3f(0, -10, 0), Gf.Vec3f(0, -20, 0)]
    rigidBodyStartRotations = [Gf.Quath(1.0), Gf.Quath(1.0), Gf.Quath(1.0)]
    rigidBodyStartLinVelocities = [Gf.Vec3f(0), Gf.Vec3f(0), Gf.Vec3f(0)]
    rigidBodyStartAngVelocities = [
        Gf.Vec3f(0, 0, 30), Gf.Vec3f(0, 0, 45), Gf.Vec3f(0, 0, 60)
    ]
    pointInstancer.GetProtoIndicesAttr().Set(prototypeIndices)
    pointInstancer.GetPositionsAttr().Set(rigidBodyStartPositions)
    pointInstancer.GetOrientationsAttr().Set(rigidBodyStartRotations)
    pointInstancer.GetVelocitiesAttr().Set(rigidBodyStartLinVelocities)
    pointInstancer.GetAngularVelocitiesAttr().Set(rigidBodyStartAngVelocities)


# Create point instancer and 3 instanced rigid dynamic bodies
pointInstancerPath = "/World/pointinstancer"
createInstancedRigidBodies(stage, pointInstancerPath)

# joint instancer path and the path to the prototype spherical joint
jointInstancerPath =  "/World/jointinstancer"
prototypeSphericalJointPath = jointInstancerPath + "/sphericalJoint"

# Joint instancer setup
jointInstancer = PhysxSchema.PhysxPhysicsJointInstancer.Define(stage, jointInstancerPath)

# Create a prototype spherical joint
prototypeSphericalJointPath = jointInstancerPath + "/sphericalJoint"
prototypeSphericalJoint = UsdPhysics.SphericalJoint.Define(stage, prototypeSphericalJointPath)

# Add the prototype  spherical joint to the joint instancer
jointPrototypeList = jointInstancer.GetPhysicsPrototypesRel()
jointPrototypeList.AddTarget(prototypeSphericalJointPath)

jointIndices = [0, 0]
jointInstancer.GetPhysicsProtoIndicesAttr().Set(jointIndices)

body0s = []
body0s.append(pointInstancerPath)
body1s = []
body1s.append(pointInstancerPath)
body0indices = [0, 1]
body1indices = [1, 2]
localPositions0 = [Gf.Vec3f(0.0), Gf.Vec3f(0.0)]
localPositions1 = [Gf.Vec3f(0.0, 10.0, 0.0), Gf.Vec3f(0.0, 10.0, 0.0)]
localRotations0 = [Gf.Quath(1.0), Gf.Quath(1.0)]
localRotations1 = [Gf.Quath(1.0), Gf.Quath(1.0)]
jointInstancer.GetPhysicsBody0sRel().SetTargets(body0s)
jointInstancer.GetPhysicsBody0IndicesAttr().Set(body0indices)
jointInstancer.GetPhysicsBody1sRel().SetTargets(body1s)
jointInstancer.GetPhysicsBody1IndicesAttr().Set(body1indices)
jointInstancer.GetPhysicsLocalPos0sAttr().Set(localPositions0)
jointInstancer.GetPhysicsLocalPos1sAttr().Set(localPositions1)
jointInstancer.GetPhysicsLocalRot0sAttr().Set(localRotations0)
jointInstancer.GetPhysicsLocalRot1sAttr().Set(localRotations1)

The Python snippet begins by creating three dynamic instanced rigid bodies using point instancing (see section Point Instancing) arranged in a line along the y-axis and two instanced spherical joints connecting each adjacent body pair.

Note

Joint instancing using UsdGeom.PointInstancer is not supported for articulation joints, see Articulation Limitations and Differences.