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)
