def _noisy_action_impl( scene_node: hsim.SceneNode, translate_amount: float, rotate_amount: float, multiplier: float, model: MotionNoiseModel, ): # Perform the action in the coordinate system of the node transform = scene_node.transformation move_ax = -transform[_Z_AXIS].xyz perp_ax = transform[_X_AXIS].xyz # + EPS to make sure 0 is positive. We multiply the mean by the sign of the translation # as otherwise forward would overshoot on average and backward would undershoot, while # both should overshoot translation_noise = multiplier * np.random.multivariate_normal( np.sign(translate_amount + 1e-8) * model.linear.mean, model.linear.cov) scene_node.translate_local(move_ax * (translate_amount + translation_noise[0]) + perp_ax * translation_noise[1]) # Same deal with rotation about + EPS and why we multiply by the sign rot_noise = multiplier * np.random.multivariate_normal( np.sign(rotate_amount + 1e-8) * model.rotation.mean, model.rotation.cov) scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise[0])) scene_node.rotation = scene_node.rotation.normalized()
def action( self, obj: hsim.SceneNode, action_name: str, actuation_spec: ActuationSpec, apply_filter: bool = True, ): r"""Performs the action specified by :py:attr:`action_name` on the object Args: obj (hsim.SceneNode): SceneNode to perform the action on action_name (str): Name of the action. Used to index the move_func_map to retrieve the function which implements this action actuation_spec (ActuationSpec): Specifies the parameters needed by the function apply_filter (bool): Whether or not to apply the move_filter_fn after the action """ assert ( action_name in move_func_map), f"No action named {action_name} in the move map" start_pos = obj.absolute_position() move_func_map[action_name](obj, actuation_spec) end_pos = obj.absolute_position() if apply_filter: filter_end = self.move_filter_fn(start_pos, end_pos) obj.translate(filter_end - end_pos)
def _noisy_action_impl( scene_node: hsim.SceneNode, translate_amount: float, rotate_amount: float, multiplier: float, model: MotionNoiseModel, motion_type: str, ): # Perform the action in the coordinate system of the node transform = scene_node.transformation move_ax = -transform[_Z_AXIS].xyz perp_ax = transform[_X_AXIS].xyz if motion_type == "rotational": translation_noise = multiplier * model.linear.sample() else: # The robot will always move a little bit. This has to be defined based on the intended actuation # as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll # get to the truncation of 3 sigma trunc = [(-0.95 * np.abs(translate_amount), None), None] translation_noise = multiplier * model.linear.sample(trunc) # + EPS to make sure 0 is positive. We multiply by the sign of the translation # as otherwise forward would overshoot on average and backward would undershoot, while # both should overshoot translation_noise *= np.sign(translate_amount + 1e-8) scene_node.translate_local( move_ax * (translate_amount + translation_noise[0]) + perp_ax * translation_noise[1] ) if motion_type == "linear": rot_noise = multiplier * model.rotation.sample() else: # The robot will always turn a little bit. This has to be defined based on the intended actuation # as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll # get to the truncation of 3 sigma trunc = [(-0.95 * np.abs(np.deg2rad(rotate_amount)), None)] rot_noise = multiplier * model.rotation.sample(trunc) # Same deal with rotation about + EPS and why we multiply by the sign rot_noise *= np.sign(rotate_amount + 1e-8) scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise)) scene_node.rotation = scene_node.rotation.normalized()
def __init__( self, scene_node: hsim.SceneNode, agent_config=None, sensors=None, controls=None ): self.agent_config = agent_config if agent_config else AgentConfiguration() self.sensors = sensors if sensors else SensorSuite() self.controls = controls if controls else ObjectControls() self.body = mn.scenegraph.AbstractFeature3D(scene_node) scene_node.type = hsim.SceneNodeType.AGENT self.reconfigure(self.agent_config)
def action( self, obj: hsim.SceneNode, action_name: str, actuation_spec: ActuationSpec, apply_filter: bool = True, ) -> bool: r"""Performs the action specified by :py:attr:`action_name` on the object Args: obj (hsim.SceneNode): SceneNode to perform the action on action_name (str): Name of the action. Used to index the move_func_map to retrieve the function which implements this action actuation_spec (ActuationSpec): Specifies the parameters needed by the function apply_filter (bool): Whether or not to apply the move_filter_fn after the action Returns: bool: Whether or not the action taken resulted in a collision """ assert ( action_name in move_func_map), f"No action named {action_name} in the move map" start_pos = obj.absolute_translation move_func_map[action_name](obj, actuation_spec) end_pos = obj.absolute_translation collided = False if apply_filter: filter_end = self.move_filter_fn(start_pos, end_pos) # Update the position to respect the filter obj.translate(filter_end - end_pos) dist_moved_before_filter = (end_pos - start_pos).dot() dist_moved_after_filter = (filter_end - start_pos).dot() # NB: There are some cases where ||filter_end - end_pos|| > 0 when a # collision _didn't_ happen. One such case is going up stairs. Instead, # we check to see if the the amount moved after the application of the filter # is _less_ the the amount moved before the application of the filter collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter return collided
def action( self, obj: hsim.SceneNode, action_name: str, actuation_spec: ActuationSpec, apply_filter: bool = True, ) -> bool: r"""Performs the action specified by :p:`action_name` on the object :param obj: `scene.SceneNode` to perform the action on :param action_name: Name of the action. Used to query the `registry` to retrieve the function which implements this action :param actuation_spec: Specifies the parameters needed by the function :param apply_filter: Whether or not to apply the `move_filter_fn` after the action :return: Whether or not the action taken resulted in a collision """ start_pos = obj.absolute_translation move_fn = registry.get_move_fn(action_name) assert move_fn is not None, f"No move_fn for action '{action_name}'" move_fn(obj, actuation_spec) end_pos = obj.absolute_translation collided = False if apply_filter: filter_end = self.move_filter_fn(start_pos, end_pos) # Update the position to respect the filter obj.translate(filter_end - end_pos) dist_moved_before_filter = (end_pos - start_pos).dot() dist_moved_after_filter = (filter_end - start_pos).dot() # NB: There are some cases where ||filter_end - end_pos|| > 0 when a # collision _didn't_ happen. One such case is going up stairs. Instead, # we check to see if the the amount moved after the application of the filter # is _less_ the the amount moved before the application of the filter collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter return collided
def __init__( self, scene_node: hsim.SceneNode, agent_config: Optional[AgentConfiguration] = None, _sensors: Optional[SensorSuite] = None, controls: Optional[ObjectControls] = None, ) -> None: self.agent_config = agent_config if agent_config else AgentConfiguration() self._sensors = _sensors if _sensors else SensorSuite() self.controls = controls if controls else ObjectControls() self.body = mn.scenegraph.AbstractFeature3D(scene_node) scene_node.type = hsim.SceneNodeType.AGENT self.reconfigure(self.agent_config) self.initial_state: Optional[AgentState] = None
def _rotate_local(scene_node: hsim.SceneNode, theta: float, axis: int): _rotate_local_fns[axis](scene_node, mn.Deg(theta)) scene_node.rotation = scene_node.rotation.normalized()
def _move_along(scene_node: hsim.SceneNode, distance: float, axis: int): ax = scene_node.absolute_transformation()[axis].xyz scene_node.translate_local(ax * distance)
def _rotate_local(obj: hsim.SceneNode, theta: float, axis: int): ax = np.zeros(3, dtype=np.float32) ax[axis] = 1 obj.rotate_local(np.deg2rad(theta), ax) obj.normalize()
def _move_along(obj: hsim.SceneNode, distance: float, axis: int): ax = obj.absolute_transformation()[0:3, axis] obj.translate_local(ax * distance)