.. _isaac_dynamic_control: =============================== Dynamic Control =============================== .. _isaac_dynamic_control_about: About ================== The :ref:`isaac_dynamic_control` Extension is used in |isaac-sim_short| as a core, high-level interface to PhysX for different physics objects in a scene. The extension provides opaque handles that remain valid between PhysX scene resets. This means the handles are persistent, even even when the simulation starts / stops when the ``PLAY`` and ``STOP`` buttons are pressed. .. _isaac_dynamic_control_api_doc: API Documentation ================== See the |link_ext| for complete usage information. .. |link_ext| raw:: html API Documentation .. _isaac_dynamic_control_code_snippets: Code Snippets ===================== To run the following code snippets: #. The following code snippets assume that the :ref:`isaac_sim_glossary_stage` contains a `Franka` robot at the ``/Franka`` prim path. Go to the top menu bar and click `Create > Isaac > Robots > From Library > Manipulators > Franka` to add a Franka to the scene. #. At least one frame of simulation must occur before the Dynamic Control APIs work correctly. To start the simulation: #. `(Option #1)` Press the ``PLAY`` button to begin simulating. #. `(Option #2)` Use the following code snippet to start the simulation using the Python API before running any of the snippets below. .. code-block:: python :linenos: import omni omni.timeline.get_timeline_interface().play() We recommend using the built-in Script Editor to test these snippets. For deeper development, please see the :ref:`isaac_sim_app_tutorial_intro_workflows` tutorial. To access the Script Editor, go to the top menu bar and click `Window > Script Editor`. .. note:: The snippets are disparate examples, running them out of order may have unintended consequences. .. note:: The snippets are for demonstrative purposes, the resulting movements may not respect the robot's kinematic limitations. Position Control ^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: :emphasize-lines: 5-6 from omni.isaac.dynamic_control import _dynamic_control import numpy as np dc = _dynamic_control.acquire_dynamic_control_interface() articulation = dc.get_articulation("/Franka") # Call this each frame of simulation step if the state of the articulation is changing. dc.wake_up_articulation(articulation) joint_angles = [np.random.rand(9) * 2 - 1] dc.set_articulation_dof_position_targets(articulation, joint_angles) Single DOF Position Control ^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: from omni.isaac.dynamic_control import _dynamic_control import numpy as np dc = _dynamic_control.acquire_dynamic_control_interface() articulation = dc.get_articulation("/Franka") dc.wake_up_articulation(articulation) dof_ptr = dc.find_articulation_dof(articulation, "panda_joint2") dc.set_dof_position_target(dof_ptr, -1.5) Velocity Control ^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: :emphasize-lines: 10, 14-15 from pxr import UsdPhysics stage = omni.usd.get_context().get_stage() for prim in stage.TraverseAll(): prim_type = prim.GetTypeName() if prim_type in ["PhysicsRevoluteJoint" , "PhysicsPrismaticJoint"]: if prim_type == "PhysicsRevoluteJoint": drive = UsdPhysics.DriveAPI.Get(prim, "angular") else: drive = UsdPhysics.DriveAPI.Get(prim, "linear") drive.GetStiffnessAttr().Set(0) from omni.isaac.dynamic_control import _dynamic_control import numpy as np dc = _dynamic_control.acquire_dynamic_control_interface() #Note: getting the articulation has to happen after changing the drive stiffness articulation = dc.get_articulation("/Franka") dc.wake_up_articulation(articulation) joint_vels = [-np.random.rand(9)*10] dc.set_articulation_dof_velocity_targets(articulation, joint_vels) Single DOF Velocity Control ^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: :emphasize-lines: 4, 8-9 from pxr import UsdPhysics stage = omni.usd.get_context().get_stage() panda_joint2_drive = UsdPhysics.DriveAPI.Get(stage.GetPrimAtPath("/Franka/panda_link1/panda_joint2"), "angular") panda_joint2_drive.GetStiffnessAttr().Set(0) from omni.isaac.dynamic_control import _dynamic_control import numpy as np dc = _dynamic_control.acquire_dynamic_control_interface() #Note: getting the articulation has to happen after changing the drive stiffness articulation = dc.get_articulation("/Franka") dc.wake_up_articulation(articulation) dof_ptr = dc.find_articulation_dof(articulation, "panda_joint2") dc.set_dof_velocity_target(dof_ptr, 0.2) Torque Control ^^^^^^^^^^^^^^ .. code-block:: python :linenos: :emphasize-lines: 6-7 from omni.isaac.dynamic_control import _dynamic_control import numpy as np dc = _dynamic_control.acquire_dynamic_control_interface() articulation = dc.get_articulation("/Franka") dc.wake_up_articulation(articulation) joint_efforts = [-np.random.rand(9) * 1000] dc.set_articulation_dof_efforts(articulation, joint_efforts) Check Object Type ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: from omni.isaac.dynamic_control import _dynamic_control dc = _dynamic_control.acquire_dynamic_control_interface() # Check to see what type of object the target prim is obj_type = dc.peek_object_type("/Franka") # This print statement should print ObjectType.OBJECT_ARTICULATION print(obj_type) Query Articulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: from omni.isaac.dynamic_control import _dynamic_control dc = _dynamic_control.acquire_dynamic_control_interface() # Get a handle to the Franka articulation # This handle will automatically update if simulation is stopped and restarted art = dc.get_articulation("/Franka") # Get information about the structure of the articulation num_joints = dc.get_articulation_joint_count(art) num_dofs = dc.get_articulation_dof_count(art) num_bodies = dc.get_articulation_body_count(art) # Get a specific degree of freedom on an articulation dof_ptr = dc.find_articulation_dof(art, "panda_joint2") Read Joint State ^^^^^^^^^^^^^^^^ .. code-block:: python :linenos: from omni.isaac.dynamic_control import _dynamic_control dc = _dynamic_control.acquire_dynamic_control_interface() # Print the state of each degree of freedom in the articulation art = dc.get_articulation("/Franka") dof_states = dc.get_articulation_dof_states(art, _dynamic_control.STATE_ALL) print(dof_states) # Get state for a specific degree of freedom dof_ptr = dc.find_articulation_dof(art, "panda_joint2") dof_state = dc.get_dof_state(dof_ptr, _dynamic_control.STATE_ALL) # print position for the degree of freedom print(dof_state.pos)