[Question] Why Robot Articulation not starting at default joint configuration, when simulation is running

[kumar-sanjeeev]

Question

I have observed one thing: although we initialize the default position of the robot in the configuration file, for ex: FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG, it does not start in the same configuration when we step forward in the simulation.

What I am expecting to get after initializing with FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG config file :

robot default/start configuration = tensor([[ 0.0000, -0.5690,  0.0000, -2.8100,  0.0000,  3.0370,  0.7410,  0.0400,
          0.0400]])

Since I have initialized the robot at this default configuration, and if I run the physics simulation without executing any physics-related actions, the robot should start at this default configuration. [I mean to say robot default position == robot current joint positions]

what I got in return:

robot current position = tensor([[ 1.0774e-05, -1.0071e+00,  1.1733e-03, -2.2216e+00, -7.3199e-04,
          2.3979e+00,  8.0006e-01,  2.0000e-02,  2.0000e-02]]) # used get_joint_positions() method from  ArticulationView API

script used to check this behavior:

import argparse

from omni.isaac.kit import SimulationApp

# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
args_cli = parser.parse_args()

# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)


"""Rest everything follows."""


import torch

import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.viewports import set_camera_view

import omni.isaac.orbit.utils.kit as kit_utils
from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG
from omni.isaac.orbit.robots.single_arm import SingleArmManipulator
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR

"""
Main
"""


def main():
    """Spawns a single arm manipulator and applies random joint commands."""

    # Load kit helper
    sim = SimulationContext(physics_dt=0.01, rendering_dt=0.01, backend="torch")
    # Set main camera
    set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])

    # Spawn things into stage
    # Ground-plane
    kit_utils.create_ground_plane("/World/defaultGroundPlane", z_position=-1.05)
    # Lights-1
    prim_utils.create_prim(
        "/World/Light/GreySphere",
        "SphereLight",
        translation=(4.5, 3.5, 10.0),
        attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)},
    )
    # Lights-2
    prim_utils.create_prim(
        "/World/Light/WhiteSphere",
        "SphereLight",
        translation=(-4.5, 3.5, 10.0),
        attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)},
    )
    # Table
    table_usd_path = f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"
    prim_utils.create_prim("/World/Table_1", usd_path=table_usd_path, translation=(0.55, -1.0, 0.0))
    # Robots
    # -- Resolve robot config from command-line arguments
    if args_cli.robot == "franka_panda":
        robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
    elif args_cli.robot == "ur10":
        robot_cfg = UR10_CFG
    else:
        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
    # -- Spawn robot
    robot = SingleArmManipulator(cfg=robot_cfg)
    robot.spawn("/World/Robot_1", translation=(0.0, -1.0, 0.0))

    # Play the simulator
    sim.reset()
    # Acquire handles
    # Initialize handles
    robot.initialize("/World/Robot.*")
    # Reset states
    robot.reset_buffers()

    # Now we are ready!
    print("[INFO]: Setup complete...")

    # dummy actions
    actions = torch.rand(robot.count, robot.num_actions, device=robot.device)
    has_gripper = robot.cfg.meta_info.tool_num_dof > 0

    # Define simulation stepping
    sim_dt = sim.get_physics_dt()

    # Simulate physics
    while simulation_app.is_running():
        # If simulation is stopped, then exit.
        if sim.is_stopped():
            break
        # If simulation is paused, then skip.
        if not sim.is_playing():
            sim.step(render=not args_cli.headless)
            continue
        
        #### changes
        dof_pos, dof_vel = robot.get_default_dof_state()
        print(f"Default DOF positions: {dof_pos}")
        print(f"Current joint positions: {robot.articulations.get_joint_positions()}")
        sim.step()
        #### 
        


if __name__ == "__main__":
    # Run the main function
    main()
    # Close the simulator
    simulation_app.close()

Hack around this is: Set the desired default configuration before stepping the simulation:

robot.articulations.set_joint_positions(desired_config)

Is this a bug, or is it a desired behavior explicitly coded in the Orbit extension?

[thkkk]

I also encountered the same problem. robot._data.root_state_w was not updated during the running process and was always the initial value (even if I executed robot.update_buffers(sim_dt) after each timestep)