def check_invalid_state(self):
"""Checks whether the physics state is invalid at exit.
Yields:
None
Raises:
PhysicsError: if the simulation state is invalid at exit, unless this
context is nested inside a `suppress_physics_errors` context, in which
case a warning will be logged instead.
"""
# `np.copyto(dst, src)` is marginally faster than `dst[:] = src`.
np.copyto(self._warnings_before, self._warnings)
yield
np.greater(self._warnings,
self._warnings_before,
out=self._new_warnings)
if any(self._new_warnings):
warning_names = np.compress(self._new_warnings,
enums.mjtWarning._fields)
message = _INVALID_PHYSICS_STATE.format(
warning_names=', '.join(warning_names))
if self._warnings_cause_exception:
raise _control.PhysicsError(message)
else:
logging.warn(message)
def check_invalid_state(self):
"""Raises a `base.PhysicsError` if the simulation state is invalid."""
warning_counts_before = self.data.warning.number.copy()
yield
warnings_raised = self.data.warning.number > warning_counts_before
if any(warnings_raised):
warning_names = [
enums.mjtWarning._fields[i] for i in np.where(warnings_raised)[0]]
raise _control.PhysicsError(
_INVALID_PHYSICS_STATE.format(warning_names=', '.join(warning_names)))
def check_invalid_state(self):
"""Raises a `base.PhysicsError` if the simulation state is invalid."""
# `np.copyto(dst, src)` is marginally faster than `dst[:] = src`.
np.copyto(self._warnings_before, self._warnings)
yield
np.greater(self._warnings, self._warnings_before, out=self._new_warnings)
if any(self._new_warnings):
warning_names = np.compress(self._new_warnings, enums.mjtWarning._fields)
raise _control.PhysicsError(
_INVALID_PHYSICS_STATE.format(warning_names=', '.join(warning_names)))
def check_invalid_state(self):
"""Raises a `base.PhysicsError` if the simulation state is invalid."""
warning_counts = [self.data.warning[i].number for i in
xrange(enums.mjtWarning.mjNWARNING)]
if any(warning_counts):
warning_names = []
for i in np.where(warning_counts)[0]:
warning_names.append(enums.mjtWarning._fields[i])
raise _control.PhysicsError(
'Physics state is invalid. Warning(s) raised: {}'.format(
', '.join(warning_names)))
def check_divergence(self):
"""Raises a `base.PhysicsError` if the simulation state is divergent."""
warning_counts = [
self.data.warning[i].number for i in _DIVERGENCE_WARNINGS
]
if any(warning_counts):
warning_names = []
for i in np.where(warning_counts)[0]:
field_idx = _DIVERGENCE_WARNINGS[i]
warning_names.append(enums.mjtWarning._fields[field_idx])
raise _control.PhysicsError(
'Physics state has diverged. Warning(s) raised: {}'.format(
', '.join(warning_names)))
def __call__(self, physics, random_state, ignore_contacts_with_entities=None):
"""Sets initial prop poses.
Args:
physics: An `mjcf.Physics` instance.
random_state: a `np.random.RandomState` instance.
ignore_contacts_with_entities: a list of `composer.Entity` instances
to ignore when detecting collisions. This can be used to ignore props
that are not being placed by this initializer, but are known to be
colliding in the current state of the simulation (for example if they
are going to be placed by a different initializer that will be called
subsequently).
Raises:
RuntimeError: If `ignore_collisions == False` and a non-colliding prop
pose could not be found within `max_attempts_per_prop`.
"""
if ignore_contacts_with_entities is None:
ignore_contacts_with_entities = []
# Temporarily disable contacts for all geoms that belong to props which
# haven't yet been placed in order to free up space in the contact buffer.
cached_contact_params = self._disable_and_cache_contact_parameters(
physics, self._props + ignore_contacts_with_entities)
try:
physics.forward()
except control.PhysicsError as e:
_, _, tb = sys.exc_info()
message = 'Despite disabling contact for all props in this initializer, '
'`physics.forward()` resulted in a `PhysicsError`: {}'.format(e)
new_exc = control.PhysicsError(message)
six.reraise(control.PhysicsError, new_exc, tb)
for prop in self._props:
# Restore the original contact parameters for all geoms in the prop we're
# about to place, so that we can detect if the new pose results in
# collisions.
self._restore_contact_parameters(physics, prop, cached_contact_params)
success = False
initial_position, initial_quaternion = prop.get_pose(physics)
next_position, next_quaternion = initial_position, initial_quaternion
for _ in range(self._max_attempts_per_prop):
next_position = variation.evaluate(self._position,
initial_value=initial_position,
current_value=next_position,
random_state=random_state)
next_quaternion = variation.evaluate(self._quaternion,
initial_value=initial_quaternion,
current_value=next_quaternion,
random_state=random_state)
prop.set_pose(physics, next_position, next_quaternion)
try:
# If this pose results in collisions then there's a chance we'll
# encounter a PhysicsError error here due to a full contact buffer,
# in which case reject this pose and sample another.
physics.forward()
except control.PhysicsError:
continue
if (self._ignore_collisions
or not self._has_collisions_with_prop(physics, prop)):
success = True
break
if not success:
raise RuntimeError(_REJECTION_SAMPLING_FAILED.format(
model_name=prop.mjcf_model.model,
max_attempts=self._max_attempts_per_prop))
for prop in ignore_contacts_with_entities:
self._restore_contact_parameters(physics, prop, cached_contact_params)
if self._settle_physics:
original_time = physics.data.time
props_isolator = utils.JointStaticIsolator(physics, self._prop_joints)
prop_joints_mj = physics.bind(self._prop_joints)
while physics.data.time - original_time < self._max_settle_physics_time:
with props_isolator:
physics.step()
if (np.max(np.abs(prop_joints_mj.qvel)) < _SETTLE_QVEL_TOL and
np.max(np.abs(prop_joints_mj.qacc)) < _SETTLE_QACC_TOL):
break
physics.data.time = original_time
def __call__(self, physics, random_state, ignore_contacts_with_entities=None):
"""Sets initial prop poses.
Args:
physics: An `mjcf.Physics` instance.
random_state: a `np.random.RandomState` instance.
ignore_contacts_with_entities: a list of `composer.Entity` instances
to ignore when detecting collisions. This can be used to ignore props
that are not being placed by this initializer, but are known to be
colliding in the current state of the simulation (for example if they
are going to be placed by a different initializer that will be called
subsequently).
Raises:
RuntimeError: If `ignore_collisions == False` and a non-colliding prop
pose could not be found within `max_attempts_per_prop`.
"""
if ignore_contacts_with_entities is None:
ignore_contacts_with_entities = []
# Temporarily disable contacts for all geoms that belong to props which
# haven't yet been placed in order to free up space in the contact buffer.
cached_contact_params = self._disable_and_cache_contact_parameters(
physics, self._props + ignore_contacts_with_entities)
try:
physics.forward()
except control.PhysicsError as cause:
effect = control.PhysicsError(
'Despite disabling contact for all props in this initializer, '
'`physics.forward()` resulted in a `PhysicsError`')
raise effect from cause
def place_props():
for prop in self._props:
# Restore the original contact parameters for all geoms in the prop
# we're about to place, so that we can detect if the new pose results in
# collisions.
self._restore_contact_parameters(physics, prop, cached_contact_params)
success = False
initial_position, initial_quaternion = prop.get_pose(physics)
next_position, next_quaternion = initial_position, initial_quaternion
for _ in range(self._max_attempts_per_prop):
next_position = variation.evaluate(self._position,
initial_value=initial_position,
current_value=next_position,
random_state=random_state)
next_quaternion = variation.evaluate(self._quaternion,
initial_value=initial_quaternion,
current_value=next_quaternion,
random_state=random_state)
prop.set_pose(physics, next_position, next_quaternion)
try:
# If this pose results in collisions then there's a chance we'll
# encounter a PhysicsError error here due to a full contact buffer,
# in which case reject this pose and sample another.
physics.forward()
except control.PhysicsError:
continue
if (self._ignore_collisions
or not self._has_collisions_with_prop(physics, prop)):
success = True
break
if not success:
raise RuntimeError(_REJECTION_SAMPLING_FAILED.format(
model_name=prop.mjcf_model.model,
max_attempts=self._max_attempts_per_prop))
for prop in ignore_contacts_with_entities:
self._restore_contact_parameters(physics, prop, cached_contact_params)
# Place the props and settle the physics. If settling was requested and it
# it fails, re-place the props.
def place_and_settle():
for _ in range(self._max_settle_physics_attempts):
place_props()
# Step physics and check prop states.
original_time = physics.data.time
props_isolator = utils.JointStaticIsolator(physics, self._prop_joints)
prop_joints_mj = physics.bind(self._prop_joints)
while physics.data.time - original_time < self._max_settle_physics_time:
with props_isolator:
physics.step()
max_qvel = np.max(np.abs(prop_joints_mj.qvel))
max_qacc = np.max(np.abs(prop_joints_mj.qacc))
if (max_qvel < _SETTLE_QVEL_TOL and max_qacc < _SETTLE_QACC_TOL):
return True
physics.data.time = original_time
if self._raise_exception_on_settle_failure:
raise RuntimeError(
_SETTLING_PHYSICS_FAILED.format(
max_attempts=self._max_settle_physics_attempts,
max_time=self._max_settle_physics_time,
max_qvel=max_qvel,
max_qacc=max_qacc,
))
else:
log_str = _SETTLING_PHYSICS_FAILED.format(
max_attempts='%s',
max_time='%s',
max_qvel='%s',
max_qacc='%s',
)
logging.warning(log_str, self._max_settle_physics_attempts,
self._max_settle_physics_time, max_qvel, max_qacc)
return False
if self._settle_physics:
place_and_settle()
else:
place_props()
