Omnigraph with ROS1 Bridge: Python Scripting Not Working

Omniverse Isaac Sim
1

Hi,

I am using Isaac Sim 2022.2.0.

I am trying to make the “Joint Control Omni Graph with Python Scripting”. I followed the online tutorial using visual scripting: 7.1.10. Joint Control: Extension Python Scripting — Omniverse IsaacSim latest documentation . It works great.

But when I try to integrate it with my own code using franka robot from the server (_FRANKA_PANDA_ARM_INSTANCEABLE_USD = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd"). It does not work at all.

While the robot joint positions are not zeros, the topic “/joint_states” publishes always :

header:

seq: 10716

stamp:

secs: 126

nsecs: 379997175

frame_id: ‘’

name:

  • panda_joint1

  • panda_joint2

  • panda_joint3

  • panda_joint4

  • panda_joint5

  • panda_joint6

  • panda_joint7

  • panda_finger_joint1

  • panda_finger_joint2

position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

effort: [0.0, -3.611, 0.0, -3.8255, 0.0, 1.6987, 0.0, 0.0, 0.0]

And the “/joint_command” topis is not getting executed.

In addition, I get this warning during execution:

2024-01-08 14:31:38 [131,497ms] [Warning] [omni.graph.core.plugin] /ActionGraph/ArticulationController: OmniGraph Warning: Index put requires the source and destination dtypes match, got Float for the destination and Double for the source.

               (from compute() at line 110 in /home/xxx/xxx/_isaac_sim/exts/omni.isaac.core_nodes/omni/isaac/core_nodes/ogn/nodes/OgnIsaacArticulationController.py)

Could you please take at look at my implementation below and let me know what was the issue? Thanks!

Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto

All rights reserved.

SPDX-License-Identifier: BSD-3-Clause

“”"
This script demonstrates how to use the physics engine to simulate a single-arm manipulator.

We currently support the following robots:

  • Franka Emika Panda
  • Universal Robot UR10

From the default configuration file for these robots, zero actions imply a default pose.
“”"

“”“Launch Isaac Sim Simulator first.”“”

import argparse
from omni.isaac.kit import SimulationApp

add argparse arguments

parser = argparse.ArgumentParser(“Welcome to Orbit: Omniverse Robotics Environments!”)
parser.add_argument(“–robot”, type=str, default=“franka_panda”, help=“Name of the robot.”)
args_cli = parser.parse_args()

This sample enables a livestream server to connect to when running headless

CONFIG = {
“width”: 1280,
“height”: 720,
“window_width”: 1920,
“window_height”: 1080,
“headless”: False,
“renderer”: “RayTracedLighting”,
“display_options”: 3286, # Set display options to show default grid
}

launch omniverse app

simulation_app = SimulationApp(launch_config=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.extensions import enable_extension
from omni.isaac.core.utils.viewports import set_camera_view

import omni.isaac.orbit.utils.kit as kit_utils
from omni.isaac.orbit.robots.single_arm import SingleArmManipulator
from omni.isaac.core.utils.carb import set_carb_setting

from omni.isaac.orbit.robots.config.franka_roma import ROMA_FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG

from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG

enable_extension(“omni.isaac.ros_bridge”)
import omni.graph.core as og
from omni.isaac.core_nodes.scripts.utils import set_target_prims

“”"
Main
“”"

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

# Default Livestream settings
simulation_app.set_setting("/app/window/drawMouse", True)
simulation_app.set_setting("/app/livestream/proto", "ws")
simulation_app.set_setting("/app/livestream/websocket/framerate_limit", 120)
simulation_app.set_setting("/ngx/enabled", False)
# Note: Only one livestream extension can be enabled at a time
# Enable Native Livestream extension
# Default App: Streaming Client from the Omniverse Launcher
enable_extension("omni.kit.livestream.native")

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

# ############# Added this change to solve bud related to GPU instancind
# Enable fltacache which avoids passing data over to USD structure
# this speed up the read-write operation of GPU buffers
if sim.get_physics_context().use_gpu_pipeline:
    sim.get_physics_context().enable_flatcache(True)
# Enable hydra scene-graph instancing
# this is needed to visualize the scene when flatcache is enabled
set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True)
# ###############

# 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)},
)

# Robots
robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG

# enable filling buffers for dyamic variables
robot_cfg.data_info.enable_coriolis = True
robot_cfg.data_info.enable_mass_matrix = True
robot_cfg.data_info.enable_gravity = True
robot_cfg.data_info.enable_jacobian = True

# -- Spawn robot
robot = SingleArmManipulator(cfg=robot_cfg)

robot.spawn("/World/myRobot", translation=(0.0, 0.0, 0.0))

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

keys = og.Controller.Keys  # Usually handy to keep this name short as it is used often
controller = og.Controller()

(graph, nodes, prims, name_to_object_map) = og.Controller.edit(
{"graph_path": "/ActionGraph", "evaluator_name": "execution"},
{
    og.Controller.Keys.CREATE_NODES: [
        ("OnPlaybackTick", "omni.graph.action.OnPlaybackTick"),
        ("PublishJointState", "omni.isaac.ros_bridge.ROS1PublishJointState"),
        ("SubscribeJointState", "omni.isaac.ros_bridge.ROS1SubscribeJointState"),
        ("ArticulationController", "omni.isaac.core_nodes.IsaacArticulationController"),
        ("ReadSimTime", "omni.isaac.core_nodes.IsaacReadSimulationTime"),
    ],

    og.Controller.Keys.CONNECT: [
        ("OnPlaybackTick.outputs:tick", "PublishJointState.inputs:execIn"),
        ("OnPlaybackTick.outputs:tick", "SubscribeJointState.inputs:execIn"),
        ("OnPlaybackTick.outputs:tick", "ArticulationController.inputs:execIn"),
        ("ReadSimTime.outputs:simulationTime", "PublishJointState.inputs:timeStamp"),
        ("SubscribeJointState.outputs:execOut", "ArticulationController.inputs:execIn"),
        ("SubscribeJointState.outputs:jointNames", "ArticulationController.inputs:jointNames"),
        ("SubscribeJointState.outputs:positionCommand", "ArticulationController.inputs:positionCommand"),
        ("SubscribeJointState.outputs:velocityCommand", "ArticulationController.inputs:velocityCommand"),
        ("SubscribeJointState.outputs:effortCommand", "ArticulationController.inputs:effortCommand"),
    ],

    og.Controller.Keys.SET_VALUES: [
        # # Setting topic names to joint state publisher and subscriber.
        ("PublishJointState.inputs:topicName", "/joint_states"),
        ("SubscribeJointState.inputs:topicName", "/joint_command"),

        # Providing path to /panda robot to Articulation Controller node
        # Providing the robot path is equivalent to setting the targetPrim in Articulation Controller node
        ("ArticulationController.inputs:usePath", True),
        ("ArticulationController.inputs:robotPath", "/World/myRobot"),
    ],
    },
)
set_target_prims(
        primPath="/ActionGraph" + "/PublishJointState", inputName="inputs:targetPrim", targetPrimPaths=["/World/myRobot"]
    )

# 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()
# episode counter
sim_time = 0.0
ep_step_count = 0
# 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
    # reset
    if ep_step_count % 1000 == 0:
        sim_time = 0.0
        ep_step_count = 0
        # reset dof state
        dof_pos, dof_vel = robot.get_default_dof_state()
        robot.set_dof_state(dof_pos, dof_vel)
        robot.reset_buffers()
        # reset command
        actions = torch.rand(robot.count, robot.num_actions, device=robot.device)
        # reset gripper
        if has_gripper:
            actions[:, -1] = -1
        print("[INFO]: Resetting robots state...")
    # change the gripper action
    if ep_step_count % 200 == 0 and has_gripper:
        # flip command for the gripper
        actions[:, -1] = -actions[:, -1]
        print(f"[INFO]: [Step {ep_step_count:03d}]: Flipping gripper command...")
    # apply action to the robot
    # robot.apply_action(actions)
    # perform step
    sim.step()
    # update sim-time
    sim_time += sim_dt
    ep_step_count += 1
    # note: to deal with timeline events such as stopping, we need to check if the simulation is playing
    if sim.is_playing():
        # update buffers
        robot.update_buffers(sim_dt)
        
        # # print dynamic parameters
        # print("ee_jacobian : ", robot.data.ee_jacobian[:])
        # print("mass_matrix : ", robot.data.mass_matrix[:])
        # print("coriolis    : ", robot.data.coriolis[:])
        # print("gravity     : ", robot.data.gravity[:])

if name == “main”:
# Run the main function
main()
# Close the simulator
simulation_app.close()