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!