From 45df0d87e25613e9edcc87739963539a93d390a9 Mon Sep 17 00:00:00 2001 From: Runyu Ding Date: Sat, 28 Dec 2024 10:25:11 +0800 Subject: [PATCH 1/3] [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/3] [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/3] [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