[fix] fix actuator control with actuator-joint mapping #602
Conversation
| 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 | ||
| ] | ||
| ) | ||
|
|
There was a problem hiding this comment.
I think we should have the same code in fixed_base_robot.py as well, right?
| @@ -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( | |||
There was a problem hiding this comment.
add a very small comment in the code explaining how this mapping works (conceptually at a high level)
|
|
||
| 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] | ||
|
|
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
What this does
The exisiting joint controllers (
JointPositionController,JointVelocityController, andJointTorqueController) would break under two conditions:The PR fixes this with actuator-joint indexing and mapping in the control function.