Skip to content

[fix] fix actuator control with actuator-joint mapping #602

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Dingry wants to merge 6 commits into
base: master
Choose a base branch
from
Open

[fix] fix actuator control with actuator-joint mapping #602

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
45df0d8
[fix] fix actuator control with actuator-joint mapping
Dingry Dec 28, 2024
5ac046a
[style] formatting
Dingry Dec 30, 2024
a1cd002
[fix] apply actuator-joint indexing only when using JointxxxController
Dingry Dec 30, 2024
8db1f9a
[add] also add actuator-joint mapping for fixed_base_robot and wheele…
Dingry Jan 28, 2025
19574b9
[fix] fix import
Dingry Jan 28, 2025
0b02cac
[style] reformat
Dingry Jan 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 25 additions & 2 deletions robosuite/robots/fixed_base_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

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


Expand Down Expand Up @@ -118,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
Expand Down Expand Up @@ -149,8 +159,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 policy_step:
# Update proprioceptive values
Expand Down
12 changes: 12 additions & 0 deletions robosuite/robots/legged_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -188,6 +189,17 @@ 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]

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]

Comment on lines +192 to +202
Copy link
Contributor

@snasiriany snasiriany Jan 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

would this logic apply more generally to other robots? other than legged_robot.py

also, why is the logic only applied for JointPositionController/JointVelocityController/JointTorqueController? Is it because these are the controllers where we directly output joint targets?

Copy link
Contributor Author

@Dingry Dingry Jan 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, this logic should apply to all robots.

Yes. The JointController generates control signals for all joints, while the GripperController only produces control signals for actuators. This difference explains why we need these conditional statements. I'm uncertain about the behavior of other controllers.
Also I was thinking that the GripperController seems essentially similar to JointController but focuses only on actuator-driven joints. If so, we could potentially unify their implementation since their fundamental problem if the same: align joint with actuator actions.

applied_action = np.clip(applied_action / actuator_gears, applied_action_low, applied_action_high)
self.sim.data.ctrl[actuator_indexes] = applied_action

Expand Down
9 changes: 9 additions & 0 deletions robosuite/robots/mobile_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,6 +266,15 @@ 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(
Copy link
Contributor

@snasiriany snasiriany Jan 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add a very small comment in the code explaining how this mapping works (conceptually at a high level)

[
self._ref_joints_indexes_dict[part_name].index(self.sim.model.actuator_trnid[i, 0])
for i in actuator_ids
]
)

Comment on lines +269 to +277
Copy link
Contributor

@snasiriany snasiriany Jan 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should have the same code in fixed_base_robot.py as well, right?

Copy link
Contributor Author

@Dingry Dingry Jan 8, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes.

def control(self, action, policy_step=False):
"""
Actuate the robot with the
Expand Down
1 change: 1 addition & 0 deletions robosuite/robots/robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
18 changes: 16 additions & 2 deletions robosuite/robots/wheeled_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@

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

Expand Down Expand Up @@ -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:
Expand Down