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()