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)
