From aeed298638a1f7b5421b38f5f3cc2d1079b6d9c3 Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Wed, 18 Oct 2023 09:22:21 -0700 Subject: [PATCH] merge release 1.5.1 from dev to main --- docs/release_notes.md | 4 ++ .../cfg/task/IndustRealTaskGearsInsert.yaml | 8 +-- .../tasks/industreal/industreal_algo_utils.py | 4 +- .../tasks/industreal/industreal_base.py | 57 +++++++++++++++++++ setup.py | 2 +- 5 files changed, 68 insertions(+), 7 deletions(-) diff --git a/docs/release_notes.md b/docs/release_notes.md index c9e8eb4da..f1b720488 100644 --- a/docs/release_notes.md +++ b/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 ----- diff --git a/isaacgymenvs/cfg/task/IndustRealTaskGearsInsert.yaml b/isaacgymenvs/cfg/task/IndustRealTaskGearsInsert.yaml index cd94df0a7..0537b29a2 100644 --- a/isaacgymenvs/cfg/task/IndustRealTaskGearsInsert.yaml +++ b/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] diff --git a/isaacgymenvs/tasks/industreal/industreal_algo_utils.py b/isaacgymenvs/tasks/industreal/industreal_algo_utils.py index cdc278583..af636314d 100644 --- a/isaacgymenvs/tasks/industreal/industreal_algo_utils.py +++ b/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( diff --git a/isaacgymenvs/tasks/industreal/industreal_base.py b/isaacgymenvs/tasks/industreal/industreal_base.py index e2de3284f..662c8ebe7 100644 --- a/isaacgymenvs/tasks/industreal/industreal_base.py +++ b/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.""" diff --git a/setup.py b/setup.py index 8f9d6a1e9..c1ad6fa58 100644 --- a/setup.py +++ b/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,