How to eliminate the rotation in spherical joints

Note: For any Isaac Lab topics, please submit your topic to its GitHub repo (GitHub - isaac-sim/IsaacLab: Unified framework for robot learning built on NVIDIA Isaac Sim) following the instructions provided on Isaac Lab’s Contributing Guidelines (Contribution Guidelines — Isaac Lab Documentation).

Please provide all relevant details below before submitting your post. This will help the community provide more accurate and timely assistance. After submitting, you can check the appropriate boxes. Remember, you can always edit your post later to include additional information if needed.

Isaac Sim Version

4.5.0
4.2.0
4.1.0
4.0.0
4.5.0
2023.1.1
2023.1.0-hotfix.1
Other (please specify):

Operating System

Ubuntu 22.04
Ubuntu 20.04
Windows 11
Windows 10
Other (please specify):

GPU Information

  • Model: 4090
  • Driver Version: 570.133.07

Topic Description

Detailed Description

I’m trying to simulate a spherical joint which has a sphere and four poles. My model is a simplified model which only has joint constraints. But once the simulation started, the sphere begined to rotate by itself while the four polese started to ratatiuon around theirs pole direction, which i do not want. How can it preserve the spherical properties but eliminate such rotation?

Screenshots or Videos

Hi @1127642041, thank you for posting your issue. The behavior that is happening is correct as each pole is not constrained in rotation around the pole axis. Also, the ball is not constrained in any rotational direction. One way to deal with this (if I imagined a real system built like this) is to add friction or joint damping to stop the spinning. To do that you will have to add JointDrive property to the joint, however, currently joint drive is not implemented for spherical joints. Instead, I would recommend using D6 joint to simulate a spherical joint with joint drive. Here is an example code to do that:

#!/usr/bin/env python3

# Copyright (c) 2022-2024, NVIDIA CORPORATION. All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto. Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.

"""
This script demonstrates how to create two capsules connected by a D6 joint
that is constrained to allow only spherical motion (pure rotation).
The D6 joint has all translational degrees of freedom locked and only
rotational degrees of freedom enabled.
"""

from isaacsim import SimulationApp

# Launch Isaac Sim
simulation_app = SimulationApp({"headless": False})

import time
from pxr import Usd, UsdGeom, UsdPhysics, Gf, Sdf, PhysxSchema, UsdShade, UsdLux
from omni.physx.scripts import physicsUtils
from omni.timeline import get_timeline_interface
from isaacsim.core.api import World

# Set Light stage to Grey Studio
stage = simulation_app.context.get_stage()
light_stage = stage.GetPrimAtPath("/Environment")
if not light_stage:
    light_stage = stage.DefinePrim("/Environment", "Xform")
    
# Create Grey Studio lighting environment
dome_light = UsdLux.DomeLight.Define(stage, "/Environment/GreyStudio")
dome_light.CreateIntensityAttr().Set(1000.0)

class SphericalD6Joint:
    def __init__(self, stage, joint_path, body0_path, body1_path, local_pos0, local_pos1, 
                 damping=0.0, stiffness=0.0):
        """
        Create a D6 joint that behaves like a spherical joint (pure rotation only)
        
        Args:
            stage: USD stage
            joint_path: Path for the joint prim
            body0_path: Path to first rigid body
            body1_path: Path to second rigid body
            local_pos0: Local position on body0
            local_pos1: Local position on body1
            damping: Joint damping for rotational axes
            stiffness: Joint stiffness for rotational axes
        """
        self.stage = stage
        self.joint_path = joint_path
        self.body0_path = body0_path
        self.body1_path = body1_path
        self.local_pos0 = local_pos0
        self.local_pos1 = local_pos1
        self.damping = damping
        self.stiffness = stiffness
        self.joint = None
        self._create_joint()

    def _create_joint(self):
        # Create a D6 joint
        self.joint = UsdPhysics.Joint.Define(self.stage, self.joint_path)
        joint_prim = self.joint.GetPrim()

        # Lock all translational degrees of freedom (transX, transY, transZ)
        for axis in ["transX", "transY", "transZ"]:
            limitAPI = UsdPhysics.LimitAPI.Apply(joint_prim, axis)
            limitAPI.CreateLowAttr(0.0)
            limitAPI.CreateHighAttr(0.0)

        # Set the bodies to connect
        self.joint.GetBody0Rel().SetTargets([self.body0_path])
        self.joint.GetBody1Rel().SetTargets([self.body1_path])

        # Set local positions
        self.joint.CreateLocalPos0Attr().Set(self.local_pos0)
        self.joint.CreateLocalPos1Attr().Set(self.local_pos1)

        # Set local rotations (identity quaternions)
        self.joint.CreateLocalRot0Attr().Set(Gf.Quatf(1.0))
        self.joint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0))

        # Configure joint drives for rotational axes (rotX, rotY, rotZ)
        # These will allow free rotation around all axes
        for axis in ["rotX", "rotY", "rotZ"]:
            # Configure drive for damping/stiffness
            driveAPI = UsdPhysics.DriveAPI.Apply(joint_prim, axis)
            driveAPI.CreateTypeAttr("force")
            driveAPI.CreateDampingAttr(self.damping)
            driveAPI.CreateStiffnessAttr(self.stiffness)

    def get_joint(self):
        return self.joint

def create_capsule(stage, path, position, height=0.2, radius=0.05, mass=0.1, color=None):
    """
    Create a capsule rigid body
    
    Args:
        stage: USD stage
        path: Path for the capsule prim
        position: Position of the capsule
        height: Height of the capsule
        radius: Radius of the capsule
        mass: Mass of the capsule
        color: Display color (Gf.Vec3f)
    """
    if color is None:
        color = Gf.Vec3f(0.2, 0.6, 0.8)  # Default blue-ish color
    
    # Create capsule geometry
    capsule_geom = UsdGeom.Capsule.Define(stage, path)
    capsule_geom.CreateHeightAttr(height)
    capsule_geom.CreateRadiusAttr(radius)
    capsule_geom.CreateAxisAttr("X")
    capsule_geom.CreateDisplayColorAttr().Set([color])
    
    # Set position
    capsule_geom.AddTranslateOp().Set(position)

    # Apply physics APIs
    UsdPhysics.CollisionAPI.Apply(capsule_geom.GetPrim())
    UsdPhysics.RigidBodyAPI.Apply(capsule_geom.GetPrim())
    
    # Set mass
    mass_api = UsdPhysics.MassAPI.Apply(capsule_geom.GetPrim())
    mass_api.CreateMassAttr().Set(mass)
    
    # Configure collision properties
    physx_collision_api = PhysxSchema.PhysxCollisionAPI.Apply(capsule_geom.GetPrim())
    physx_collision_api.CreateRestOffsetAttr().Set(0.0)
    physx_collision_api.CreateContactOffsetAttr().Set(0.01)
    
    return capsule_geom

# Create a world
my_world = World(stage_units_in_meters=1.0)

# Add default ground plane
my_world.scene.add_default_ground_plane()

# Get the stage
stage = simulation_app.context.get_stage()

# Enable Physics
scene = UsdPhysics.Scene.Define(stage, "/physicsScene")
scene.CreateGravityDirectionAttr().Set(Gf.Vec3f(0.0, 0.0, -1.0))
scene.CreateGravityMagnitudeAttr().Set(9.81)
physx_scene = PhysxSchema.PhysxSceneAPI.Apply(stage.GetPrimAtPath("/physicsScene"))
physx_scene.GetTimeStepsPerSecondAttr().Set(120)

# Create default prim
default_prim_path = Sdf.Path("/World")
UsdGeom.Xform.Define(stage, default_prim_path)
stage.SetDefaultPrim(stage.GetPrimAtPath(default_prim_path))

# Create physics material
physics_material_path = default_prim_path.AppendChild("PhysicsMaterial")
UsdShade.Material.Define(stage, physics_material_path)
material = UsdPhysics.MaterialAPI.Apply(stage.GetPrimAtPath(physics_material_path))
material.CreateStaticFrictionAttr().Set(0.5)
material.CreateDynamicFrictionAttr().Set(0.5)
material.CreateRestitutionAttr().Set(0)

# Create two capsules
capsule1_path = default_prim_path.AppendChild("capsule1")
capsule2_path = default_prim_path.AppendChild("capsule2")

# Position capsules with some separation
capsule1_pos = Gf.Vec3f(0.0, 0.0, 1.0)
capsule2_pos = Gf.Vec3f(0.3, 0.0, 1.0)  # 0.3 units apart in X direction

# Create the capsules
capsule1 = create_capsule(stage, capsule1_path, capsule1_pos, 
                         height=0.2, radius=0.05, mass=0.1, 
                         color=Gf.Vec3f(0.8, 0.2, 0.2))  # Red
capsule2 = create_capsule(stage, capsule2_path, capsule2_pos, 
                         height=0.2, radius=0.05, mass=0.1, 
                         color=Gf.Vec3f(0.2, 0.8, 0.2))  # Green

# Apply physics material to both capsules
physicsUtils.add_physics_material_to_prim(stage, capsule1.GetPrim(), physics_material_path)
physicsUtils.add_physics_material_to_prim(stage, capsule2.GetPrim(), physics_material_path)

# Create spherical D6 joint between the capsules
joint_path = default_prim_path.AppendChild("spherical_joint")

# Local positions for the joint (at the ends of the capsules)
local_pos0 = Gf.Vec3f(0.15, 0.0, 0.0)  # End of first capsule
local_pos1 = Gf.Vec3f(-0.15, 0.0, 0.0)  # End of second capsule

# Create the spherical joint
spherical_joint = SphericalD6Joint(
    stage,
    joint_path,
    capsule1_path,
    capsule2_path,
    local_pos0,
    local_pos1,
    damping=10.0,  # Add some damping to prevent excessive oscillation
    stiffness=0.0  # No stiffness - free rotation
)

# Create a fixed joint to anchor the first capsule to the world
anchor_joint_path = default_prim_path.AppendChild("anchor_joint")
anchor_joint = UsdPhysics.FixedJoint.Define(stage, anchor_joint_path)

# Set the first capsule as body1 (body0 will be the world/ground)
anchor_joint.GetBody0Rel().SetTargets(['/World'])
anchor_joint.GetBody1Rel().SetTargets([capsule1_path])

# Set the local position for the anchor joint
anchor_joint.CreateLocalPos0Attr().Set(capsule1_pos)
anchor_joint.CreateLocalPos1Attr().Set(Gf.Vec3f(0.0, 0.0, 0.0))

# Initial setup for the simulation
timeline = get_timeline_interface()

# Start the simulation
timeline.play()

# Wait for the simulation to start
while not timeline.is_playing():
    simulation_app.update()
    time.sleep(0.01)

# Keep the simulation running
print("Simulation started with two capsules connected by a spherical D6 joint.")
print("The first capsule is anchored to the world.")
print("The second capsule can rotate freely around the joint.")
print("Press Ctrl+C to exit")

try:
    while timeline.is_playing():
        simulation_app.update()
        time.sleep(0.01)
except KeyboardInterrupt:
    print("Simulation stopped by user")
finally:
    # Stop the simulation
    timeline.stop()
    simulation_app.close()
1 Like

Thanks for your reply. I tried your configuration in isaac sim and it works well. But when I import it into the isaaclab through a edited version of “add_new_robot.py” , the robot model exploded, which was not the case in isaac sim.

Here’s my code and the model i’m using:

# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

import argparse

from isaaclab.app import AppLauncher

# add argparse arguments
parser = argparse.ArgumentParser(
    description="This script demonstrates adding a custom robot to an Isaac Lab environment."
)
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.")
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()

# launch omniverse app
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

import numpy as np
import torch

import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import AssetBaseCfg
from isaaclab.assets.articulation import ArticulationCfg
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR

GEOTRUSSROVER_CONFIG = ArticulationCfg(
    spawn=sim_utils.UsdFileCfg(
        usd_path="/home/houzhinan/Workspace/GeoTrussRover-simulation/opt_isaac/sqaure_D6test.usd",
        rigid_props=sim_utils.RigidBodyPropertiesCfg(
            disable_gravity=False,
            retain_accelerations=False,
            rigid_body_enabled=True,
            max_linear_velocity=1000.0,
            max_angular_velocity=1000.0,
            max_depenetration_velocity=100.0,
            linear_damping=0.0,
            angular_damping=0.0,
        ),
        articulation_props=sim_utils.ArticulationRootPropertiesCfg(
            enabled_self_collisions=False, 
            solver_position_iteration_count=8, 
            solver_velocity_iteration_count=4,
            sleep_threshold=0.005,
            stabilization_threshold=0.001,
        ),
    ),
    init_state=ArticulationCfg.InitialStateCfg(
        pos=(0.0, 0.0, 0.0),          
        rot=(1.0, 0.0, 0.0, 0.0),     
        lin_vel=(0.0, 0.0, 0.0),      
        ang_vel=(0.0, 0.0, 0.0), 
    ),
    actuators={
        "prismatic_joints": ImplicitActuatorCfg(
            joint_names_expr=[".*_pj.*"],
            effort_limit=None,  
            velocity_limit=None,  
            stiffness=None, 
            damping=None,  
        ),
        # "revolute_joints": ImplicitActuatorCfg(
        #     joint_names_expr=[".*RevoluteJoint.*"],
        #     effort_limit=200.0,
        #     velocity_limit=50.0,
        #     stiffness=100.0,
        #     damping=10.0,
        # ),
    },
)

class NewRobotsSceneCfg(InteractiveSceneCfg):
    """Designs the scene."""

    ground = AssetBaseCfg(
        prim_path="/World/defaultGroundPlane", 
        spawn=sim_utils.GroundPlaneCfg(
            physics_material=sim_utils.RigidBodyMaterialCfg(
                static_friction=1.0,
                dynamic_friction=1.0,
                restitution=0.0,
            )
        )
    )

    # lights
    dome_light = AssetBaseCfg(
        prim_path="/World/Light", 
        spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
    )

    # robot
    GeoTrussRover = GEOTRUSSROVER_CONFIG.replace(prim_path="/GeoTrussRover")


def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
    sim_dt = sim.get_physics_dt()
    sim_time = 0.0
    count = 0

    while simulation_app.is_running():
        # reset
        if count % 500 == 0:
            # reset counters
            count = 0
            # reset the scene entities to their initial positions offset by the environment origins
            root_geotrussrover_state = scene["GeoTrussRover"].data.default_root_state.clone()
            root_geotrussrover_state[:, :3] += scene.env_origins

            root_geotrussrover_state[:, 2] = torch.maximum(
                root_geotrussrover_state[:, 2], 
                torch.tensor(1.0, device=root_geotrussrover_state.device)
            )

            # copy the default root state to the sim
            scene["GeoTrussRover"].write_root_pose_to_sim(root_geotrussrover_state[:, :7])
            scene["GeoTrussRover"].write_root_velocity_to_sim(root_geotrussrover_state[:, 7:])

            # copy the default joint states to the sim
            joint_pos, joint_vel = (
                scene["GeoTrussRover"].data.default_joint_pos.clone(),
                scene["GeoTrussRover"].data.default_joint_vel.clone(),
            )
            scene["GeoTrussRover"].write_joint_state_to_sim(joint_pos, joint_vel)
            # clear internal buffers
            scene.reset()
            print("[INFO]: Resetting GeoTrussRover state...")

        # if count % 100 < 50:
        #     if scene["GeoTrussRover"].num_joints > 0:
        #         action = torch.zeros(scene["GeoTrussRover"].num_joints, device=scene.device)
        #         scene["GeoTrussRover"].set_joint_position_target(action)

        scene.write_data_to_sim()
        sim.step()
        sim_time += sim_dt
        count += 1
        scene.update(sim_dt)


def main():
    """Main function."""
    sim_cfg = sim_utils.SimulationCfg(
        device=args_cli.device,
        dt=1.0/60,  
        physx=sim_utils.PhysxCfg(
            solver_type=1,
            max_position_iteration_count=8,
            max_velocity_iteration_count=4,
        )
    )
    sim = sim_utils.SimulationContext(sim_cfg)

    sim.set_camera_view([5.0, 5.0, 3.0], [0.0, 0.0, 1.0])
    scene_cfg = NewRobotsSceneCfg(args_cli.num_envs, env_spacing=3.0)
    scene = InteractiveScene(scene_cfg)

    sim.reset()
    
    print("[INFO]: Setup complete...")
    
    if hasattr(scene["GeoTrussRover"], 'data'):
        print(f"[DEBUG]: Root position: {scene['GeoTrussRover'].data.root_state_w[:, :3]}")
        print(f"[DEBUG]: Root orientation: {scene['GeoTrussRover'].data.root_state_w[:, 3:7]}")
    
    run_simulator(sim, scene)


if __name__ == "__main__":
    main()
    simulation_app.close()

Robot model file

https://github.com/lozoRUST/Robot_Model/blob/main/sqaure_D6test.usd

@michalin could you please figure out what happened with my model?

I will take a look