PhysX error occurs when I move joint with efforts control and ROS message

Hi all!
I’m trying to write a simple extension to control Franka robot. In the extension I move joint with efforts control. And the efforts come from ROS message given by Message Publisher in RQT tools. I use the function named set_articulation_dof_efforts in ROS call-back function. I did this by modifying the example in Isaac Examples > Dynamic Control > Joint Controller.
When I run this extension, although the articulation is moved as I want, an error occurs:

2022-01-24 09:04:19 [542,234ms] [Error] [omni.physx.plugin] PhysX error: PxArticulationReducedCoordinate::copyInternalStateToCache() not allowed while simulation is running. Call will be ignored., FILE /buildAgent/work/c6f19e3bba7d41ab/source/physx/src/NpArticulationReducedCoordinate.cpp, LINE 197
2022-01-24 09:04:19 [542,234ms] [Error] [omni.physx.plugin] PhysX error: PxArticulationReducedCoordinate::applyCache() not allowed while simulation is running. Call will be ignored., FILE /buildAgent/work/c6f19e3bba7d41ab/source/physx/src/NpArticulationReducedCoordinate.cpp, LINE 166

Here is the code:

import os
import asyncio
import weakref
import carb
import omni
import omni.ui as ui
import omni.kit.test
from omni.kit.menu.utils import add_menu_items, remove_menu_items, MenuItemDescription
from omni.isaac.ui.ui_utils import setup_ui_headers, get_style, btn_builder, scrolling_frame_builder
from omni.isaac.core.utils.stage import open_stage_async
from omni.isaac.dynamic_control import _dynamic_control
from pxr import Usd
import omni.physx as _physx
import numpy as np
import rospy
from sensor_msgs.msg import JointState

EXTENSION_NAME = “Joint Controller”

def clamp(x, min_value, max_value):
return max(min(x, max_value), min_value)

def _print_body_rec(dc, body, indent_level=0):
indent = " " * indent_level
body_name = dc.get_rigid_body_name(body)
str_output = “%sBody: %s\n” % (indent, body_name)
for i in range(dc.get_rigid_body_child_joint_count(body)):
joint = dc.get_rigid_body_child_joint(body, i)
joint_name = dc.get_joint_name(joint)
child = dc.get_joint_child_body(joint)
child_name = dc.get_rigid_body_name(child)
str_output = str_output + “%s Joint: %s → %s\n” % (indent, joint_name, child_name)
str_output = str_output + _print_body_rec(dc, child, indent_level + 4)
return str_output

class Extension(omni.ext.IExt):

def on_startup(self, ext_id: str):
    self._dc = _dynamic_control.acquire_dynamic_control_interface()
    self._window = None
    self._physxIFace = _physx.acquire_physx_interface()
    self._physx_subscription = None
    self._timeline = omni.timeline.get_timeline_interface()
    self.ar = _dynamic_control.INVALID_HANDLE
    ext_manager = omni.kit.app.get_app().get_extension_manager()
    dc_ext_id = ext_manager.get_enabled_extension_id("omni.isaac.dynamic_control")
    self._asset_path = ext_manager.get_extension_path(dc_ext_id)
    self._ext_id = ext_id
    menu_items = [
        MenuItemDescription(name=EXTENSION_NAME, onclick_fn=lambda a=weakref.proxy(self): a._menu_callback())
    ]
    self._menu_items = [MenuItemDescription(name="Dynamic Control", sub_menu=menu_items)]
    add_menu_items(self._menu_items, "Isaac Examples")

def _menu_callback(self):
    self._build_ui()

def _build_ui(self):
    if not self._window:
        self._window = ui.Window(
            title=EXTENSION_NAME, width=500, height=500, visible=True, dockPreference=ui.DockPreference.LEFT_BOTTOM
        )
        with self._window.frame:
            with ui.VStack(spacing=5, height=0):
                title = "Robot Joint Controller Example"
                doc_link = "https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html"
                overview = "This example shows how move a robot arm by driving its joints."
                overview += "First press the 'Load Robot' button and then press `Move Joints` to simulate."
                overview += "\n\nPress the 'Open in IDE' button to view the source code."
                setup_ui_headers(self._ext_id, __file__, title, doc_link, overview)
                frame = ui.CollapsableFrame(
                    title="Command Panel",
                    height=0,
                    collapsed=False,
                    style=get_style(),
                    style_type_name_override="CollapsableFrame",
                    horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_AS_NEEDED,
                    vertical_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_ON,
                )
                with frame:
                    with ui.VStack(style=get_style(), spacing=5, height=0):
                        kwargs = {
                            "label": "Load Robot",
                            "type": "button",
                            "text": "Load",
                            "tooltip": "Loads a Robot Arm and sets its properties",
                            "on_clicked_fn": self._on_load_robot,
                        }
                        btn_builder(**kwargs)
                        kwargs = {
                            "label": "Move Joints",
                            "type": "button",
                            "text": "Move",
                            "tooltip": "Moves the Robot's Joints",
                            "on_clicked_fn": self._on_move_joints,
                        }
                        btn_builder(**kwargs)                      
                        kwargs = {
                            "label": "Start_ROS_Script",
                            "type": "button",
                            "text": "START",
                            "tooltip": "Start_ROS_Script",
                            "on_clicked_fn": self._on_start_ROS_thread,
                        }
                        btn_builder(**kwargs)                            
                        kwargs = {
                            "label": "Stop_ROS_Script",
                            "type": "button",
                            "text": "STOP",
                            "tooltip": "Stop_ROS_Script",
                            "on_clicked_fn": self._on_stop_ROS_thread,
                        }
                        btn_builder(**kwargs)                            
                        kwargs = {
                            "label": "Init_Node",
                            "type": "button",
                            "text": "Init_Node",
                            "tooltip": "Init_Node",
                            "on_clicked_fn": self._init_node,
                        }
                        btn_builder(**kwargs)                            
                        kwargs = {
                            "label": "Shut_down_Node",
                            "type": "button",
                            "text": "Shut_down",
                            "tooltip": "Shut_down_Node",
                            "on_clicked_fn": self._shutdown_node,
                        }
                        btn_builder(**kwargs)
                        self.dof_states_label = scrolling_frame_builder()
                        self.dof_props_label = scrolling_frame_builder()        
                
def _init_node(self):
    rospy.init_node("isaac_sim_trans")      

def _shutdown_node(self):
    rospy.signal_shutdown("isaac_sim_trans")        

def Joint_Command_init(self):        
    sub=rospy.Subscriber("/sim_joint_effort",JointState,callback=self.Joint_Command_callback)

def Joint_Command_callback(self,msg):   
    if self._dc.is_simulating():
        print("simulating")
        art = self._dc.get_articulation("/panda")
        num_dofs = self._dc.get_articulation_dof_count(art)
        dof_efforts = np.zeros(num_dofs, dtype=np.float32)
        for i,effort in enumerate(msg.effort):
            dof_efforts[i] = effort
        if art == _dynamic_control.INVALID_HANDLE:
            return
        self._dc.set_articulation_dof_efforts(art, dof_efforts)        

async def ROS_thread(self):
    print("start_new_thread!!!")
    self.Joint_Command_init()
    while self.rospy_flag:
        await asyncio.sleep(0.5)        

def _on_stop_ROS_thread(self):
    self.rospy_flag=False        

def _on_start_ROS_thread(self):
    self.rospy_flag=True
    asyncio.ensure_future(self.ROS_thread())

def on_shutdown(self):
    self._sub_stage_event = None
    self._physx_subscription = None
    remove_menu_items(self._menu_items, "Isaac Examples")
    self._window = None

async def _setup_scene(self):
    await open_stage_async(self._asset_path + "/data/usd/robots/franka/franka.usd")
    await omni.kit.app.get_app().next_update_async()
    self._viewport = omni.kit.viewport.get_default_viewport_window()
    self._viewport.set_camera_position("/OmniverseKit_Persp", 150, -150, 150, True)
    self._viewport.set_camera_target("/OmniverseKit_Persp", -96, 108, 0, True)
    self._timeline.play()
    await omni.kit.app.get_app().next_update_async()

def _on_load_robot(self):
    asyncio.ensure_future(self._setup_scene())

def _on_move_joints(self):
    self._physx_subscription = self._physxIFace.subscribe_physics_step_events(self._on_physics_step)
    self._timeline.play()
    self._sub_stage_event = (
        omni.usd.get_context().get_stage_event_stream().create_subscription_to_pop(self._on_stage_event)
    )

def _on_stage_event(self, event):
    if event.type == int(omni.usd.StageEventType.OPENED) or event.type == int(omni.usd.StageEventType.CLOSED):
        # stage was opened or closed, cleanup
        self._physx_subscription = None
        self.ar = _dynamic_control.INVALID_HANDLE

def _on_first_step(self):
    self.ar = self._dc.get_articulation("/panda")
    if self.ar == _dynamic_control.INVALID_HANDLE:
        carb.log_warn("'%s' is not an articulation, please click load button first" % "/panda")
        return False

    num_dofs = self._dc.get_articulation_dof_count(self.ar)
    dof_props = self._dc.get_articulation_dof_properties(self.ar)
    self.dof_props_label.text = str("--- Degree of freedom (DOF) properties:\n") +"num_dof:"+str(num_dofs)+"\n" + str(dof_props) + "\n"
    return True

def _on_physics_step(self, step):
    if self._dc.is_simulating():
        if self.ar == _dynamic_control.INVALID_HANDLE:
            if not self._on_first_step():
                return
        
        dof_states = self._dc.get_articulation_dof_states(self.ar, _dynamic_control.STATE_ALL)
        self.dof_states_label.text = str("--- Degree of freedom (DOF) states:\n") +"\n"+ str(dof_states) + "\n"

    return

Hers is the picture for the robot and the error:

What does it mean? How can solve the problem?
Thanks!

Hi @JayWong

An approach to avoid this error is to program the logic that set the DOF efforts inside the method subscribed to the physics step event…

In your case: self._dc.set_articulation_dof_efforts(art, dof_efforts) in the _on_physics_step method. Tips: set the art and dof_efforts as class properties, modify them in the ROS subscriber callback and use them in the _on_physics_step method.

1 Like

Thank you! It works! By the way, do you have some examples about combining dynamic control and ROS message, which are not using ROS bridge?

Hi,

Could you expand on your question with more specific descriptions of what you expect?

Thanks! Maybe I can find it myself.

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.