def __init__(self, target):
'''
target - 3d (v_x, v_y, v_z) vector
'''
self.target_xy = np.array(target[:2])
self.target_z = target[2]
# PID controller
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
def __init__(self, target_roomba):
self.target_roomba = target_roomba
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
# estimate roomba velocity
self.last_target_xy = None
self.land_time = None
def __init__(self, hold_duration):
self.hold_duration = hold_duration
self.state = HoldPositionTaskStates.init
# Hold position properties
self.start_time = 0
self.hold_xy = np.array([0, 0])
self.hold_z = 0
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
def __init__(self, target_roomba, offset_xy):
'''
target_roomba - target roomba tag
offset_xy - float[2] that defines x and y offset from the roomba
'''
self.target_roomba = target_roomba
self.offset_xy = offset_xy
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
# estimate roomba velocity
self.last_target_xy = None
class XYZTranslationTask(Task):
'''
A task to move the drone to an absolute position somewhere
in the arena. The task accepts a single 3d vector that defines
the target position as an [x,y,z] coordinate. Two PID controllers
are used: one for the xy-plane and one for the z-axis, in order
to control the drone's position.
The task terminates with SUCCESS when the drone is within
PITTRAS_XYZ_TRANSLATION_ACCURACY meters of the target position.
'''
def __init__(self, target):
'''
target - 3d (x,y,z) vector
'''
self.target_xy = np.array(target[:2])
self.target_z = target[2]
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
def update(self, delta, elapsed, state_controller, environment):
# fetch current odometry
drone_state = state_controller.query('DroneState', environment)
# check if we have reached the target
dist = np.linalg.norm(self.target_xy - drone_state['xy_pos'])
if dist < cfg.PITTRAS_XYZ_TRANSLATION_ACCURACY:
self.complete(TaskState.SUCCESS)
return
# PID calculations
control_xy = self.pid_xy.get_control(
self.target_xy - drone_state['xy_pos'], -drone_state['xy_vel'],
delta)
control_z = self.pid_z.get_control(
self.target_z - drone_state['z_pos'], -drone_state['z_vel'], delta)
# rotate the control xy vector counterclockwise about
# the origin by the current yaw
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# perform control action
environment.agent.control(adjusted_xy, 0, control_z)
def __init__(self, target_roomba, block_vector):
'''
target_roomba - target roomba tag
block_vector - float[2] that specifies where to land with respect to the
the target roomba
'''
self.target_roomba = target_roomba
self.block_vector = np.array(block_vector)
self.land_time = None
# calculated on first update
self.target_yaw = None
self.target_xy = None
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_yaw = PIDController(cfg.PITTRAS_PID_YAW)
def __init__(self, target_roomba, offset_xy, timeout):
'''
target_roomba - target roomba tag
offset_xy - float[2] that defines x and y offset from the roomba
timeout - task duration in milliseconds. If less than or equal to zero,
the task will run indefinitely
'''
self.target_roomba = target_roomba
self.offset_xy = offset_xy
self.timeout = timeout
# TODO(hgarrereyn): remove once tasks get per-task elapsed time rather than global
self.start_time = None
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
# estimate roomba velocity
self.last_target_xy = None
class LandTask(Task):
'''
A task to land the drone.
'''
def __init__(self):
# state machine
self._state = LandTaskState.init
# xy PID controller for velocity control
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
def update(self, delta, elapsed, state_controller, environment):
# fetch drone state
drone_state = state_controller.query('DroneState', environment)
height = drone_state['z_pos']
# we don't care about position but the horizontal target velocity is zero
control_xy = self.pid_xy.get_control(
np.zeros(2),
- drone_state['xy_vel'],
delta
)
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# Check if we reached the target landing height tolerance
# TODO: if landing sensors are provided via the state controller
# then use them to determine if the drone has landed
if height > 0:
# Descend if above the landing height tolerance
self._state = LandTaskState.descend
environment.agent.control(adjusted_xy, 0, cfg.PITTRAS_LAND_VELOCITY)
else:
# Stop descending if below the target height tolerance
self._state = LandTaskState.done
self.complete(TaskState.SUCCESS)
class VelocityTask(Task):
'''
A task to accelerate/decelerate the drone to a given velocity. The task
accepts a single 3d vector that defines the target velocty.
'''
def __init__(self, target):
'''
target - 3d (v_x, v_y, v_z) vector
'''
self.target_xy = np.array(target[:2])
self.target_z = target[2]
# PID controller
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
def update(self, delta, elapsed, state_controller, environment):
# Fetch current odometry
drone_state = state_controller.query('DroneState', environment)
if np.linalg.norm(self.target_xy - drone_state['xy_vel']) < \
cfg.PITTRAS_VELOCITY_TOLERANCE:
self.complete(TaskState.SUCCESS)
return
# Calculate control acceleration vector
control_xy = self.pid_xy.get_control(
self.target_xy - drone_state['xy_vel'],
[0, 0],
delta
)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# Perform control action
environment.agent.control(adjusted_xy, 0, self.target_z)
def __init__(self):
# state machine
self._state = LandTaskState.init
# xy PID controller for velocity control
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
class GoToRoombaTask(Task):
def __init__(self, target_roomba, offset_xy):
'''
target_roomba - target roomba tag
offset_xy - float[2] that defines x and y offset from the roomba
'''
self.target_roomba = target_roomba
self.offset_xy = offset_xy
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
# estimate roomba velocity
self.last_target_xy = None
def update(self, delta, elapsed, state_controller, environment):
# fetch roomba odometry
target_roombas, _ = state_controller.query('RoombaState', environment)
# fetch drone odometry
drone_state = state_controller.query('DroneState', environment)
if not self.target_roomba in target_roombas:
self.complete(TaskState.FAILURE, "Roomba not found")
return
roomba = target_roombas[self.target_roomba]
adjusted_offset_xy = geometry.rotate_vector(self.offset_xy,
roomba['heading'])
target_xy = np.array(roomba['pos']) + adjusted_offset_xy
if np.linalg.norm(target_xy - drone_state['xy_pos']
) < cfg.PITTRAS_XYZ_TRANSLATION_ACCURACY:
self.complete(TaskState.SUCCESS)
return
if self.last_target_xy is None:
self.last_target_xy = target_xy
target_vel_xy = ((target_xy - self.last_target_xy) / delta)
# PID calculations
control_xy = self.pid_xy.get_control(
target_xy - drone_state['xy_pos'],
target_vel_xy - drone_state['xy_vel'], delta)
control_z = self.pid_z.get_control(
cfg.PITTRAS_TRACK_ROOMBA_HEIGHT - drone_state['z_pos'],
-drone_state['z_vel'], delta)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# perform control action
environment.agent.control(adjusted_xy, 0, control_z)
# update delayed trackers
self.last_target_xy = target_xy
class TrackRoombaTask(Task):
'''
A task to follow a roomba with an optional offset vector. If the supplied timeout
duration is less than or equal to zero, the task will run indefinitely.
The drone will attempt to maintain a height of PITTRAS_TRACK_ROOMBA_HEIGHT while
tracking the roomba.
'''
def __init__(self, target_roomba, offset_xy, timeout):
'''
target_roomba - target roomba tag
offset_xy - float[2] that defines x and y offset from the roomba
timeout - task duration in milliseconds. If less than or equal to zero,
the task will run indefinitely
'''
self.target_roomba = target_roomba
self.offset_xy = offset_xy
self.timeout = timeout
# TODO(hgarrereyn): remove once tasks get per-task elapsed time rather than global
self.start_time = None
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
# estimate roomba velocity
self.last_target_xy = None
def update(self, delta, elapsed, state_controller, environment):
# first update
if self.start_time is None:
self.start_time = elapsed
# fetch roomba odometry
target_roombas, _ = state_controller.query('RoombaState', environment)
# fetch drone odometry
drone_state = state_controller.query('DroneState', environment)
if not self.target_roomba in target_roombas:
self.complete(TaskState.FAILURE, "Roomba not found")
return
# calculate xy target position
roomba = target_roombas[self.target_roomba]
target_xy = np.array(roomba['pos']) + self.offset_xy
# check if we should timeout
if self.timeout > 0 and elapsed - self.start_time >= self.timeout:
self.complete(TaskState.SUCCESS)
return
if self.last_target_xy is None:
self.last_target_xy = target_xy
target_vel_xy = ((target_xy - self.last_target_xy) / delta)
# PID calculations
control_xy = self.pid_xy.get_control(
target_xy - drone_state['xy_pos'],
target_vel_xy - drone_state['xy_vel'], delta)
control_z = self.pid_z.get_control(
cfg.PITTRAS_TRACK_ROOMBA_HEIGHT - drone_state['z_pos'],
-drone_state['z_vel'], delta)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
self.last_target_xy = target_xy
# perform control action
environment.agent.control(adjusted_xy, 0, control_z)
class HitRoombaTask(Task):
'''
A task to bump the top of a roomba.
'''
def __init__(self, target_roomba):
self.target_roomba = target_roomba
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
# estimate roomba velocity
self.last_target_xy = None
self.land_time = None
def update(self, delta, elapsed, state_controller, environment):
# fetch roomba odometry
target_roombas, _ = state_controller.query('RoombaState', environment)
# fetch drone odometry
drone_state = state_controller.query('DroneState', environment)
if not self.target_roomba in target_roombas:
self.complete(TaskState.FAILURE, "Roomba not found")
return
roomba = target_roombas[self.target_roomba]
target_xy = np.array(roomba['pos'])
# check if the roomba is too far away
if np.linalg.norm(target_xy - drone_state['xy_pos']
) > cfg.PITTRAS_HIT_ROOMBA_MAX_START_DIST:
self.complete(TaskState.FAILURE, "Roomba is too far away")
return
if self.last_target_xy is None:
self.last_target_xy = target_xy
target_vel_xy = ((target_xy - self.last_target_xy) / delta)
# PID calculations
control_xy = self.pid_xy.get_control(
target_xy - drone_state['xy_pos'],
target_vel_xy - drone_state['xy_vel'], delta)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# perform control action
environment.agent.control(adjusted_xy, 0,
cfg.PITTRAS_HIT_ROOMBA_DESCENT_VELOCITY)
# check if we have hit the roomba
if drone_state['z_pos'] < cfg.PITTRAS_DRONE_PAD_ACTIVIATION_HEIGHT:
if self.land_time is None:
self.land_time = elapsed
if elapsed - self.land_time > cfg.PITTRAS_HIT_ROOMBA_FLOOR_TIME * 1000:
self.complete(TaskState.SUCCESS)
return
# update delayed trackers
self.last_target_xy = target_xy
class BlockRoombaTask(Task):
'''
A task that lands in front of a roomba.
'''
def __init__(self, target_roomba, block_vector):
'''
target_roomba - target roomba tag
block_vector - float[2] that specifies where to land with respect to the
the target roomba
'''
self.target_roomba = target_roomba
self.block_vector = np.array(block_vector)
self.land_time = None
# calculated on first update
self.target_yaw = None
self.target_xy = None
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_yaw = PIDController(cfg.PITTRAS_PID_YAW)
def update(self, delta, elapsed, state_controller, environment):
# fetch roomba odometry
target_roombas, _ = state_controller.query('RoombaState', environment)
# fetch drone odometry
drone_state = state_controller.query('DroneState', environment)
if self.land_time is not None:
if elapsed - self.land_time > cfg.PITTRAS_BLOCK_FLOOR_TIME * 1000:
self.complete(TaskState.SUCCESS)
return
if drone_state['z_pos'] == 0:
self.land_time = elapsed
if not self.target_roomba in target_roombas:
self.complete(TaskState.FAILURE, "Roomba not found")
return
roomba = target_roombas[self.target_roomba]
# calculate the target yaw so that we turn less than 45 degrees
# in either direction and land with a bumper side perpendicular
# to the direction of roomba motion
self.target_yaw = BlockRoombaTask._calculate_target_yaw(
drone_state['yaw'], roomba['heading'])
# calculate the target landing position
block_vector = geometry.rotate_vector(self.block_vector,
roomba['heading'])
self.target_xy = np.array(roomba['pos']) + block_vector
# PID calculations
control_xy = self.pid_xy.get_control(
self.target_xy - drone_state['xy_pos'], -drone_state['xy_vel'],
delta)
control_yaw = self.pid_yaw.get_control(
self.target_yaw - drone_state['yaw'], -drone_state['yaw'], delta)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# perform control action
environment.agent.control(adjusted_xy, control_yaw,
cfg.PITTRAS_BLOCK_DESCENT_VEL)
@staticmethod
def _calculate_target_yaw(drone_heading, roomba_heading):
'''
Returns a yaw within 45 degrees of the current drone heading
such that one of the four faces is perpendicular to the roomba.
'''
angle_diff = (drone_heading - roomba_heading) % (np.pi / 2)
if angle_diff < (np.pi / 4):
return drone_heading - angle_diff
else:
return drone_heading - angle_diff + (np.pi / 2)
class HoldPositionTask(Task):
'''
A task to hold the drone at its current absolute position for some duration
of time. The task accepts a float64 that specifies such duration in seconds,
which means holding the current position indefinitely if less than or equal
to zero.
'''
def __init__(self, hold_duration):
self.hold_duration = hold_duration
self.state = HoldPositionTaskStates.init
# Hold position properties
self.start_time = 0
self.hold_xy = np.array([0, 0])
self.hold_z = 0
# PID controllers
self.pid_xy = PIDController(cfg.PITTRAS_PID_XY, dimensions=2)
self.pid_z = PIDController(cfg.PITTRAS_PID_Z)
def update(self, delta, elapsed, state_controller, environment):
if (self.state == HoldPositionTaskStates.done
or self.state == HoldPositionTaskStates.failed):
return
# Fetch current odometry
drone_state = state_controller.query('DroneState', environment)
if self.state == HoldPositionTaskStates.init:
# TODO(zacyu): Remove this when `elapsed` is changed to represent
# the elipsed time since the start of the task.
self.start_time = elapsed
self.state = HoldPositionTaskStates.holding
self.hold_xy = drone_state['xy_pos']
self.hold_z = drone_state['z_pos']
return
if (self.hold_duration > 0) and (elapsed - self.start_time >
self.hold_duration * 1000):
if (np.linalg.norm(drone_state['xy_pos'] - self.hold_xy) <
cfg.PITTRAS_HOLD_POSITION_TOLERANCE
and abs(drone_state['z_pos'] - self.hold_z) <
cfg.PITTRAS_HOLD_POSITION_TOLERANCE):
self.complete(TaskState.SUCCESS)
self.state = HoldPositionTaskStates.done
else:
self.complete(TaskState.FAILURE)
self.state = HoldPositionTaskStates.failed
# PID calculations
control_xy = self.pid_xy.get_control(
self.hold_xy - drone_state['xy_pos'], -drone_state['xy_vel'],
delta)
control_z = self.pid_z.get_control(self.hold_z - drone_state['z_pos'],
-drone_state['z_vel'], delta)
# normalize acceleration vector
adjusted_xy = geometry.rotate_vector(control_xy, -drone_state['yaw'])
# Perform control action
environment.agent.control(adjusted_xy, 0, control_z)