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