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_transformation()._translation move_func_map[action_name](obj, actuation_spec) end_pos = obj.absolute_transformation()._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 = np.linalg.norm(end_pos - start_pos) dist_moved_after_filter = np.linalg.norm(filter_end - start_pos) # 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 _move_along(scene_node: hsim.SceneNode, distance: float, axis: int): ax = scene_node.absolute_transformation()[axis].xyz scene_node.translate_local(ax * distance)
def _move_along(obj: hsim.SceneNode, distance: float, axis: int): ax = obj.absolute_transformation()[0:3, axis] obj.translate_local(ax * distance)