Sets position and/or orientation of this entity relative to its parent. If the entity is attached with a free joint, this method will set the respective DoFs of the joint. If the entity is either fixed or attached with a different joint type, this method will update the position and/or
(self, physics, position=None, quaternion=None)
| 453 | return position, quaternion |
| 454 | |
| 455 | def set_pose(self, physics, position=None, quaternion=None): |
| 456 | """Sets position and/or orientation of this entity relative to its parent. |
| 457 | |
| 458 | If the entity is attached with a free joint, this method will set the |
| 459 | respective DoFs of the joint. If the entity is either fixed or attached with |
| 460 | a different joint type, this method will update the position and/or |
| 461 | quaternion of the attachment frame. |
| 462 | |
| 463 | Note that the semantics differ slightly between the two cases: the DoFs of a |
| 464 | free body are specified in global coordinates, whereas the position of a |
| 465 | non-free body is specified in relative coordinates with respect to the |
| 466 | parent frame. However, for entities that are either attached directly to the |
| 467 | worldbody, or to other entities that are positioned at the global origin |
| 468 | (e.g. the arena), there is no difference between the two cases. |
| 469 | |
| 470 | Args: |
| 471 | physics: An instance of `mjcf.Physics`. |
| 472 | position: (optional) A NumPy array of size 3. |
| 473 | quaternion: (optional) A NumPy array of size 4. |
| 474 | |
| 475 | Raises: |
| 476 | RuntimeError: If the entity is not attached. |
| 477 | """ |
| 478 | root_joint = mjcf.get_frame_freejoint(self.mjcf_model) |
| 479 | if root_joint: |
| 480 | if position is not None: |
| 481 | physics.bind(root_joint).qpos[:3] = position |
| 482 | if quaternion is not None: |
| 483 | normalised_quaternion = quaternion / np.linalg.norm(quaternion) |
| 484 | physics.bind(root_joint).qpos[3:] = normalised_quaternion |
| 485 | else: |
| 486 | attachment_frame = mjcf.get_attachment_frame(self.mjcf_model) |
| 487 | if attachment_frame is None: |
| 488 | raise RuntimeError(_NO_ATTACHMENT_FRAME) |
| 489 | if position is not None: |
| 490 | physics.bind(attachment_frame).pos = position |
| 491 | if quaternion is not None: |
| 492 | normalised_quaternion = quaternion / np.linalg.norm(quaternion) |
| 493 | physics.bind(attachment_frame).quat = normalised_quaternion |
| 494 | |
| 495 | def shift_pose(self, |
| 496 | physics, |