|
2 | 2 | import json |
3 | 3 | import os |
4 | 4 | from collections import OrderedDict |
5 | | -from typing import Optional, Dict |
| 5 | +from typing import Dict, Optional |
6 | 6 |
|
7 | 7 | import numpy as np |
8 | 8 |
|
@@ -329,9 +329,7 @@ def setup_references(self): |
329 | 329 | self.sim.model.get_joint_qpos_addr(x) for x in self.robot_model._torso_joints |
330 | 330 | ] |
331 | 331 | # indices for all joints (includes arms, base, torso, but not gripper joints, as those are handled separately) |
332 | | - self._ref_all_joint_pos_indexes = [ |
333 | | - self.sim.model.get_joint_qpos_addr(x) for x in self.robot_model.all_joints |
334 | | - ] |
| 332 | + self._ref_all_joint_pos_indexes = [self.sim.model.get_joint_qpos_addr(x) for x in self.robot_model.all_joints] |
335 | 333 |
|
336 | 334 | def setup_observables(self): |
337 | 335 | """ |
@@ -664,25 +662,25 @@ def get_robot_joint_positions(self): |
664 | 662 | def get_all_robot_joint_positions(self) -> Dict[str, float]: |
665 | 663 | """ |
666 | 664 | Returns all joint positions including robot joints (arms, base, torso) and gripper joints. |
667 | | - |
| 665 | +
|
668 | 666 | Returns: |
669 | 667 | dict: joint name -> joint position (in angles / radians) |
670 | 668 | """ |
671 | 669 | joint_positions = {} |
672 | | - |
| 670 | + |
673 | 671 | # Get all robot joint positions (includes arms, base, torso, etc.) |
674 | 672 | all_joint_names = self.robot_model.all_joints |
675 | 673 | for i, joint_name in enumerate(all_joint_names): |
676 | 674 | joint_positions[joint_name] = self.sim.data.qpos[self._ref_all_joint_pos_indexes[i]] |
677 | | - |
| 675 | + |
678 | 676 | # Add gripper joint positions for all arms |
679 | 677 | for arm in self.arms: |
680 | 678 | if self.has_gripper[arm]: |
681 | 679 | gripper_joint_names = self.gripper_joints[arm] |
682 | 680 | gripper_positions = self.sim.data.qpos[self._ref_gripper_joint_pos_indexes[arm]] |
683 | 681 | for joint_name, position in zip(gripper_joint_names, gripper_positions): |
684 | 682 | joint_positions[joint_name] = position |
685 | | - |
| 683 | + |
686 | 684 | return joint_positions |
687 | 685 |
|
688 | 686 | def get_gripper_joint_positions(self, gripper_arm: Optional[str] = None): |
|
0 commit comments