def _put_in_world_referential(orientation: float, cmd: Pose) -> Pose:
if config['GAME']['on_negative_side']:
cmd.position = rotate(cmd.position, -np.pi - orientation)
cmd.x *= -1
cmd.orientation *= -1
else:
cmd.position = rotate(cmd.position, orientation)
return cmd
def _put_in_robots_referential(robot: Robot, cmd: Pose) -> Pose:
if config['GAME']['on_negative_side']:
cmd.x *= -1
cmd.orientation *= -1
cmd.position = rotate(cmd.position, np.pi + robot.orientation)
else:
cmd.position = rotate(cmd.position, -robot.orientation)
return cmd