Method shift_pose

dm_control/composer/entity.py:495–536

Shifts the position and/or orientation from its current configuration. This is a convenience function that performs the same operation as `set_pose`, but where the specified `position` is added to the current position, and the specified `quaternion` is premultiplied to the current q

(self,
                 physics,
                 position=None,
                 quaternion=None,
                 rotate_velocity=False)

        physics.bind(attachment_frame).quat = normalised_quaternion

  def shift_pose(self,
                 physics,
                 position=None,
                 quaternion=None,
                 rotate_velocity=False):
    """Shifts the position and/or orientation from its current configuration.

    This is a convenience function that performs the same operation as
    `set_pose`, but where the specified `position` is added to the current
    position, and the specified `quaternion` is premultiplied to the current
    quaternion.

    Args:
      physics: An instance of `mjcf.Physics`.
      position: (optional) A NumPy array of size 3.
      quaternion: (optional) A NumPy array of size 4.
      rotate_velocity: (optional) A bool, whether to shift the current linear
        velocity along with the pose. This will rotate the current linear
        velocity, which is expressed relative to the world frame. The angular
        velocity, which is expressed relative to the local frame is left
        unchanged.

    Raises:
      RuntimeError: If the entity is not attached.
    """
    current_position, current_quaternion = self.get_pose(physics)
    new_position, new_quaternion = None, None
    if position is not None:
      new_position = current_position + position
    if quaternion is not None:
      quaternion = np.asarray(quaternion, dtype=np.float64)
      new_quaternion = _multiply_quaternions(quaternion, current_quaternion)
      root_joint = mjcf.get_frame_freejoint(self.mjcf_model)
      if root_joint and rotate_velocity:
        # Rotate the linear velocity. The angular velocity (qvel[3:])
        # is left unchanged, as it is expressed in the local frame.
        # When rotatating the body frame the angular velocity already
        # tracks the rotation but the linear velocity does not.
        velocity = physics.bind(root_joint).qvel[:3]
        rotated_velocity = _rotate_vector(velocity, quaternion)
        self.set_velocity(physics, rotated_velocity)
    self.set_pose(physics, new_position, new_quaternion)

  def get_velocity(self, physics):
    """Gets the linear and angular velocity of this free entity.