Skip to content

Commit bdbaf55

Browse files
Update composite_controller.py (#700)
* Update composite_controller.py Explain what `get_controller_base_pose` does * Update composite_controller.py Update docstring * Update composite_controller.py Update docstring * Format
1 parent ea9e700 commit bdbaf55

1 file changed

Lines changed: 17 additions & 0 deletions

File tree

robosuite/controllers/composite/composite_controller.py

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -122,6 +122,23 @@ def get_control_dim(self, part_name):
122122
return self.part_controllers[part_name].control_dim
123123

124124
def get_controller_base_pose(self, controller_name):
125+
"""
126+
Get the base position and orientation of a specified controller's part. Note: this pose may likely differ from
127+
the robot base's pose.
128+
129+
Args:
130+
controller_name (str): The name of the controller, used to look up part-specific information.
131+
132+
Returns:
133+
tuple[np.ndarray, np.ndarray]: A tuple containing:
134+
- base_pos (np.ndarray): The 3D position of the part's center in world coordinates (shape: (3,)).
135+
- base_ori (np.ndarray): The 3x3 rotation matrix representing the part's orientation in world coordinates.
136+
137+
Details:
138+
- Uses the controller's `naming_prefix` and `part_name` to construct the corresponding site name.
139+
- Queries the simulation (`self.sim`) for the site's position (`site_xpos`) and orientation (`site_xmat`).
140+
- The site orientation matrix is reshaped from a flat array of size 9 to a 3x3 rotation matrix.
141+
"""
125142
naming_prefix = self.part_controllers[controller_name].naming_prefix
126143
part_name = self.part_controllers[controller_name].part_name
127144
base_pos = np.array(self.sim.data.site_xpos[self.sim.model.site_name2id(f"{naming_prefix}{part_name}_center")])

0 commit comments

Comments
 (0)