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
