merge release 1.5.1 from dev to main

Author: kellyguo11

docs/release_notes.md

@@ -1,6 +1,10 @@
 Release Notes
 =============
 
+1.5.1
+-----
+* Fix bug in IndustRealSim example - overwrite `generate_ctrl_signals`, `_set_dof_pos_target`, and `_set_dof_torque` in `industreal_base.py` to resolve `fingertip_midpoint` and `fingertip_centered` discrepancy
+
 1.5.0
 -----
 

isaacgymenvs/cfg/task/IndustRealTaskGearsInsert.yaml

@@ -50,17 +50,17 @@ rl:
     num_keypoints: 4  # number of keypoints used in reward
     keypoint_scale: 0.5  # length of line of keypoints
 
-    max_episode_length: 256
+    max_episode_length: 128
 
     # SAPU
     interpen_thresh: 0.001  # max allowed interpenetration between gear and shaft
 
     # SDF-Based Reward
     sdf_reward_scale: 10.0
-    sdf_reward_num_samples: 1000
+    sdf_reward_num_samples: 5000
 
     # SBC
-    initial_max_disp: 0.015  # max initial downward displacement of gear at beginning of curriculum
+    initial_max_disp: 0.01  # max initial downward displacement of gear at beginning of curriculum
     curriculum_success_thresh: 0.6  # success rate threshold for increasing curriculum difficulty
     curriculum_failure_thresh: 0.3  # success rate threshold for decreasing curriculum difficulty
     curriculum_height_step: [-0.005, 0.002]  # how much to increase max initial downward displacement after hitting success or failure thresh
@@ -97,7 +97,7 @@ ctrl:
         joint_deriv_gains: [8, 8, 8, 8, 8, 8, 8]
     task_space_impedance:
         motion_ctrl_axes: [1, 1, 1, 1, 1, 1]
-        task_prop_gains: [300, 300, 300, 50, 50, 50]
+        task_prop_gains: [300, 300, 600, 50, 50, 50]
         task_deriv_gains: [34, 34, 34, 1.4, 1.4, 1.4]
     operational_space_motion:
         motion_ctrl_axes: [1, 1, 1, 1, 1, 1]

isaacgymenvs/tasks/industreal/industreal_algo_utils.py

@@ -183,8 +183,8 @@ def get_sapu_reward_scale(
     )
 
     # Determine if envs have low interpenetration or high interpenetration
-    low_interpen_envs = torch.argwhere(max_interpen_dists <= interpen_thresh)
-    high_interpen_envs = torch.argwhere(max_interpen_dists > interpen_thresh)
+    low_interpen_envs = torch.nonzero(max_interpen_dists <= interpen_thresh)
+    high_interpen_envs = torch.nonzero(max_interpen_dists > interpen_thresh)
 
     # Compute reward scale
     reward_scale = 1 - torch.tanh(

isaacgymenvs/tasks/industreal/industreal_base.py

@@ -350,6 +350,63 @@ def acquire_base_tensors(self):
             (self.num_envs, self.num_actions), device=self.device
         )
 
+    def generate_ctrl_signals(self):
+        """Get Jacobian. Set Franka DOF position targets or DOF torques."""
+        # Get desired Jacobian
+        if self.cfg_ctrl['jacobian_type'] == 'geometric':
+            self.fingertip_midpoint_jacobian_tf = self.fingertip_centered_jacobian
+        elif self.cfg_ctrl['jacobian_type'] == 'analytic':
+            self.fingertip_midpoint_jacobian_tf = fc.get_analytic_jacobian(
+                fingertip_quat=self.fingertip_quat,
+                fingertip_jacobian=self.fingertip_centered_jacobian,
+                num_envs=self.num_envs,
+                device=self.device)
+        # Set PD joint pos target or joint torque
+        if self.cfg_ctrl['motor_ctrl_mode'] == 'gym':
+            self._set_dof_pos_target()
+        elif self.cfg_ctrl['motor_ctrl_mode'] == 'manual':
+            self._set_dof_torque()
+
+    def _set_dof_pos_target(self):
+        """Set Franka DOF position target to move fingertips towards target pose."""
+        self.ctrl_target_dof_pos = fc.compute_dof_pos_target(
+            cfg_ctrl=self.cfg_ctrl,
+            arm_dof_pos=self.arm_dof_pos,
+            fingertip_midpoint_pos=self.fingertip_centered_pos,
+            fingertip_midpoint_quat=self.fingertip_centered_quat,
+            jacobian=self.fingertip_midpoint_jacobian_tf,
+            ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_centered_pos,
+            ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_centered_quat,
+            ctrl_target_gripper_dof_pos=self.ctrl_target_gripper_dof_pos,
+            device=self.device)
+        self.gym.set_dof_position_target_tensor_indexed(self.sim,
+                                                        gymtorch.unwrap_tensor(self.ctrl_target_dof_pos),
+                                                        gymtorch.unwrap_tensor(self.franka_actor_ids_sim),
+                                                        len(self.franka_actor_ids_sim))
+    def _set_dof_torque(self):
+        """Set Franka DOF torque to move fingertips towards target pose."""
+        self.dof_torque = fc.compute_dof_torque(
+            cfg_ctrl=self.cfg_ctrl,
+            dof_pos=self.dof_pos,
+            dof_vel=self.dof_vel,
+            fingertip_midpoint_pos=self.fingertip_centered_pos,
+            fingertip_midpoint_quat=self.fingertip_centered_quat,
+            fingertip_midpoint_linvel=self.fingertip_centered_linvel,
+            fingertip_midpoint_angvel=self.fingertip_centered_angvel,
+            left_finger_force=self.left_finger_force,
+            right_finger_force=self.right_finger_force,
+            jacobian=self.fingertip_midpoint_jacobian_tf,
+            arm_mass_matrix=self.arm_mass_matrix,
+            ctrl_target_gripper_dof_pos=self.ctrl_target_gripper_dof_pos,
+            ctrl_target_fingertip_midpoint_pos=self.ctrl_target_fingertip_centered_pos,
+            ctrl_target_fingertip_midpoint_quat=self.ctrl_target_fingertip_centered_quat,
+            ctrl_target_fingertip_contact_wrench=self.ctrl_target_fingertip_contact_wrench,
+            device=self.device)
+        self.gym.set_dof_actuation_force_tensor_indexed(self.sim,
+                                                        gymtorch.unwrap_tensor(self.dof_torque),
+                                                        gymtorch.unwrap_tensor(self.franka_actor_ids_sim),
+                                                        len(self.franka_actor_ids_sim))
+
     def simulate_and_refresh(self):
         """Simulate one step, refresh tensors, and render results."""
 

setup.py

@@ -34,7 +34,7 @@
 setup(
     name="isaacgymenvs",
     author="NVIDIA",
-    version="1.5.0",
+    version="1.5.1",
     description="Benchmark environments for high-speed robot learning in NVIDIA IsaacGym.",
     keywords=["robotics", "rl"],
     include_package_data=True,