From 45df0d87e25613e9edcc87739963539a93d390a9 Mon Sep 17 00:00:00 2001 From: Runyu Ding Date: Sat, 28 Dec 2024 10:25:11 +0800 Subject: [PATCH 1/6] [fix] fix actuator control with actuator-joint mapping --- robosuite/robots/legged_robot.py | 4 ++++ robosuite/robots/mobile_robot.py | 7 +++++++ robosuite/robots/robot.py | 1 + 3 files changed, 12 insertions(+) diff --git a/robosuite/robots/legged_robot.py b/robosuite/robots/legged_robot.py index 8d89431b5d..97e4c02eae 100644 --- a/robosuite/robots/legged_robot.py +++ b/robosuite/robots/legged_robot.py @@ -188,6 +188,10 @@ def control(self, action, policy_step=False): applied_action_high = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 1] actuator_indexes = self._ref_actuators_indexes_dict[part_name] actuator_gears = self.sim.model.actuator_gear[actuator_indexes, 0] + + # select only the joints that are actuated + actuated_joint_indexes = self._ref_actuator_to_joint_id[actuator_indexes] + applied_action = applied_action[actuated_joint_indexes] applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high) self.sim.data.ctrl[actuator_indexes] = applied_action diff --git a/robosuite/robots/mobile_robot.py b/robosuite/robots/mobile_robot.py index 60c28bf35d..c220fb31ca 100644 --- a/robosuite/robots/mobile_robot.py +++ b/robosuite/robots/mobile_robot.py @@ -266,6 +266,13 @@ def setup_references(self): self.sim.model.joint_name2id(joint) for joint in self.robot_model.head_joints ] + self._ref_actuator_to_joint_id = np.ones(self.sim.model.nu).astype(np.int32) * (-1) + for part_name, actuator_ids in self._ref_actuators_indexes_dict.items(): + self._ref_actuator_to_joint_id[actuator_ids] = np.array([ + self._ref_joints_indexes_dict[part_name].index(self.sim.model.actuator_trnid[i, 0]) + for i in actuator_ids + ]) + def control(self, action, policy_step=False): """ Actuate the robot with the diff --git a/robosuite/robots/robot.py b/robosuite/robots/robot.py index bc1faca82b..44c953a95e 100644 --- a/robosuite/robots/robot.py +++ b/robosuite/robots/robot.py @@ -130,6 +130,7 @@ def __init__( self._ref_actuators_indexes_dict = {} self._ref_joints_indexes_dict = {} + self._ref_actuator_to_joint_id = None self._enabled_parts = {} self.composite_controller = None From 5ac046a772647beb47c8b5b3d512c82077f1d055 Mon Sep 17 00:00:00 2001 From: Runyu Ding Date: Mon, 30 Dec 2024 10:05:16 +0800 Subject: [PATCH 2/6] [style] formatting --- robosuite/robots/mobile_robot.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/robosuite/robots/mobile_robot.py b/robosuite/robots/mobile_robot.py index c220fb31ca..74ab9520e0 100644 --- a/robosuite/robots/mobile_robot.py +++ b/robosuite/robots/mobile_robot.py @@ -268,10 +268,12 @@ def setup_references(self): self._ref_actuator_to_joint_id = np.ones(self.sim.model.nu).astype(np.int32) * (-1) for part_name, actuator_ids in self._ref_actuators_indexes_dict.items(): - self._ref_actuator_to_joint_id[actuator_ids] = np.array([ - self._ref_joints_indexes_dict[part_name].index(self.sim.model.actuator_trnid[i, 0]) - for i in actuator_ids - ]) + self._ref_actuator_to_joint_id[actuator_ids] = np.array( + [ + self._ref_joints_indexes_dict[part_name].index(self.sim.model.actuator_trnid[i, 0]) + for i in actuator_ids + ] + ) def control(self, action, policy_step=False): """ From a1cd0026ecd372e37173002e41eac1ec50c0711b Mon Sep 17 00:00:00 2001 From: Runyu Ding Date: Mon, 30 Dec 2024 11:20:19 +0800 Subject: [PATCH 3/6] [fix] apply actuator-joint indexing only when using JointxxxController --- robosuite/robots/legged_robot.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/robosuite/robots/legged_robot.py b/robosuite/robots/legged_robot.py index 97e4c02eae..267ae4f012 100644 --- a/robosuite/robots/legged_robot.py +++ b/robosuite/robots/legged_robot.py @@ -8,6 +8,7 @@ import robosuite.utils.transform_utils as T from robosuite.controllers import composite_controller_factory, load_part_controller_config +from robosuite.controllers.parts.generic import JointPositionController, JointTorqueController, JointVelocityController from robosuite.models.bases.leg_base_model import LegBaseModel from robosuite.robots.mobile_robot import MobileRobot from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER @@ -189,9 +190,16 @@ def control(self, action, policy_step=False): actuator_indexes = self._ref_actuators_indexes_dict[part_name] actuator_gears = self.sim.model.actuator_gear[actuator_indexes, 0] - # select only the joints that are actuated - actuated_joint_indexes = self._ref_actuator_to_joint_id[actuator_indexes] - applied_action = applied_action[actuated_joint_indexes] + part_controllers = self.composite_controller.get_controller(part_name) + if ( + isinstance(part_controllers, JointPositionController) + or isinstance(part_controllers, JointVelocityController) + or isinstance(part_controllers, JointTorqueController) + ): + # select only the joints that are actuated + actuated_joint_indexes = self._ref_actuator_to_joint_id[actuator_indexes] + applied_action = applied_action[actuated_joint_indexes] + applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high) self.sim.data.ctrl[actuator_indexes] = applied_action From 8db1f9a4ae033317ae89f7d48de3b6222d4d67ea Mon Sep 17 00:00:00 2001 From: runyud Date: Tue, 28 Jan 2025 15:33:53 -0800 Subject: [PATCH 4/6] [add] also add actuator-joint mapping for fixed_base_robot and wheeled_robot --- robosuite/robots/fixed_base_robot.py | 19 +++++++++++++++++-- robosuite/robots/wheeled_robot.py | 18 ++++++++++++++++-- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/robosuite/robots/fixed_base_robot.py b/robosuite/robots/fixed_base_robot.py index 430bc5fd59..ff3a6c399b 100644 --- a/robosuite/robots/fixed_base_robot.py +++ b/robosuite/robots/fixed_base_robot.py @@ -7,6 +7,7 @@ import robosuite.utils.transform_utils as T from robosuite.controllers import composite_controller_factory from robosuite.robots.robot import Robot +from robosuite.controllers import JointPositionController, JointVelocityController, JointTorqueController class FixedBaseRobot(Robot): @@ -149,8 +150,22 @@ def control(self, action, policy_step=False): for part_name, applied_action in applied_action_dict.items(): applied_action_low = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 0] applied_action_high = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 1] - applied_action = np.clip(applied_action, applied_action_low, applied_action_high) - self.sim.data.ctrl[self._ref_actuators_indexes_dict[part_name]] = applied_action + actuator_indexes = self._ref_actuators_indexes_dict[part_name] + actuator_gears = self.sim.model.actuator_gear[actuator_indexes, 0] + + part_controllers = self.composite_controller.get_controller(part_name) + if ( + isinstance(part_controllers, JointPositionController) + or isinstance(part_controllers, JointVelocityController) + or isinstance(part_controllers, JointTorqueController) + ): + # select only the joints that are actuated + actuated_joint_indexes = self._ref_actuator_to_joint_id[actuator_indexes] + applied_action = applied_action[actuated_joint_indexes] + + applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high) + self.sim.data.ctrl[actuator_indexes] = applied_action + if policy_step: # Update proprioceptive values diff --git a/robosuite/robots/wheeled_robot.py b/robosuite/robots/wheeled_robot.py index 88cbd4265a..ea71f314d4 100644 --- a/robosuite/robots/wheeled_robot.py +++ b/robosuite/robots/wheeled_robot.py @@ -8,6 +8,7 @@ from robosuite.controllers import composite_controller_factory from robosuite.robots.mobile_robot import MobileRobot from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER +from robosuite.controllers import JointPositionController, JointVelocityController, JointTorqueController class WheeledRobot(MobileRobot): @@ -124,8 +125,21 @@ def control(self, action, policy_step=False): for part_name, applied_action in applied_action_dict.items(): applied_action_low = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 0] applied_action_high = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 1] - applied_action = np.clip(applied_action, applied_action_low, applied_action_high) - self.sim.data.ctrl[self._ref_actuators_indexes_dict[part_name]] = applied_action + actuator_indexes = self._ref_actuators_indexes_dict[part_name] + actuator_gears = self.sim.model.actuator_gear[actuator_indexes, 0] + + part_controllers = self.composite_controller.get_controller(part_name) + if ( + isinstance(part_controllers, JointPositionController) + or isinstance(part_controllers, JointVelocityController) + or isinstance(part_controllers, JointTorqueController) + ): + # select only the joints that are actuated + actuated_joint_indexes = self._ref_actuator_to_joint_id[actuator_indexes] + applied_action = applied_action[actuated_joint_indexes] + + applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high) + self.sim.data.ctrl[actuator_indexes] = applied_action # If this is a policy step, also update buffers holding recent values of interest if policy_step: From 19574b96b0d5655280441ca2d9ca5e9c5a2bfcc6 Mon Sep 17 00:00:00 2001 From: runyud Date: Tue, 28 Jan 2025 15:50:08 -0800 Subject: [PATCH 5/6] [fix] fix import --- robosuite/robots/fixed_base_robot.py | 2 +- robosuite/robots/wheeled_robot.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/robosuite/robots/fixed_base_robot.py b/robosuite/robots/fixed_base_robot.py index ff3a6c399b..d3fd071324 100644 --- a/robosuite/robots/fixed_base_robot.py +++ b/robosuite/robots/fixed_base_robot.py @@ -7,7 +7,7 @@ import robosuite.utils.transform_utils as T from robosuite.controllers import composite_controller_factory from robosuite.robots.robot import Robot -from robosuite.controllers import JointPositionController, JointVelocityController, JointTorqueController +from robosuite.controllers.parts.generic import JointPositionController, JointVelocityController, JointTorqueController class FixedBaseRobot(Robot): diff --git a/robosuite/robots/wheeled_robot.py b/robosuite/robots/wheeled_robot.py index ea71f314d4..4b420c6518 100644 --- a/robosuite/robots/wheeled_robot.py +++ b/robosuite/robots/wheeled_robot.py @@ -8,7 +8,7 @@ from robosuite.controllers import composite_controller_factory from robosuite.robots.mobile_robot import MobileRobot from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER -from robosuite.controllers import JointPositionController, JointVelocityController, JointTorqueController +from robosuite.controllers.parts.generic import JointPositionController, JointVelocityController, JointTorqueController class WheeledRobot(MobileRobot): From 0b02caceabdcfdef86c951baf4f0e9aaefbbf592 Mon Sep 17 00:00:00 2001 From: runyud Date: Tue, 28 Jan 2025 16:10:51 -0800 Subject: [PATCH 6/6] [style] reformat --- robosuite/robots/fixed_base_robot.py | 12 ++++++++++-- robosuite/robots/wheeled_robot.py | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/robosuite/robots/fixed_base_robot.py b/robosuite/robots/fixed_base_robot.py index d3fd071324..fdb761364a 100644 --- a/robosuite/robots/fixed_base_robot.py +++ b/robosuite/robots/fixed_base_robot.py @@ -6,8 +6,8 @@ import robosuite.utils.transform_utils as T from robosuite.controllers import composite_controller_factory +from robosuite.controllers.parts.generic import JointPositionController, JointTorqueController, JointVelocityController from robosuite.robots.robot import Robot -from robosuite.controllers.parts.generic import JointPositionController, JointVelocityController, JointTorqueController class FixedBaseRobot(Robot): @@ -119,6 +119,15 @@ def setup_references(self): self.eef_site_id[arm] = self.sim.model.site_name2id(self.gripper[arm].important_sites["grip_site"]) self.eef_cylinder_id[arm] = self.sim.model.site_name2id(self.gripper[arm].important_sites["grip_cylinder"]) + self._ref_actuator_to_joint_id = np.ones(self.sim.model.nu).astype(np.int32) * (-1) + for part_name, actuator_ids in self._ref_actuators_indexes_dict.items(): + self._ref_actuator_to_joint_id[actuator_ids] = np.array( + [ + self._ref_joints_indexes_dict[part_name].index(self.sim.model.actuator_trnid[i, 0]) + for i in actuator_ids + ] + ) + def control(self, action, policy_step=False): """ Actuate the robot with the @@ -166,7 +175,6 @@ def control(self, action, policy_step=False): applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high) self.sim.data.ctrl[actuator_indexes] = applied_action - if policy_step: # Update proprioceptive values self.recent_qpos.push(self._joint_positions) diff --git a/robosuite/robots/wheeled_robot.py b/robosuite/robots/wheeled_robot.py index 4b420c6518..f40a804a35 100644 --- a/robosuite/robots/wheeled_robot.py +++ b/robosuite/robots/wheeled_robot.py @@ -6,9 +6,9 @@ import robosuite.utils.transform_utils as T from robosuite.controllers import composite_controller_factory +from robosuite.controllers.parts.generic import JointPositionController, JointTorqueController, JointVelocityController from robosuite.robots.mobile_robot import MobileRobot from robosuite.utils.log_utils import ROBOSUITE_DEFAULT_LOGGER -from robosuite.controllers.parts.generic import JointPositionController, JointVelocityController, JointTorqueController class WheeledRobot(MobileRobot):