Returns origin position in the torso frame.
(self)
| 212 | return np.asarray(np.linalg.norm(self.named.data.site_xpos['workspace'])) |
| 213 | |
| 214 | def origin(self): |
| 215 | """Returns origin position in the torso frame.""" |
| 216 | torso_frame = self.named.data.xmat['torso'].reshape(3, 3) |
| 217 | torso_pos = self.named.data.xpos['torso'] |
| 218 | return -torso_pos.dot(torso_frame) |
| 219 | |
| 220 | def ball_state(self): |
| 221 | """Returns ball position and velocity relative to the torso frame.""" |