Skip to content

K
KincoActuatorIsaacLab
Commit 0d1738f2 authored by Kourosh Darvish's avatar Kourosh Darvish Committed by Mayank Mittal
Browse files
Options

Fixes bug in RmpFlowController and adds demo script (#19) 

* adds initialize to rmpflow controller
* adds demo script to play rmpflow
* fixes bugs in dealing with multiple command types
* adds right end-effector frame for franka lula
* adds demo script for rmpflow
* updates config and changelog

---------
Co-authored-by: 's avatarMayank Mittal <mittalma@leggedrobotics.com>
parent 629136c1
Show whitespace changes
Inline Side-by-side
Showing with 776 additions and 178 deletions
docs/source/refs/roadmap.rst
View file @ 0d1738f2
...@@ -20,7 +20,7 @@ If you have any questions or suggestions, let us know on ...@@ -20,7 +20,7 @@ If you have any questions or suggestions, let us know on
* |check_| Joint-space control * |check_| Joint-space control
* |check_| Differential inverse kinematics control * |check_| Differential inverse kinematics control
* |uncheck| Riemannian Motion Policies (RMPs) * |check_| Riemannian Motion Policies (RMPs)
* Supported robots * Supported robots
......
source/extensions/omni.isaac.orbit/config/extension.toml
View file @ 0d1738f2
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.3.0" version = "0.3.1"
# Description # Description
title = "ORBIT framework for Robot Learning" title = "ORBIT framework for Robot Learning"
......
source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
View file @ 0d1738f2
Changelog Changelog
--------- ---------
0.3.1 (2023-04-23)
~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added a modified version of ``lula_franka_gen.urdf`` which includes an end-effector frame.
* Added a standalone script ``play_rmpflow.py`` to show RMPFlow controller.
Fixed
^^^^^
* Fixed the splitting of commands in the :meth:`ActuatorGroup.compute` method. Earlier it was reshaping the
commands to the shape ``(num_actuators, num_commands)`` which was causing the commands to be split incorrectly.
* Fixed the processing of actuator command in the :meth:`RobotBase._process_actuators_cfg` to deal with multiple
command types when using "implicit" actuator group.
0.3.0 (2023-04-20) 0.3.0 (2023-04-20)
~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~
...@@ -26,6 +43,7 @@ Fixed ...@@ -26,6 +43,7 @@ Fixed
0.2.7 (2023-04-07) 0.2.7 (2023-04-07)
~~~~~~~~~~~~~~~~~~
Fixed Fixed
^^^^^ ^^^^^
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/group/actuator_group.py
View file @ 0d1738f2
...@@ -307,19 +307,19 @@ class ActuatorGroup: ...@@ -307,19 +307,19 @@ class ActuatorGroup:
# pre-processing of commands based on group. # pre-processing of commands based on group.
group_actions = self._format_command(group_actions) group_actions = self._format_command(group_actions)
# reshape actions for sub-groups # reshape actions for sub-groups
group_actions = group_actions.view(self.num_articulation, self.num_actuators, -1) group_actions = group_actions.split([self.num_actuators] * len(self.command_types), dim=-1)
# pre-process relative commands # pre-process relative commands
for index, command_type in enumerate(self.command_types): for command_type, command_value in zip(self.command_types, group_actions):
if command_type == "p_rel": if command_type == "p_rel":
group_des_dof_pos = self.dof_pos_scale * group_actions[..., index] + self._dof_pos group_des_dof_pos = self.dof_pos_scale * command_value + self._dof_pos
elif command_type == "p_abs": elif command_type == "p_abs":
group_des_dof_pos = self.dof_pos_scale * group_actions[..., index] + self.dof_pos_offset group_des_dof_pos = self.dof_pos_scale * command_value + self.dof_pos_offset
elif command_type == "v_rel": elif command_type == "v_rel":
group_des_dof_vel = self.dof_vel_scale * group_actions[..., index] + self._dof_vel group_des_dof_vel = self.dof_vel_scale * command_value + self._dof_vel
elif command_type == "v_abs": elif command_type == "v_abs":
group_des_dof_vel = self.dof_vel_scale * group_actions[..., index] # offset = 0 group_des_dof_vel = self.dof_vel_scale * command_value # offset = 0
elif command_type == "t_abs": elif command_type == "t_abs":
group_des_dof_torque = self.dof_torque_scale * group_actions[..., index] # offset = 0 group_des_dof_torque = self.dof_torque_scale * command_value # offset = 0
else: else:
raise ValueError(f"Invalid action command type for actuators: '{command_type}'.") raise ValueError(f"Invalid action command type for actuators: '{command_type}'.")
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/data/lula_franka_gen.urdf 0 → 100644
View file @ 0d1738f2
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from lula_franka.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<!-- This file has been modified from the one in "omni.isaac.motion_generation" to have
and end-effector frame that matches the official URDF of Franka arm. -->
<robot name="franka" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="aluminum">
<color rgba="0.5 0.5 0.5 1"/>
</material>
<!-- camera body, with origin at bottom screw mount -->
<joint name="camera_joint" type="fixed">
<origin rpy="0 -1.57079632679 3.14159265359" xyz="0.0358116 0 0.0360562"/>
<parent link="panda_hand"/>
<child link="camera_bottom_screw_frame"/>
</joint>
<link name="camera_bottom_screw_frame"/>
<joint name="camera_link_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0.02 0.0115"/>
<parent link="camera_bottom_screw_frame"/>
<child link="camera_link"/>
</joint>
<link name="camera_link">
<visual>
<origin rpy="1.57079632679 0 1.57079632679" xyz="0.00987 -0.02 0"/>
<geometry>
<box size="0.099 0.023 0.02005"/>
<!--<mesh filename="package://realsense2_camera/meshes/d415.stl" />-->
</geometry>
<material name="aluminum"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.02 0"/>
<geometry>
<box size="0.02005 0.099 0.023"/>
</geometry>
</collision>
<inertial>
<!-- The following are not reliable values, and should not be used for modeling -->
<mass value="0.564"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
</inertial>
</link>
<!-- camera depth joints and links -->
<joint name="camera_depth_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="camera_link"/>
<child link="camera_depth_frame"/>
</joint>
<link name="camera_depth_frame"/>
<joint name="camera_depth_optical_joint" type="fixed">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
<parent link="camera_depth_frame"/>
<child link="camera_depth_optical_frame"/>
</joint>
<link name="camera_depth_optical_frame"/>
<!-- camera left IR joints and links -->
<joint name="camera_left_ir_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0.0 0"/>
<parent link="camera_depth_frame"/>
<child link="camera_left_ir_frame"/>
</joint>
<link name="camera_left_ir_frame"/>
<joint name="camera_left_ir_optical_joint" type="fixed">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
<parent link="camera_left_ir_frame"/>
<child link="camera_left_ir_optical_frame"/>
</joint>
<link name="camera_left_ir_optical_frame"/>
<!-- camera right IR joints and links -->
<joint name="camera_right_ir_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.055 0"/>
<parent link="camera_depth_frame"/>
<child link="camera_right_ir_frame"/>
</joint>
<link name="camera_right_ir_frame"/>
<joint name="camera_right_ir_optical_joint" type="fixed">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
<parent link="camera_right_ir_frame"/>
<child link="camera_right_ir_optical_frame"/>
</joint>
<link name="camera_right_ir_optical_frame"/>
<!-- camera color joints and links -->
<joint name="camera_color_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0.015 0"/>
<parent link="camera_depth_frame"/>
<child link="camera_color_frame"/>
</joint>
<link name="camera_color_frame"/>
<joint name="camera_color_optical_joint" type="fixed">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
<parent link="camera_color_frame"/>
<child link="camera_color_optical_frame"/>
</joint>
<link name="camera_color_optical_frame"/>
<link name="panda_link0">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link0.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link0.stl"/>
</geometry>
</collision>
</link>
<link name="panda_link1">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link1.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link1.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="0 0 0" xyz="0 0 0.333"/>
<parent link="panda_link0"/>
<child link="panda_link1"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link2">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link2.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link2.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link1"/>
<child link="panda_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
</joint>
<link name="panda_link3">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link3.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link3.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
<parent link="panda_link2"/>
<child link="panda_link3"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link4">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link4.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link4.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
<parent link="panda_link3"/>
<child link="panda_link4"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
</joint>
<link name="panda_link5">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link5.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link5.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
<parent link="panda_link4"/>
<child link="panda_link5"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
</joint>
<link name="panda_link6">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link6.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link6.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link5"/>
<child link="panda_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
</joint>
<link name="panda_link7">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link7.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/link7.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
<parent link="panda_link6"/>
<child link="panda_link7"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
</joint>
<link name="panda_link8"/>
<joint name="panda_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="panda_link7"/>
<child link="panda_link8"/>
<axis xyz="0 0 0"/>
</joint>
<joint name="panda_hand_joint" type="fixed">
<parent link="panda_link8"/>
<child link="panda_hand"/>
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
</joint>
<link name="panda_hand">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/hand.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/hand.stl"/>
</geometry>
</collision>
</link>
<link name="panda_leftfinger">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
</geometry>
</collision>
</link>
<link name="panda_rightfinger">
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="package://franka_description/meshes/collision/finger.stl"/>
</geometry>
</collision>
</link>
<joint name="panda_finger_joint1" type="prismatic">
<parent link="panda_hand"/>
<child link="panda_leftfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
</joint>
<joint name="panda_finger_joint2" type="prismatic">
<parent link="panda_hand"/>
<child link="panda_rightfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 -1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="panda_finger_joint1"/>
</joint>
<link name="base_link"/>
<joint name="base_link_robot" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<axis xyz="0 0 1"/>
<parent link="base_link"/>
<child link="panda_link0"/>
</joint>
<link name="right_gripper"/>
<joint name="right_gripper" type="fixed">
<origin rpy="0 0 2.35619449019" xyz="0.0 0 0.1"/>
<axis xyz="0 0 1"/>
<parent link="panda_link8"/>
<child link="right_gripper"/>
</joint>
<link name="panda_rightfingertip"/>
<joint name="panda_rightfingertip" type="fixed">
<origin rpy="0 0 3.14159265359" xyz="0.0 0 0.045"/>
<axis xyz="0 0 1"/>
<parent link="panda_rightfinger"/>
<child link="panda_rightfingertip"/>
</joint>
<link name="panda_leftfingertip"/>
<joint name="panda_leftfingertip" type="fixed">
<origin rpy="0 0 3.14159265359" xyz="0.0 0 0.045"/>
<axis xyz="0 0 1"/>
<parent link="panda_leftfinger"/>
<child link="panda_leftfingertip"/>
</joint>
<link name="panda_forearm_end_pt"/>
<joint name="panda_forearm_end_pt" type="fixed">
<origin rpy="0 0 0.0" xyz="-0.09 0.02 0.0"/>
<axis xyz="0 0 1"/>
<parent link="panda_link4"/>
<child link="panda_forearm_end_pt"/>
</joint>
<link name="panda_forearm_mid_pt"/>
<joint name="panda_forearm_mid_pt" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.175 0.0"/>
<axis xyz="0 0 1"/>
<parent link="panda_forearm_end_pt"/>
<child link="panda_forearm_mid_pt"/>
</joint>
<link name="panda_forearm_mid_pt_shifted"/>
<joint name="panda_forearm_mid_pt_shifted" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 -0.05"/>
<axis xyz="0 0 1"/>
<parent link="panda_forearm_mid_pt"/>
<child link="panda_forearm_mid_pt_shifted"/>
</joint>
<link name="panda_forearm_distal"/>
<joint name="panda_forearm_distal" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.1 0.0"/>
<axis xyz="0 0 1"/>
<parent link="panda_link5"/>
<child link="panda_forearm_distal"/>
</joint>
<link name="panda_wrist_end_pt"/>
<joint name="panda_wrist_end_pt" type="fixed">
<origin rpy="0 0 0.0" xyz="0.0 0.0 -0.07"/>
<axis xyz="0 0 1"/>
<parent link="panda_link7"/>
<child link="panda_wrist_end_pt"/>
</joint>
<link name="panda_face_left"/>
<joint name="panda_face_left" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.07 0.04"/>
<axis xyz="0 0 1"/>
<parent link="panda_hand"/>
<child link="panda_face_left"/>
</joint>
<link name="panda_face_right"/>
<joint name="panda_face_right" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.07 0.04"/>
<axis xyz="0 0 1"/>
<parent link="panda_hand"/>
<child link="panda_face_right"/>
</joint>
<link name="panda_face_back_left"/>
<joint name="panda_face_back_left" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.07 0.0"/>
<axis xyz="0 0 1"/>
<parent link="panda_hand"/>
<child link="panda_face_back_left"/>
</joint>
<link name="panda_face_back_right"/>
<joint name="panda_face_back_right" type="fixed">
<origin rpy="0 0 0" xyz="0.0 -0.07 0.0"/>
<axis xyz="0 0 1"/>
<parent link="panda_hand"/>
<child link="panda_face_back_right"/>
</joint>
<!-- Modifications made to the one from Lula library -->
<link name="panda_end_effector"/>
<joint name="panda_end_effector" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="panda_hand"/>
<child link="panda_end_effector"/>
</joint>
</robot>
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/config/rmp_flow.py
View file @ 0d1738f2
...@@ -12,12 +12,14 @@ from omni.isaac.orbit.controllers.rmp_flow import RmpFlowControllerCfg ...@@ -12,12 +12,14 @@ from omni.isaac.orbit.controllers.rmp_flow import RmpFlowControllerCfg
# Note: RMP-Flow config files for supported robots are stored in the motion_generation extension # Note: RMP-Flow config files for supported robots are stored in the motion_generation extension
_RMP_CONFIG_DIR = os.path.join(get_extension_path_from_name("omni.isaac.motion_generation"), "motion_policy_configs") _RMP_CONFIG_DIR = os.path.join(get_extension_path_from_name("omni.isaac.motion_generation"), "motion_policy_configs")
# Path to current directory
_CUR_DIR = os.path.dirname(os.path.realpath(__file__))
FRANKA_RMPFLOW_CFG = RmpFlowControllerCfg( FRANKA_RMPFLOW_CFG = RmpFlowControllerCfg(
config_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "franka_rmpflow_common.yaml"), config_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "franka_rmpflow_common.yaml"),
urdf_file=os.path.join(_RMP_CONFIG_DIR, "franka", "lula_franka_gen.urdf"), urdf_file=os.path.join(_CUR_DIR, "data", "lula_franka_gen.urdf"),
collision_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "robot_descriptor.yaml"), collision_file=os.path.join(_RMP_CONFIG_DIR, "franka", "rmpflow", "robot_descriptor.yaml"),
frame_name="right_gripper", frame_name="panda_end_effector",
evaluations_per_frame=5, evaluations_per_frame=5,
) )
"""Configuration of RMPFlow for Franka arm (default from `omni.isaac.motion_generation`).""" """Configuration of RMPFlow for Franka arm (default from `omni.isaac.motion_generation`)."""
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_inverse_kinematics.py
View file @ 0d1738f2
...@@ -146,6 +146,10 @@ class DifferentialInverseKinematics: ...@@ -146,6 +146,10 @@ class DifferentialInverseKinematics:
Operations. Operations.
""" """
def initialize(self):
"""Initialize the internals."""
pass
def reset_idx(self, robot_ids: torch.Tensor = None): def reset_idx(self, robot_ids: torch.Tensor = None):
"""Reset the internals.""" """Reset the internals."""
pass pass
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/joint_impedance.py
View file @ 0d1738f2
...@@ -134,6 +134,10 @@ class JointImpedanceController: ...@@ -134,6 +134,10 @@ class JointImpedanceController:
Operations. Operations.
""" """
def initialize(self):
"""Initialize the internals."""
pass
def reset_idx(self, robot_ids: torch.Tensor = None): def reset_idx(self, robot_ids: torch.Tensor = None):
"""Reset the internals.""" """Reset the internals."""
pass pass
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/operational_space.py
View file @ 0d1738f2
...@@ -177,6 +177,10 @@ class OperationSpaceController: ...@@ -177,6 +177,10 @@ class OperationSpaceController:
Operations. Operations.
""" """
def initialize(self):
"""Initialize the internals."""
pass
def reset_idx(self, robot_ids: torch.Tensor = None): def reset_idx(self, robot_ids: torch.Tensor = None):
"""Reset the internals.""" """Reset the internals."""
pass pass
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow.py
View file @ 0d1738f2
...@@ -11,7 +11,7 @@ import omni.isaac.core.utils.prims as prim_utils ...@@ -11,7 +11,7 @@ import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.core.articulations import Articulation from omni.isaac.core.articulations import Articulation
from omni.isaac.core.simulation_context import SimulationContext from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.motion_generation import ArticulationMotionPolicy from omni.isaac.motion_generation import ArticulationMotionPolicy
from omni.isaac.motion_generation.lula import RmpFlow from omni.isaac.motion_generation.lula.motion_policies import RmpFlow, RmpFlowSmoothed
from omni.isaac.orbit.utils import configclass from omni.isaac.orbit.utils import configclass
...@@ -20,6 +20,8 @@ from omni.isaac.orbit.utils import configclass ...@@ -20,6 +20,8 @@ from omni.isaac.orbit.utils import configclass
class RmpFlowControllerCfg: class RmpFlowControllerCfg:
"""Configuration for RMP-Flow controller (provided through LULA library).""" """Configuration for RMP-Flow controller (provided through LULA library)."""
name: str = "rmp_flow"
"""Name of the controller. Supported: "rmp_flow", "rmp_flow_smoothed". Defaults to "rmp_flow"."""
config_file: str = MISSING config_file: str = MISSING
"""Path to the configuration file for the controller.""" """Path to the configuration file for the controller."""
urdf_file: str = MISSING urdf_file: str = MISSING
...@@ -28,36 +30,59 @@ class RmpFlowControllerCfg: ...@@ -28,36 +30,59 @@ class RmpFlowControllerCfg:
"""Path to collision model description of the robot.""" """Path to collision model description of the robot."""
frame_name: str = MISSING frame_name: str = MISSING
"""Name of the robot frame for task space (must be present in the URDF).""" """Name of the robot frame for task space (must be present in the URDF)."""
evaluations_per_frame: int = MISSING evaluations_per_frame: float = MISSING
"""Number of substeps during Euler integration inside LULA world model.""" """Number of substeps during Euler integration inside LULA world model."""
ignore_robot_state_updates: bool = False ignore_robot_state_updates: bool = False
"""If true, then state of the world model inside controller is rolled out. (default: False).""" """If true, then state of the world model inside controller is rolled out. Defaults to False."""
class RmpFlowController: class RmpFlowController:
"""Wraps around RMP-Flow from IsaacSim for batched environments.""" """Wraps around RMPFlow from IsaacSim for batched environments."""
def __init__(self, cfg: RmpFlowControllerCfg, prim_paths_expr: str, device: str): def __init__(self, cfg: RmpFlowControllerCfg, device: str):
"""Initialize the controller. """Initialize the controller.
Args: Args:
cfg (RmpFlowControllerCfg): The configuration for the controller. cfg (RmpFlowControllerCfg): The configuration for the controller.
prim_paths_expr (str): The expression to find the articulation prim paths.
device (str): The device to use for computation. device (str): The device to use for computation.
Raises:
NotImplementedError: When the robot name is not supported.
""" """
# store input # store input
self.cfg = cfg self.cfg = cfg
self._device = device self._device = device
# display info
print(f"[INFO]: Loading RMPFlow controller URDF from: {self.cfg.urdf_file}")
"""
Properties.
"""
print(f"[INFO]: Loading controller URDF from: {self.cfg.urdf_file}") @property
def num_actions(self) -> int:
"""Dimension of the action space of controller."""
return 7
"""
Operations.
"""
def initialize(self, prim_paths_expr: str):
"""Initialize the controller.
Args:
prim_paths_expr (str): The expression to find the articulation prim paths.
"""
# obtain the simulation time # obtain the simulation time
physics_dt = SimulationContext.instance().get_physics_dt() physics_dt = SimulationContext.instance().get_physics_dt()
# find all prims # find all prims
self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr) self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr)
self.num_robots = len(self._prim_paths) self.num_robots = len(self._prim_paths)
# resolve controller
if self.cfg.name == "rmp_flow":
controller_cls = RmpFlow
elif self.cfg.name == "rmp_flow_smoothed":
controller_cls = RmpFlowSmoothed
else:
raise ValueError(f"Unsupported controller in Lula library: {self.cfg.name}")
# create all franka robots references and their controllers # create all franka robots references and their controllers
self.articulation_policies = list() self.articulation_policies = list()
for prim_path in self._prim_paths: for prim_path in self._prim_paths:
...@@ -65,12 +90,12 @@ class RmpFlowController: ...@@ -65,12 +90,12 @@ class RmpFlowController:
robot = Articulation(prim_path) robot = Articulation(prim_path)
robot.initialize() robot.initialize()
# add controller # add controller
rmpflow = RmpFlow( rmpflow = controller_cls(
rmpflow_config_path=self.cfg.config_file,
urdf_path=self.cfg.urdf_file,
robot_description_path=self.cfg.collision_file, robot_description_path=self.cfg.collision_file,
urdf_path=self.cfg.urdf_file,
rmpflow_config_path=self.cfg.config_file,
end_effector_frame_name=self.cfg.frame_name, end_effector_frame_name=self.cfg.frame_name,
evaluations_per_frame=self.cfg.evaluations_per_frame, maximum_substep_size=physics_dt / self.cfg.evaluations_per_frame,
ignore_robot_state_updates=self.cfg.ignore_robot_state_updates, ignore_robot_state_updates=self.cfg.ignore_robot_state_updates,
) )
# wrap rmpflow to connect to the Franka robot articulation # wrap rmpflow to connect to the Franka robot articulation
...@@ -86,22 +111,14 @@ class RmpFlowController: ...@@ -86,22 +111,14 @@ class RmpFlowController:
self.dof_pos_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) self.dof_pos_target = torch.zeros((self.num_robots, self.num_dof), device=self._device)
self.dof_vel_target = torch.zeros((self.num_robots, self.num_dof), device=self._device) self.dof_vel_target = torch.zeros((self.num_robots, self.num_dof), device=self._device)
"""
Properties.
"""
@property
def num_actions(self) -> int:
"""Dimension of the action space of controller."""
return 7
"""
Operations.
"""
def reset_idx(self, robot_ids: torch.Tensor = None): def reset_idx(self, robot_ids: torch.Tensor = None):
"""Reset the internals.""" """Reset the internals."""
pass # if no robot ids are provided, then reset all robots
if robot_ids is None:
robot_ids = torch.arange(self.num_robots, device=self._device)
# reset policies for specified robots
for index in robot_ids:
self.articulation_policies[index].motion_policy.reset()
def set_command(self, command: torch.Tensor): def set_command(self, command: torch.Tensor):
"""Set target end-effector pose command.""" """Set target end-effector pose command."""
...@@ -127,9 +144,7 @@ class RmpFlowController: ...@@ -127,9 +144,7 @@ class RmpFlowController:
# apply action on the robot # apply action on the robot
action = policy.get_next_articulation_action() action = policy.get_next_articulation_action()
# copy actions into buffer # copy actions into buffer
# TODO: Make this more efficient? self.dof_pos_target[i, :] = torch.from_numpy(action.joint_positions[:]).to(self.dof_pos_target)
for dof_index in range(self.num_dof): self.dof_vel_target[i, :] = torch.from_numpy(action.joint_velocities[:]).to(self.dof_vel_target)
self.dof_pos_target[i, dof_index] = action.joint_positions[dof_index]
self.dof_vel_target[i, dof_index] = action.joint_velocities[dof_index]
return self.dof_pos_target, self.dof_vel_target return self.dof_pos_target, self.dof_vel_target
source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/rmp_flow_smoothed.py deleted 100644 → 0
View file @ 629136c1
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import os
import torch
from typing import Tuple
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.utils.prims import find_matching_prim_paths
from omni.isaac.motion_generation import ArticulationMotionPolicy
from omni.isaac.motion_generation.lula import RmpFlow
class RmpFlowController:
"""Wraps around RMP-Flow from IsaacSim for batched environments."""
def __init__(self, prim_paths_expr: str, robot_name: str, dt: float, device: str):
"""Initialize the controller.
Args:
prim_paths_expr (str): The expression to find the articulation prim paths.
robot_name (str): The name of the robot. Supported robots: franka, ur10.
dt (float): The time step of the simulation.
device (str): The device to use for computation.
Raises:
NotImplementedError: When the robot name is not supported.
"""
# store input
self._device = device
# Load configuration for the controller
# Note: RMP-Flow config files for supported robots are stored in the motion_generation extension
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
# configuration for the robot
if robot_name == "franka":
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs", "franka")
robot_description_path = os.path.join(rmp_config_dir, "rmpflow", "robot_descriptor.yaml")
robot_urdf_path = os.path.join(rmp_config_dir, "lula_franka_gen.urdf")
rmpflow_config_path = os.path.join(rmp_config_dir, "rmpflow", "franka_rmpflow_common.yaml")
self.arm_dof_num = 7
self.ee_frame_name = "right_gripper"
elif robot_name == "ur10":
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs", "ur10")
robot_description_path = os.path.join(rmp_config_dir, "rmpflow", "ur10_robot_description.yaml")
robot_urdf_path = os.path.join(rmp_config_dir, "ur10_robot.urdf")
rmpflow_config_path = os.path.join(rmp_config_dir, "rmpflow", "ur10_rmpflow_config.yaml")
self.arm_dof_num = 6
self.ee_frame_name = "ee_link"
else:
raise NotImplementedError(f"Input robot has no configuration: {robot_name}")
print(f"[INFO]: Loading controller URDF from: {robot_urdf_path}")
# find all prims
self._prim_paths = find_matching_prim_paths(prim_paths_expr)
self.num_robots = len(self._prim_paths)
# create all franka robots references and their controllers
self.articulation_policies = list()
for prim_path in self._prim_paths:
# add robot reference
robot = Articulation(prim_path)
robot.initialize()
# add controller
rmpflow = RmpFlow(
robot_description_path=robot_description_path,
urdf_path=robot_urdf_path,
rmpflow_config_path=rmpflow_config_path,
end_effector_frame_name=self.ee_frame_name,
evaluations_per_frame=5,
)
# wrap rmpflow to connect to the Franka robot articulation
articulation_policy = ArticulationMotionPolicy(robot, rmpflow, dt)
self.articulation_policies.append(articulation_policy)
# create buffers
# -- for storing command
self.desired_ee_pos = None
self.desired_ee_rot = None
# -- for policy output
self.joint_positions = torch.zeros((self.num_robots, self.arm_dof_num), device=self._device)
self.joint_velocities = torch.zeros((self.num_robots, self.arm_dof_num), device=self._device)
# update timer
self._update_timer = torch.zeros(self.num_robots, device=self._device)
"""
Operations.
"""
def reset_idx(self, robot_ids: torch.Tensor = None):
"""Reset the internals."""
if robot_ids is None:
robot_ids = ...
self._update_timer[robot_ids] = 0.0
def set_command(self, desired_ee_pos: torch.Tensor, desired_ee_rot: torch.Tensor):
"""Set target end-effector pose command."""
# convert pose to numpy
self.desired_ee_pos = desired_ee_pos.cpu().numpy()
self.desired_ee_rot = desired_ee_rot.cpu().numpy()
def advance(self, dt: float) -> Tuple[torch.Tensor, torch.Tensor]:
"""Performs inference with the controller.
Returns:
Tuple[torch.Tensor, torch.Tensor]: The target joint positions and velocity commands.
"""
# check valid
if self.desired_ee_pos is None or self.desired_ee_rot is None:
raise RuntimeError("Target command has not been set. Please call `set_command()` first.")
# update timer
self._update_timer += dt
# compute control actions
for i, policy in enumerate(self.articulation_policies):
# set rmpflow target to be the current position of the target cube.
policy.get_motion_policy().set_end_effector_target(
target_position=self.desired_ee_pos[i], target_orientation=self.desired_ee_rot[i]
)
# apply action on the robot
action = policy.get_next_articulation_action()
# copy actions into buffer
# arm-action
for dof_index in range(self.arm_dof_num):
if action.joint_positions[dof_index] is not None:
self.joint_positions[i, dof_index] = torch.tensor(
action.joint_positions[dof_index], device=self._device
)
if action.joint_velocities[dof_index] is not None:
self.joint_velocities[i, dof_index] = torch.tensor(
action.joint_velocities[dof_index], device=self._device
)
return self.joint_positions, self.joint_velocities
source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/config/universal_robots.py
View file @ 0d1738f2
...@@ -41,7 +41,7 @@ UR10_CFG = SingleArmManipulatorCfg( ...@@ -41,7 +41,7 @@ UR10_CFG = SingleArmManipulatorCfg(
}, },
dof_vel={".*": 0.0}, dof_vel={".*": 0.0},
), ),
ee_info=SingleArmManipulatorCfg.EndEffectorFrameCfg(body_name="wrist_3_link"), ee_info=SingleArmManipulatorCfg.EndEffectorFrameCfg(body_name="ee_link"),
actuator_groups={ actuator_groups={
"arm": ActuatorGroupCfg( "arm": ActuatorGroupCfg(
dof_names=[".*"], dof_names=[".*"],
......
source/extensions/omni.isaac.orbit/omni/isaac/orbit/robots/robot_base.py
View file @ 0d1738f2
...@@ -446,7 +446,22 @@ class RobotBase: ...@@ -446,7 +446,22 @@ class RobotBase:
# store actuator group # store actuator group
self.actuator_groups[group_name] = actuator_group self.actuator_groups[group_name] = actuator_group
# store the control mode and dof indices (optimization) # store the control mode and dof indices (optimization)
self.sim_dof_control_modes[actuator_group.control_mode].extend(actuator_group.dof_indices) if actuator_group.model_type == "implicit":
for command in actuator_group.command_types:
# resolve name of control mode
if "p" in command:
command_name = "position"
elif "v" in command:
command_name = "velocity"
elif "t" in command:
command_name = "effort"
else:
continue
# store dof indices
self.sim_dof_control_modes[command_name].extend(actuator_group.dof_indices)
else:
# in explicit mode, we always use the "effort" control mode
self.sim.dof_control_mode["effort"].extend(actuator_group.dof_indices)
# perform some sanity checks to ensure actuators are prepared correctly # perform some sanity checks to ensure actuators are prepared correctly
total_act_dof = sum(group.num_actuators for group in self.actuator_groups.values()) total_act_dof = sum(group.num_actuators for group in self.actuator_groups.values())
......
source/standalone/demo/play_ik_control.py
View file @ 0d1738f2
...@@ -140,6 +140,7 @@ def main(): ...@@ -140,6 +140,7 @@ def main():
# Acquire handles # Acquire handles
# Initialize handles # Initialize handles
robot.initialize("/World/envs/env_.*/Robot") robot.initialize("/World/envs/env_.*/Robot")
ik_controller.initialize()
# Reset states # Reset states
robot.reset_buffers() robot.reset_buffers()
ik_controller.reset_idx() ik_controller.reset_idx()
...@@ -214,7 +215,7 @@ def main(): ...@@ -214,7 +215,7 @@ def main():
# apply actions # apply actions
robot.apply_action(robot_actions) robot.apply_action(robot_actions)
# perform step # perform step
sim.step() sim.step(render=not args_cli.headless)
# update sim-time # update sim-time
sim_time += sim_dt sim_time += sim_dt
count += 1 count += 1
......
source/standalone/demo/play_rmpflow.py 0 → 100644
View file @ 0d1738f2
# 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 RMPFlow controller with the simulator.
The RMP-Flow can be configured in different modes. It uses the LULA library for motion generation.
"""
"""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("--headless", action="store_true", default=False, help="Force display off at all times.")
parser.add_argument("--robot", type=str, default="ur10", help="Name of the robot. Options: franka_panda, ur10.")
parser.add_argument("--num_envs", type=int, default=5, help="Number of environments to spawn.")
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.cloner import GridCloner
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.carb import set_carb_setting
from omni.isaac.core.utils.viewports import set_camera_view
import omni.isaac.orbit.utils.kit as kit_utils
from omni.isaac.orbit.controllers.config.rmp_flow import FRANKA_RMPFLOW_CFG, UR10_RMPFLOW_CFG
from omni.isaac.orbit.controllers.rmp_flow import RmpFlowController
from omni.isaac.orbit.markers import StaticMarker
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 commands through RMPFlow kinematics control."""
# 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])
# Enable GPU pipeline and flatcache
if sim.get_physics_context().use_gpu_pipeline:
sim.get_physics_context().enable_flatcache(True)
# Enable hydra scene-graph instancing
set_carb_setting(sim._settings, "/persistent/omnihydra/useSceneGraphInstancing", True)
# Create interface to clone the scene
cloner = GridCloner(spacing=2.0)
cloner.define_base_env("/World/envs")
# Everything under the namespace "/World/envs/env_0" will be cloned
prim_utils.define_prim("/World/envs/env_0")
# Spawn things into stage
# Markers
ee_marker = StaticMarker("/Visuals/ee_current", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1))
goal_marker = StaticMarker("/Visuals/ee_goal", count=args_cli.num_envs, scale=(0.1, 0.1, 0.1))
# 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/envs/env_0/Table", usd_path=table_usd_path)
# -- Robot
# resolve robot config from command-line arguments
if args_cli.robot == "franka_panda":
# robot config
robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
robot_cfg.actuator_groups["panda_shoulder"].control_cfg.command_types = ["p_abs", "v_abs"]
robot_cfg.actuator_groups["panda_forearm"].control_cfg.command_types = ["p_abs", "v_abs"]
# rmpflow controller config
rmpflow_cfg = FRANKA_RMPFLOW_CFG
elif args_cli.robot == "ur10":
# robot config
robot_cfg = UR10_CFG
robot_cfg.actuator_groups["arm"].control_cfg.command_types = ["p_abs", "v_abs"]
# rmpflow controller config
rmpflow_cfg = UR10_RMPFLOW_CFG
else:
raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
# configure robot settings to use IK controller
robot_cfg.data_info.enable_jacobian = True
robot_cfg.rigid_props.disable_gravity = True
# spawn robot
robot = SingleArmManipulator(cfg=robot_cfg)
robot.spawn("/World/envs/env_0/Robot", translation=(0.0, 0.0, 0.0))
# Clone the scene
num_envs = args_cli.num_envs
envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs)
envs_positions = cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths)
# convert environment positions to torch tensor
envs_positions = torch.tensor(envs_positions, dtype=torch.float, device=sim.device)
# filter collisions within each environment instance
physics_scene_path = sim.get_physics_context().prim_path
cloner.filter_collisions(
physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"]
)
# Create controller
# the controller takes as command type: {position/pose}_{abs/rel}
rmp_controller = RmpFlowController(cfg=rmpflow_cfg, device=sim.device)
# Play the simulator
sim.reset()
# Acquire handles
# Initialize handles
robot.initialize("/World/envs/env_.*/Robot")
rmp_controller.initialize("/World/envs/env_.*/Robot")
# Reset states
robot.reset_buffers()
rmp_controller.reset_idx()
# Now we are ready!
print("[INFO]: Setup complete...")
# Create buffers to store actions
rmp_commands = torch.zeros(robot.count, rmp_controller.num_actions, device=robot.device)
robot_actions = torch.ones(robot.count, robot.num_actions, device=robot.device)
has_gripper = robot.cfg.meta_info.tool_num_dof > 0
# Set end effector goals
# Define goals for the arm
ee_goals = [
[0.5, 0.5, 0.7, 0.707, 0, 0.707, 0],
[0.5, -0.4, 0.6, 0.707, 0.707, 0.0, 0.0],
[0.5, 0, 0.5, 0.0, 1.0, 0.0, 0.0],
]
ee_goals = torch.tensor(ee_goals, device=sim.device)
# Track the given command
current_goal_idx = 0
rmp_commands[:] = ee_goals[current_goal_idx]
# Define simulation stepping
sim_dt = sim.get_physics_dt()
# episode counter
sim_time = 0.0
count = 0
# Note: We need to update buffers before the first step for the controller.
robot.update_buffers(sim_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
# reset
if count % 350 == 0:
# reset time
count = 0
sim_time = 0.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 controller
rmp_controller.reset_idx()
# reset actions
rmp_commands[:] = ee_goals[current_goal_idx]
# change goal
current_goal_idx = (current_goal_idx + 1) % len(ee_goals)
# step simulation
# FIXME: this is needed for lula to update the buffers!
# the bug has been reported to the lula team
sim.step(render=not args_cli.headless)
# set the controller commands
rmp_controller.set_command(rmp_commands)
# compute the joint commands
desired_joint_pos, desired_joint_vel = rmp_controller.compute()
# in some cases the zero action correspond to offset in actuators
# so we need to subtract these over here so that they can be added later on
arm_command_offset = robot.data.actuator_pos_offset[:, : robot.arm_num_dof]
# offset actuator command with position offsets
# note: valid only when doing position control of the robot
desired_joint_pos -= arm_command_offset
# set the desired joint position and velocity into buffers
# note: this is a hack for now to deal with the fact that the robot has two different
# command types. We need to split and interleave the commands for the robot.
# This will be fixed in the future.
# get all the actuator groups
group_num_actuators = [group.num_actuators for group in robot.actuator_groups.values()]
if has_gripper:
# remove gripper from the list
group_num_actuators = group_num_actuators[:-1]
# set command for the arm
group_desired_joint_pos = desired_joint_pos.split(group_num_actuators, dim=-1)
group_desired_joint_vel = desired_joint_vel.split(group_num_actuators, dim=-1)
# interleave the commands
robot_actions_list = [None] * (len(group_desired_joint_pos) + len(group_desired_joint_vel))
robot_actions_list[::2] = group_desired_joint_pos
robot_actions_list[1::2] = group_desired_joint_vel
# concatenate the list
robot_actions[:, : 2 * sum(group_num_actuators)] = torch.cat(robot_actions_list, dim=-1)
# apply actions
robot.apply_action(robot_actions)
# perform step
sim.step(not args_cli.headless)
# update sim-time
sim_time += sim_dt
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)
# update marker positions
ee_marker.set_world_poses(robot.data.ee_state_w[:, 0:3], robot.data.ee_state_w[:, 3:7])
goal_marker.set_world_poses(rmp_commands[:, 0:3] + envs_positions, rmp_commands[:, 3:7])
if __name__ == "__main__":
# Run IK example with Manipulator
main()
# Close the simulator
simulation_app.close()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment