def _find_non_contacting_height(physics, walker, orientation, position=(0, 0, 0), maxiter=1000): """Find a height with no contacts given a body orientation. Args: physics: An instance of `Physics`. walker: the focal walker. orientation: A quaternion. position: A tuple. Position along global x, y, z axes. maxiter: maximum number of iterations to try """ z_pos = position[2] # Start embedded in the floor. num_contacts = 1 count = 1 # Move up in 1cm increments until no contacts. while num_contacts > 0: try: with physics.reset_context(): freejoint = mjcf.get_frame_freejoint(walker.mjcf_model) physics.bind( freejoint).qpos[:3] = position[0], position[1], z_pos physics.bind(freejoint).qpos[3:] = orientation except control.PhysicsError: # We may encounter a PhysicsError here due to filling the contact # buffer, in which case we simply increment the height and continue. pass num_contacts = physics.data.ncon z_pos += 0.01 count += 1 if count > maxiter: raise ValueError( 'maxiter reached: possibly contacts in null pose of body.')
def __init__(self, props, position, quaternion=rotations.IDENTITY_QUATERNION, ignore_collisions=False, max_attempts_per_prop=20, settle_physics=False, max_settle_physics_time=2., max_settle_physics_attempts=1, raise_exception_on_settle_failure=False): """Initializes this PropPlacer. Args: props: A sequence of `composer.Entity` instances representing props. position: A single fixed Cartesian position, or a `composer.Variation` object that generates Cartesian positions. If a fixed sequence of positions for multiple props is desired, use `variation.deterministic.Sequence`. quaternion: (optional) A single fixed unit quaternion, or a `Variation` object that generates unit quaternions. If a fixed sequence of quaternions for multiple props is desired, use `variation.deterministic.Sequence`. ignore_collisions: (optional) If True, ignore collisions between props, i.e. do not run rejection sampling. max_attempts_per_prop: The maximum number of rejection sampling attempts per prop. If a non-colliding pose cannot be found before this limit is reached, a `RuntimeError` will be raised. settle_physics: (optional) If True, the physics simulation will be advanced for a few steps to allow the prop positions to settle. max_settle_physics_time: (optional) When `settle_physics` is True, upper bound on time (in seconds) the physics simulation is advanced. max_settle_physics_attempts: (optional) When `settle_physics` is True, the number of attempts at sampling overall scene pose and settling. raise_exception_on_settle_failure: If True, raises an exception if settling physics is unsuccessful. """ super().__init__() self._props = props self._prop_joints = [] for prop in props: freejoint = mjcf.get_frame_freejoint(prop.mjcf_model) if freejoint is not None: self._prop_joints.append(freejoint) self._prop_joints.extend(prop.mjcf_model.find_all('joint')) self._position = position self._quaternion = quaternion self._ignore_collisions = ignore_collisions self._max_attempts_per_prop = max_attempts_per_prop self._settle_physics = settle_physics self._max_settle_physics_time = max_settle_physics_time self._max_settle_physics_attempts = max_settle_physics_attempts self._raise_exception_on_settle_failure = raise_exception_on_settle_failure if max_settle_physics_attempts < 1: raise ValueError('max_settle_physics_attempts should be greater than ' 'zero to have any effect, but is ' f'{max_settle_physics_attempts}')
def test_ball_velocity(self): initializer = initializers.UniformInitializer() env = _env(_home_team(1) + _away_team(1), initializer=initializer) ball_root_joint = mjcf.get_frame_freejoint(env.task.ball.mjcf_model) # Set the velocities of the ball root joint to a non-zero sentinel value. env.physics.bind(ball_root_joint).qvel = 3.14 initializer(env.task, env.physics, env.random_state) # The initializer should set the ball velocity to zero. ball_velocity = env.physics.bind(ball_root_joint).qvel np.testing.assert_array_equal(ball_velocity, 0.)
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.array(quaternion, dtype=np.float64, copy=False) 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. Args: physics: An instance of `mjcf.Physics`. Returns: A 2-tuple where the first entry is a (3,) numpy array representing the linear velocity and the second is a (3,) numpy array representing the angular velocity. """ root_joint = mjcf.get_frame_freejoint(self.mjcf_model) if root_joint: velocity = physics.bind(root_joint).qvel[:3] angular_velocity = physics.bind(root_joint).qvel[3:] return velocity, angular_velocity else: raise ValueError( 'get_velocity cannot be used on a non-free entity')
def set_velocity(self, physics, velocity=None, angular_velocity=None): """Sets the linear velocity and/or angular velocity of this free entity. If the entity is attached with a free joint, this method will set the respective DoFs of the joint. Otherwise a warning is logged. Args: physics: An instance of `mjcf.Physics`. velocity: (optional) A NumPy array of size 3 specifying the linear velocity. angular_velocity: (optional) A NumPy array of size 3 specifying the angular velocity """ root_joint = mjcf.get_frame_freejoint(self.mjcf_model) if root_joint: if velocity is not None: physics.bind(root_joint).qvel[:3] = velocity if angular_velocity is not None: physics.bind(root_joint).qvel[3:] = angular_velocity else: logging.warning('Cannot set velocity on Entity with no free joint.')
def set_pose(self, physics, position=None, quaternion=None): """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 quaternion of the attachment frame. Note that the semantics differ slightly between the two cases: the DoFs of a free body are specified in global coordinates, whereas the position of a non-free body is specified in relative coordinates with respect to the parent frame. However, for entities that are either attached directly to the worldbody, or to other entities that are positioned at the global origin (e.g. the arena), there is no difference between the two cases. Args: physics: An instance of `mjcf.Physics`. position: (optional) A NumPy array of size 3. quaternion: (optional) A NumPy array of size 4. Raises: RuntimeError: If the entity is not attached. """ root_joint = mjcf.get_frame_freejoint(self.mjcf_model) if root_joint: if position is not None: physics.bind(root_joint).qpos[:3] = position if quaternion is not None: physics.bind(root_joint).qpos[3:] = quaternion else: attachment_frame = mjcf.get_attachment_frame(self.mjcf_model) if attachment_frame is None: raise RuntimeError(_NO_ATTACHMENT_FRAME) if position is not None: physics.bind(attachment_frame).pos = position if quaternion is not None: normalised_quaternion = quaternion / np.linalg.norm(quaternion) physics.bind(attachment_frame).quat = normalised_quaternion
def get_pose(self, physics): """Get the position and orientation of this entity relative to its parent. Note that the semantics differ slightly depending on whether or not the entity has a free joint: * If it has a free joint the position and orientation are always given in global coordinates. * If the entity is fixed or attached with a different joint type then the position and orientation are given relative to the parent frame. For entities that are either attached directly to the worldbody, or to other entities that are positioned at the global origin (e.g. the arena) the global and relative poses are equivalent. Args: physics: An instance of `mjcf.Physics`. Returns: A 2-tuple where the first entry is a (3,) numpy array representing the position and the second is a (4,) numpy array representing orientation as a quaternion. Raises: RuntimeError: If the entity is not attached. """ root_joint = mjcf.get_frame_freejoint(self.mjcf_model) if root_joint: position = physics.bind(root_joint).qpos[:3] quaternion = physics.bind(root_joint).qpos[3:] else: attachment_frame = mjcf.get_attachment_frame(self.mjcf_model) if attachment_frame is None: raise RuntimeError(_NO_ATTACHMENT_FRAME) position = physics.bind(attachment_frame).pos quaternion = physics.bind(attachment_frame).quat return position, quaternion