class KinematicModel:
def __init__(self, config: configparser.ConfigParser):
self.__distance_margin = config.getfloat('distance_controller',
'margin')
self.__current_goal: Tuple[int, int] = None
self.__v_pid = PID(config.getfloat('angle_controller', 'k_P'),
config.getfloat('angle_controller', 'k_I'),
config.getfloat('angle_controller', 'k_D'))
self.__w_pid = PID(config.getfloat('angle_controller', 'k_P'),
config.getfloat('angle_controller', 'k_I'),
config.getfloat('angle_controller', 'k_D'))
def get_kinematic_values(self, trunk: Tuple[int, int], hood: Tuple[int,
int],
goal: Tuple[int, int]) -> KinematicValues:
if goal != self.__current_goal:
self.__v_pid.reset()
self.__w_pid.reset()
self.__current_goal = goal
robot = ((trunk[0] + hood[0]) / 2, (trunk[1] + hood[1]) / 2)
delta_phi = self.get_angle_to_goal(goal, hood, robot, trunk)
dist_to_goal = self.get_distance_to_goal(robot, goal)
w = self.__w_pid(delta_phi)
v = self.__v_pid(dist_to_goal)
return KinematicValues(v, w)
@staticmethod
def get_angle_to_goal(goal, hood, robot, trunk):
delta_phi = atan2(goal[1] - robot[1], goal[0] - robot[0])
# Account for orientation
if hood[1] < trunk[1]:
delta_phi += pi + delta_phi
return delta_phi
def get_distance_to_goal(self, robot: Tuple[float, float],
goal: Tuple[float, float]) -> float:
dist_to_goal = sqrt((robot[0] - goal[0])**2 + (robot[1] - goal[1])**2)
return 0 if dist_to_goal < self.__distance_margin else dist_to_goal
def mainThread(debug=False):
""" Controls the robot including joystick input, computer vision, line following, etc.
Arguments:
debug: (optional) log debugging data
"""
DC = ControllerUtils.DriveController(flip=[1, 0, 1, 0, 0, 0, 0, 0])
# Get Controller
gamepad = None
while (not gamepad) and execute['mainThread']:
time.sleep(5)
try:
gamepad = ControllerUtils.identifyController()
except Exception as e:
print(e)
newestImage = []
newestSensorState = {
'imu': {
'calibration': {
'sys': 2,
'gyro': 3,
'accel': 0,
'mag': 0
},
'gyro': {
'x': 0,
'y': 0,
'z': 0
},
'vel': {
'x': -0.01,
'y': 0.0,
'z': -0.29
}
},
'temp': 25
}
# Initalize PID controllers
Kp = 1
Kd = 0.1
Ki = 0.05
xRotPID = PID(Kp, Kd, Ki, setpoint=0)
yRotPID = PID(Kp, Kd, Ki, setpoint=0)
zRotPID = PID(Kp, Kd, Ki, setpoint=0)
mode = "user-control"
override = False
print("mode:", mode)
print("override:", override)
lastMsgTime = time.time()
minTime = 1.0 / 10.0
while execute['mainThread']:
while not mainQueue.empty():
recvMsg = mainQueue.get()
if recvMsg['tag'] == 'cam':
#newestImage = CommunicationUtils.decodeImage(recvMsg['data'])
pass
elif recvMsg['tag'] == 'sensor':
newestSensorState = recvMsg['data']
# Get Joystick Input
event = gamepad.read_one()
if event:
if (ControllerUtils.isOverrideCode(event, action="down")):
override = True
print("override:", override)
elif (ControllerUtils.isOverrideCode(event, action="up")
and override):
override = False
print("override:", override)
if (ControllerUtils.isStopCode(event)):
handlePacket(CommunicationUtils.packet("stateChange", "close"))
time.sleep(1)
stopAllThreads()
elif (ControllerUtils.isZeroMotorCode(event)):
handlePacket(
CommunicationUtils.packet("motorData",
DC.zeroMotors(),
metadata="drivetrain"))
elif (ControllerUtils.isStabilizeCode(event)):
if (mode != "stabilize"):
# Reset PID controllers
xRotPID.reset()
yRotPID.reset()
zRotPID.reset()
xRotPID.tunings = (Kp, Ki, Kd)
yRotPID.tunings = (Kp, Ki, Kd)
zRotPID.tunings = (Kp, Ki, Kd)
# Assuming the robot has been correctly calibrated, (0,0,0) should be upright
xRotPID.setpoint = 0
yRotPID.setpoint = 0
zRotPID.setpoint = 0
mode = "stabilize"
print("mode:", mode)
else:
mode = "user-control"
print("mode:", mode)
if (mode == "user-control" or override):
DC.updateState(event)
speeds = DC.calcThrust()
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
elif (mode == "stabilize" and not override):
xTgt = xRotPID(newestSensorState["imu"]["gyro"]["x"])
yTgt = yRotPID(newestSensorState["imu"]["gyro"]["y"])
zTgt = zRotPID(newestSensorState["imu"]["gyro"]["z"])
speeds = DC.calcPIDRot(xTgt, yTgt, zTgt)
if (time.time() - lastMsgTime > minTime):
handlePacket(
CommunicationUtils.packet("motorData",
speeds,
metadata="drivetrain"))
lastMsgTime = time.time()
class DroneController():
"""A class to implement controllers, given a frame and a rectangular bounding box for an object in the frame.
There are PIDs for throttle, pitch, and yaw, each tuned carefully to work with the CoDrone and my setup."""
def __init__(self,
frame,
keep_distance=False,
setpoint_throttle=-60.0,
setpoint_yaw=0.0,
meter_distance=150.0,
ctr_rect_proportions=(3 / 16, 3 / 16)):
"""rect_proportions is a tuple with the width of the X part of rectangle as first, and width of y as second"""
# flag to know if we're controlling distance too
self.keep_distance = keep_distance
# throttle setpoint negative so that head will be in top of frame, as bboxy - frame - will be negative
# pid for x-axis, or yaw adjustments
# Kp, Ki, Kd are gain constants on the principal, integral, and derivative terms
# setpoint is the pixel x distance from center of frame to center of object
# output limits are the limits to CoDrone's yaw
# sample_time is minimum time that this will update. here, it'll be bounded naturally by the FPS
# of the computer vision system, but putting in a limit in case other folks run on upgraded hardware where that
# isn't a bottleneck
self.setpoint_throttle = setpoint_throttle
self.setpoint_yaw = setpoint_yaw
if keep_distance:
self.meter_distance = meter_distance
self.pitch_pid = PID(Kp=.3,
Ki=.1,
Kd=.1,
setpoint=0.0,
sample_time=round(1 / 14, 2),
output_limits=(-50, 50))
self.yaw_pid = PID(Kp=.1,
Ki=0.1,
Kd=0.2,
setpoint=self.setpoint_throttle,
sample_time=round(1 / 14, 2),
output_limits=(-15, 15))
# because the drone is heavy with camera going down is much easier than up
# reflecting this in output limits in the throttle pid
self.throttle_pid = PID(Kp=.5,
Ki=0.3,
Kd=0.4,
setpoint=self.setpoint_yaw,
sample_time=round(1 / 14, 2),
output_limits=(-20, 90))
# flags to determine when to reset the pid if nescessary
# for instance, when our error is within tolerance for throttle, we'll reset the throttle PID
# when we're back out of tolerance we'll use it again
self.throttle_pid_on = True
self.yaw_pid_on = True
if keep_distance:
self.pitch_pid_on = True
self.throttle_errors = []
self.throttle_outputs = []
self.yaw_errors = []
self.yaw_outputs = []
self.pitch_errors = []
self.pitch_outputs = []
# setting a center rectangle where if a face is in that area, we won't be out of tolerance
self.height, self.width = frame.shape[:2]
x1 = int((self.width / 2) - (self.width * ctr_rect_proportions[0]))
x2 = int((self.width / 2) + (self.width * ctr_rect_proportions[0]))
y1 = int((self.height / 2) - (self.height * ctr_rect_proportions[1]))
y2 = int((self.height / 2) + (self.height * ctr_rect_proportions[1]))
self.center_rectangle_coords = (x1, y1, x2, y2)
self.min_abs_x_error = (self.center_rectangle_coords[2] -
self.center_rectangle_coords[0]) / 2
self.min_abs_y_error = (self.center_rectangle_coords[3] -
self.center_rectangle_coords[1]) / 2
# setting minimum distance error in meters. The current approach for estimating distance is rather inaccurate.
# so, setting this high.
if keep_distance:
self.min_distance_error = 40
@staticmethod
def calc_x_y_error(frame, bounding_box):
"""This takes a frame object and a tuple of a bounding box where the entries are coordinates of
top left and bottom right corners, ie, (x1, y1, x2, y2) and outputs the x distance from object center to
frame center and y distance from object center to frame center
"""
# calculate center of image
height, width = frame.shape[:2]
frame_center_coords = (width / 2, height / 2)
bb_x_center = (bounding_box[2] - bounding_box[0]) / 2 + bounding_box[0]
bb_y_center = (bounding_box[3] - bounding_box[1]) / 2 + bounding_box[1]
bb_center_coords = (bb_x_center, bb_y_center)
x_distance_from_center = frame_center_coords[0] - bb_center_coords[0]
# making it so if frame center is below object center, result is negative so that pid tries to increase throttle
y_distance_from_center = bb_center_coords[1] - frame_center_coords[1]
return x_distance_from_center, y_distance_from_center
def _get_yaw_from_x_error(self, x_error):
if abs(x_error) >= self.min_abs_x_error:
yaw_output = self.yaw_pid(x_error)
components_yaw = self.yaw_pid.components
if not self.yaw_pid_on:
self.yaw_pid_on = True
else:
yaw_output = 0
components_yaw = ("no_error", "no_error", "no_error")
if self.yaw_pid_on:
self.yaw_pid_on = False
self.yaw_pid.reset()
return yaw_output, components_yaw
def _get_throttle_from_y_error(self, y_error):
if abs(y_error) > self.min_abs_y_error:
throttle_output = self.throttle_pid(y_error)
components_throttle = self.throttle_pid.components
if not self.throttle_pid_on:
self.yaw_pid_on = True
else:
throttle_output = 0
components_throttle = ("no_error", "no_error", "no_error")
if self.throttle_pid_on:
self.throttle_pid_on = False
self.throttle_pid.reset()
return throttle_output, components_throttle
def _get_pitch_from_distance(self, distance_error):
if abs(distance_error) > self.min_distance_error:
pitch_output = self.pitch_pid(distance_error)
components_pitch = self.pitch_pid.components
if not self.pitch_pid_on:
self.pitch_pid_on = True
else:
pitch_output = 0.0
components_pitch = ("no_error", "no_error", "no_error")
if self.pitch_pid_on:
self.pitch_pid_on = False
self.pitch_pid.reset()
return pitch_output, components_pitch
def get_drone_movements(self,
frame,
bounding_box,
estimate_distance=False,
write_frame_debug_info=False):
x_error, y_error = DroneController.calc_x_y_error(frame, bounding_box)
self.throttle_errors.append(y_error)
self.yaw_errors.append(x_error)
yaw_output, components_yaw = self._get_yaw_from_x_error(x_error)
throttle_output, components_throttle = self._get_throttle_from_y_error(
y_error)
if estimate_distance:
frame, distance = DroneVision.calculate_distance(
frame, bounding_box)
distance_error = self.meter_distance - distance
self.pitch_errors.append(distance_error)
pitch_output, components_pitch = self._get_pitch_from_distance(
distance_error)
else:
pitch_output = 0.0
components_pitch = ("dist. False", "dist. False", "dist. False")
distance_error = 0.0
self.throttle_outputs.append(throttle_output)
self.yaw_outputs.append(yaw_output)
self.pitch_outputs.append(pitch_output)
if write_frame_debug_info:
frame = self.write_debug(frame, x_error, y_error, distance_error,
throttle_output, yaw_output, pitch_output,
components_throttle, components_yaw,
components_pitch)
return int(throttle_output), int(yaw_output), int(pitch_output), frame
def write_debug(self,
frame,
x_error,
y_error,
distance_error,
throttle_output,
yaw_output,
pitch_output,
components_throttle,
components_yaw,
components_pitch,
font=cv2.FONT_HERSHEY_SIMPLEX,
color=(0, 255, 0),
font_scale=.3,
font_thickness=1):
throttle_text = "Throttle: {} (y_error: {})".format(
round(throttle_output, 2), round(y_error, 2))
throttle_component_text = "Throttle components: {}".format([
round(component, 2)
if not isinstance(component, str) else component
for component in components_throttle
])
yaw_text = "Yaw: {} (x_error: {})".format(round(yaw_output, 2),
round(x_error, 2))
yaw_component_text = "Yaw components: {}".format([
round(component, 2)
if not isinstance(component, str) else component
for component in components_yaw
])
pitch_text = "Pitch: {} (distance_error: {})".format(
round(pitch_output, 2), round(distance_error, 2))
pitch_components = "Pitch components: {}".format([
round(component, 2)
if not isinstance(component, str) else component
for component in components_pitch
])
cv2.putText(frame, throttle_text, (0, 20), font, font_scale, color,
font_thickness)
cv2.putText(frame, throttle_component_text, (0, 30), font, font_scale,
color, font_thickness)
cv2.putText(frame, yaw_text, (0, 40), font, font_scale, color,
font_thickness)
cv2.putText(frame, yaw_component_text, (0, 50), font, font_scale,
color, font_thickness)
cv2.putText(frame, pitch_text, (0, 60), font, font_scale, color,
font_thickness)
cv2.putText(frame, pitch_components, (0, 70), font, font_scale, color,
font_thickness)
# writing center rectangle
cv2.rectangle(
frame,
(self.center_rectangle_coords[0], self.center_rectangle_coords[1]),
(self.center_rectangle_coords[2], self.center_rectangle_coords[3]),
color=color)
return frame
def reset(self):
self.throttle_pid.reset()
self.yaw_pid.reset()
if self.keep_distance:
self.pitch_pid.reset()
class PIDMixin(BaseController):
def __init__(self, *args, **kwargs):
"""Constructor method.
"""
self.pid = PID()
super().__init__(*args, **kwargs)
self.update_settings(settings=kwargs.get('settings', {}))
self.__slow_mode = False
self.__logger = logging.getLogger('MCS.PID')
@classmethod
def get_defaults(cls) -> dict:
"""Return the default class values.
:return: A dict holding all class default values
:rtype: dict
"""
return {
'tunings': {
'Kp': 1.0, # PID Parameter P
'Ki': 0, # PID Parameter I
'Kd': 0, # PID Parameter D
},
'output': {
'minimum': 0, # Minimum PID output value
'current': 0,
'maximum': 100 # Maximum PID output value
},
'setpoint': {
'minimum': 0, # Minimum setpoint value
'current': 0,
'maximum': None # Maximum setpoint value
},
'sample_time': 0.1
}
def update_settings(self, settings: dict = {}):
"""Internal method to update the PID settings
The only reason fo this method was to improve code readbility because
:meth:`ExtendedClient.updateSettings()` got a little big.
:param settings: A dictionary holding all settings to be updated.
:type settings: dict
"""
super().update_settings(settings=settings)
if 'pid' not in self._settings:
self._settings['pid'] = PIDMixin.get_defaults()
settings = self._settings
pid_settings = settings.get('pid', None)
if pid_settings is None:
return
for key, value in pid_settings.get('tunings', {}).items():
if key in ['Kp', 'Ki', 'Kd']:
setattr(self, key, value)
if 'minimum' in pid_settings.get('output', {}):
self.pid_control_min = pid_settings['output']['minimum']
if 'maximum' in pid_settings.get('output', {}):
self.pid_control_max = pid_settings['output']['maximum']
if 'setpoint' in pid_settings:
self.pid_target = pid_settings['setpoint'].get(
'current', self.pid_target)
self.pid_target_minimum = pid_settings['setpoint'].get(
'minimum', self.pid_target_minimum)
self.pid_target_maximum = pid_settings['setpoint'].get(
'maximum', self.pid_target_maximum)
self.pid_sample_time = pid_settings.get('sample_time',
self.pid_sample_time)
def __set_slow_mode(self, state: bool = True):
"""Temporarly set different PID values
"""
if state and not self.__slow_mode:
self.__slow_mode = True
self.__previous_Kp = self.Kp
self.Kp = 0.1
self.logger.getChild('PID').info('Enable slow mode!')
if not state and self.__slow_mode:
self.__slow_mode = False
self.Kp = self.__previous_Kp
self.logger.getChild('PID').info('Disable slow mode!')
@property
def Kp(self) -> float:
"""PID P parameter
:getter: Return the current parameter P
:setter: Set the new parameter P and reset the PID control
"""
return self.pid.Kp
@Kp.setter
def Kp(self, value: float):
try:
value = float(value)
except ValueError:
pass
else:
self._settings['pid']['tunings']['Kp'] = value
self.pid.Kp = value
self.pid.reset()
@property
def Ki(self) -> float:
"""PID I parameter
:getter: Return the current parameter I
:setter: Set the new parameter I and reset the PID control
"""
return self.pid.Ki
@Ki.setter
def Ki(self, value: float):
try:
value = float(value)
except ValueError:
pass
else:
self._settings['pid']['tunings']['Ki'] = value
self.pid.Ki = value
self.pid.reset()
@property
def Kd(self) -> float:
"""PID D parameter
:getter: Return the current parameter D
:setter: Set the new parameter D and reset the PID control
"""
return self.pid.Kd
@Kd.setter
def Kd(self, value):
try:
value = float(value)
except ValueError:
pass
else:
self._settings['pid']['tunings']['Kd'] = value
self.pid.Kd = value
self.pid.reset()
@property
def pid_sample_time(self) -> float:
"""PID sample time
:getter: Return the current sample time in seconds
:setter: Set the new sample time in seconds
"""
return self.pid.sample_time
@pid_sample_time.setter
def pid_sample_time(self, value: float):
if float(value) <= 0:
raise ValueError('Sampletime cannot be 0 or less')
self.pid.sample_time = float(value)
self._settings['pid']['sample_time'] = self.pid.sample_time
@property
def pid_target(self) -> float:
"""Get the current temperature target (PID setpoint).
:getter: Return the current PID setpoint.
:setter: Set the PID setpoint after checking boundaries.
:type: float
..note::
Minimum and maximum values for the setpoint can be set with
:attr:`Temperatures.target_minimum` or :attr:`Temperatures.target_maximum`.
"""
return float(self._settings['pid']['setpoint']['current'])
@pid_target.setter
def pid_target(self, value: float):
"""Only enforce limits if set.
"""
try:
value = float(value)
except ValueError:
raise ValueError
value = max(value, self.pid_target_minimum or value)
value = min(value, self.pid_target_minimum or value)
self._settings['pid']['setpoint']['current'] = value
self.pid.setpoint = self.pid_target
self.pid.reset()
self.logger.getChild('PID').info(f'Set target to {value}')
@property
def pid_target_minimum(self) -> Union[float, None]:
"""Get the minimum setpoint (PID target) value
:getter: Return the current minimum setpoint. Default is ``None``.
:setter: Set a minimum setpoint value.
:type: float
.. note::
:attr:`Temperatures.target_minimum` must be smaller or equal to
:attr:`Temperatures.target_maximum` (if set).
"""
return self._settings['pid']['setpoint']['minimum']
@pid_target_minimum.setter
def pid_target_minimum(self, value: Union[float, None]):
"""Check that the value is not larger than an existing maximum.
"""
try:
value = None if value is None else float(value)
except ValueError:
value = None
if value is not None:
value = min(value, self.pid_target_maximum or value)
self.logger.getChild('PID').info(f'Set minimum target to {value}')
self._settings['pid']['setpoint']['minimum'] = value
@property
def pid_target_maximum(self) -> Union[float, None]:
"""Get the maximum setpoint (PID target) value
:getter: Return the current maximum setpoint. Default is ``None``.
:setter: set a maximum setpoint value.
:type: float
.. note::
:attr:`Temperatures.target_maximum` must be larger or equal to
:attr:`Temperatures.target_minimum` (if set).
"""
return self._settings['pid']['setpoint']['maximum']
@pid_target_maximum.setter
def pid_target_maximum(self, value: Union[float, None]):
try:
value = None if value is None else float(value)
except ValueError:
value = None
if value is not None:
value = max(value, self.pid_target_minimum or value)
self.logger.getChild('PID').debug(f'Set maximum target to {value}')
self._settings['pid']['setpoint']['maximum'] = value
@property
def pid_control(self) -> float:
"""Get the PID control value
:getter: Calculate the current PID output.
:setter: Update the current PID value in the settings.
:type: float
"""
if self.pid.auto_mode:
if min((self.current or 1) / (self.pid_target or 1), 1) < 0.75:
self.__set_slow_mode(True)
else:
self.__set_slow_mode(False)
self.pid_control = self.pid(self.current)
return self._settings['pid']['output']['current']
@pid_control.setter
def pid_control(self, value: float):
self._settings['pid']['output']['current'] = value
@property
def pid_control_min(self) -> Union[float, None]:
"""Get the minimum output limit.
:getter: Return the minimum output limit.
:type: float, NoneType
"""
return self._settings['pid']['output']['minimum']
@pid_control_min.setter
def pid_control_min(self, value: Union[float, None]):
try:
value = None if value is None else float(value)
except ValueError:
value = None
value = value if value is None else min(value, self.pid_control_max
or value)
self._settings['pid']['output']['minimum'] = value
self.pid.output_limits = self.pid_control_min, self.pid_control_max
self.pid.reset()
@property
def pid_control_max(self) -> Union[float, None]:
"""Get the maximum output limit.
:getter: Return the maximum output limit
:type: float, NoneType
"""
return self._settings['pid']['output']['maximum']
@pid_control_max.setter
def pid_control_max(self, value: Union[float, None]):
try:
value = None if value is None else float(value)
except ValueError:
value = None
value = value if value is None else max(value, self.pid_control_min
or value)
self._settings['pid']['output']['maximum'] = value
self.pid.output_limits = self.pid_control_min, self.pid_control_max
self.pid.reset()
def datalogger_collect(self) -> None:
super().datalogger_collect()
if 'pid' not in self.datalogger_current:
self.datalogger_current['pid'] = {}
self.datalogger_current['pid'].update({
'control_maximum': self.pid_control_max,
'control_minimum': self.pid_control_min,
'control': self.pid_control,
'target': self.pid_target,
'target_minimum': self.pid_target_minimum,
'target_maximum': self.pid_target_maximum,
'Kp': self.pid.Kp,
'Ki': self.pid.Ki,
'Kd': self.pid.Kd,
})
class SawyerEnv(MujocoEnv):
"""Initializes a Sawyer robot environment."""
def __init__(self,
gripper_type=None,
gripper_visualization=False,
use_indicator_object=False,
has_renderer=False,
has_offscreen_renderer=True,
render_collision_mesh=False,
render_visual_mesh=True,
control_freq=10,
horizon=1000,
ignore_done=False,
use_camera_obs=False,
camera_name="frontview",
camera_height=256,
camera_width=256,
camera_depth=False,
pid=True,
**kwargs):
"""
Args:
gripper_type (str): type of gripper, used to instantiate
gripper models from gripper factory.
gripper_visualization (bool): True if using gripper visualization.
Useful for teleoperation.
use_indicator_object (bool): if True, sets up an indicator object that
is useful for debugging.
has_renderer (bool): If true, render the simulation state in
a viewer instead of headless mode.
has_offscreen_renderer (bool): True if using off-screen rendering.
render_collision_mesh (bool): True if rendering collision meshes
in camera. False otherwise.
render_visual_mesh (bool): True if rendering visual meshes
in camera. False otherwise.
control_freq (float): how many control signals to receive
in every second. This sets the amount of simulation time
that passes between every action input.
horizon (int): Every episode lasts for exactly @horizon timesteps.
ignore_done (bool): True if never terminating the environment (ignore @horizon).
use_camera_obs (bool): if True, every observation includes a
rendered image.
camera_name (str): name of camera to be rendered. Must be
set if @use_camera_obs is True.
camera_height (int): height of camera frame.
camera_width (int): width of camera frame.
camera_depth (bool): True if rendering RGB-D, and RGB otherwise.
"""
self.has_gripper = gripper_type is not None
self.gripper_type = gripper_type
self.gripper_dof = 0
self.gripper_visualization = gripper_visualization
self.use_indicator_object = use_indicator_object
# Set to False if actions are supposed to be applied as is
self.normalised_actions = True
super().__init__(has_renderer=has_renderer,
has_offscreen_renderer=has_offscreen_renderer,
render_collision_mesh=render_collision_mesh,
render_visual_mesh=render_visual_mesh,
control_freq=control_freq,
horizon=horizon,
ignore_done=ignore_done,
use_camera_obs=use_camera_obs,
camera_name=camera_name,
camera_height=camera_height,
camera_width=camera_width,
camera_depth=camera_depth,
**kwargs)
if (pid):
self._setup_pid()
def _setup_pid(self):
self.pid = PID(
dim=self.mujoco_robot.dof,
sample_time=None,
)
limits = (self.sim.model.actuator_ctrlrange[:7, 0].copy(),
self.sim.model.actuator_ctrlrange[:7, 1].copy())
gains = self.mujoco_robot.velocity_pid_gains
kps = np.array([
gains['right_j{}'.format(actuator)]['p'] for actuator in range(7)
])
kis = np.array([
gains['right_j{}'.format(actuator)]['i'] for actuator in range(7)
])
kds = np.array([
gains['right_j{}'.format(actuator)]['d'] for actuator in range(7)
])
self.pid.tunings = (kps, kis, kds)
self.pid.output_limits = limits
def _load_model(self):
"""
Loads robot and optionally add grippers.
"""
super()._load_model()
if (self.pid is not None):
self.mujoco_robot = Sawyer(torque=True)
else:
self.mujoco_robot = Sawyer()
if self.has_gripper:
self.gripper = gripper_factory(self.gripper_type)
if not self.gripper_visualization:
self.gripper.hide_visualization()
self.mujoco_robot.add_gripper("right_hand", self.gripper)
self.gripper_dof = self.gripper.dof
def _reset_internal(self):
"""
Sets initial pose of arm and grippers.
"""
super()._reset_internal()
self.sim.data.qpos[
self._ref_joint_pos_indexes] = self.mujoco_robot.init_qpos
if self.has_gripper:
self.sim.data.qpos[
self._ref_gripper_joint_pos_indexes] = self.gripper.init_qpos
def _get_reference(self):
"""
Sets up necessary reference for robots, grippers, and objects.
"""
super()._get_reference()
# indices for joints in qpos, qvel
self.robot_joints = list(self.mujoco_robot.joints)
self._ref_joint_pos_indexes = [
self.sim.model.get_joint_qpos_addr(x) for x in self.robot_joints
]
self._ref_joint_vel_indexes = [
self.sim.model.get_joint_qvel_addr(x) for x in self.robot_joints
]
self._ref_joint_ids = [
self.sim.model.joint_name2id(x) for x in self.robot_joints
]
if self.use_indicator_object:
ind_qpos = self.sim.model.get_joint_qpos_addr("pos_indicator")
self._ref_indicator_pos_low, self._ref_indicator_pos_high = ind_qpos
ind_qvel = self.sim.model.get_joint_qvel_addr("pos_indicator")
self._ref_indicator_vel_low, self._ref_indicator_vel_high = ind_qvel
self.indicator_id = self.sim.model.body_name2id("pos_indicator")
# indices for grippers in qpos, qvel
if self.has_gripper:
self.gripper_joints = list(self.gripper.joints)
self._ref_gripper_joint_pos_indexes = [
self.sim.model.get_joint_qpos_addr(x)
for x in self.gripper_joints
]
self._ref_gripper_joint_vel_indexes = [
self.sim.model.get_joint_qvel_addr(x)
for x in self.gripper_joints
]
self._ref_gripper_body_indx = self.sim.model.body_name2id(
'right_gripper')
# indices for joint pos actuation, joint vel actuation, gripper actuation
# self._ref_joint_pos_actuator_indexes = [
# self.sim.model.actuator_name2id(actuator)
# for actuator in self.sim.model.actuator_names
# if actuator.startswith("pos")
# ]
self._ref_joint_vel_actuator_indexes = [
self.sim.model.actuator_name2id(actuator)
for actuator in self.sim.model.actuator_names
if actuator.startswith("vel")
]
self._ref_joint_torque_actuator_indexes = [
self.sim.model.actuator_name2id(actuator)
for actuator in self.sim.model.actuator_names
if actuator.startswith("torq")
]
if self.has_gripper:
self._ref_joint_gripper_actuator_indexes = [
self.sim.model.actuator_name2id(actuator)
for actuator in self.sim.model.actuator_names
if actuator.startswith("gripper")
]
# IDs of sites for gripper visualization
self.eef_site_id = self.sim.model.site_name2id("grip_site")
self.eef_cylinder_id = self.sim.model.site_name2id(
"grip_site_cylinder")
def move_indicator(self, pos):
"""
Sets 3d position of indicator object to @pos.
"""
if self.use_indicator_object:
index = self._ref_indicator_pos_low
self.sim.data.qpos[index:index + 3] = pos
def _pre_action(self, action):
"""
Overrides the superclass method to actuate the robot with the
passed joint velocities and gripper control.
Args:
action (numpy array): The control to apply to the robot. The first
@self.mujoco_robot.dof dimensions should be the desired
joint velocities. If self.normalised_actions is True then these actions should be normalised.
If the robot has a gripper, the next @self.gripper.dof dimensions should be
actuation controls for the gripper, which should always be normalised.
"""
assert len(
action) == self.dof, "environment got invalid action dimension"
low, high = self.action_spec
arm_action = action[:self.mujoco_robot.dof]
gripper_action = action[self.mujoco_robot.dof:self.mujoco_robot.dof +
self.gripper_dof]
if (self.normalised_actions):
arm_action = np.clip(arm_action, low[:self.mujoco_robot.dof],
high[:self.mujoco_robot.dof])
# rescale normalized action to control ranges
ctrl_range = self.control_range[:self.mujoco_robot.dof, :]
bias = 0.5 * (ctrl_range[:, 1] + ctrl_range[:, 0])
weight = 0.5 * (ctrl_range[:, 1] - ctrl_range[:, 0])
arm_action = bias + weight * arm_action
if self.has_gripper:
gripper_action = self.gripper.format_action(gripper_action)
# rescale normalized action to control ranges
ctrl_range = self.control_range[self.mujoco_robot.dof:, :]
bias = 0.5 * (ctrl_range[:, 1] + ctrl_range[:, 0])
weight = 0.5 * (ctrl_range[:, 1] - ctrl_range[:, 0])
gripper_action = bias + weight * gripper_action
action = np.concatenate([arm_action, gripper_action])
else:
action = arm_action
if (self.pid is None):
self.sim.data.ctrl[:] = action
else:
self.sim.data.ctrl[self.mujoco_robot.dof:] = gripper_action
self.pid.reset()
self.pid.setpoint = arm_action
# gravity compensation
self.sim.data.qfrc_applied[
self._ref_joint_vel_indexes] = self.sim.data.qfrc_bias[
self._ref_joint_vel_indexes]
if self.use_indicator_object:
self.sim.data.qfrc_applied[
self._ref_indicator_vel_low:self.
_ref_indicator_vel_high] = self.sim.data.qfrc_bias[
self._ref_indicator_vel_low:self._ref_indicator_vel_high]
def _set_pid_control(self):
dt = self.model_timestep if self.pid._last_output is not None else 1e-16
current_qvel = self.sim.data.qvel[self._ref_joint_vel_indexes]
self.sim.data.ctrl[:self.mujoco_robot.dof] = self.pid(current_qvel, dt)
def _post_action(self, action):
"""
(Optional) does gripper visualization after actions.
"""
ret = super()._post_action(action)
self._gripper_visualization()
return ret
def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
Important keys:
robot-state: contains robot-centric information.
"""
di = super()._get_observation()
# proprioceptive features
di["joint_pos"] = np.array(
[self.sim.data.qpos[x] for x in self._ref_joint_pos_indexes])
di["joint_vel"] = np.array(
[self.sim.data.qvel[x] for x in self._ref_joint_vel_indexes])
robot_states = [
np.sin(di["joint_pos"]),
np.cos(di["joint_pos"]),
di["joint_vel"],
]
if self.has_gripper:
di["gripper_qpos"] = np.array([
self.sim.data.qpos[x]
for x in self._ref_gripper_joint_pos_indexes
])
di["gripper_qvel"] = np.array([
self.sim.data.qvel[x]
for x in self._ref_gripper_joint_vel_indexes
])
di["eef_pos"] = np.array(self.sim.data.site_xpos[self.eef_site_id])
di["eef_quat"] = T.convert_quat(
self.sim.data.get_body_xquat("right_hand"), to="xyzw")
# add in gripper information
if (self.gripper_type == "PushingGripper"):
robot_states.extend([di["eef_pos"], di["eef_quat"]])
else:
robot_states.extend(
[di["gripper_qpos"], di["eef_pos"], di["eef_quat"]])
di["robot-state"] = np.concatenate(robot_states)
return di
@property
def control_range(self):
return self.sim.model.actuator_ctrlrange
@property
def action_spec(self):
"""
Action lower/upper limits per dimension.
"""
if self.normalised_actions:
low = np.ones(self.dof) * -1.
high = np.ones(self.dof) * 1.
return low, high
else:
ctrl_range = self.control_range
return ctrl_range[:, 0], ctrl_range[:, 1]
@property
def dof(self):
"""
Returns the DoF of the robot (with grippers).
"""
dof = self.mujoco_robot.dof
if self.has_gripper:
dof += self.gripper.dof
return dof
def pose_in_base(self, pose_in_world):
"""
A helper function that takes in a pose in world frame and returns that pose in the
the base frame.
"""
base_pos_in_world = self.sim.data.get_body_xpos("base")
base_rot_in_world = self.sim.data.get_body_xmat("base").reshape((3, 3))
base_pose_in_world = T.make_pose(base_pos_in_world, base_rot_in_world)
world_pose_in_base = T.pose_inv(base_pose_in_world)
pose_in_base = T.pose_in_A_to_pose_in_B(pose_in_world,
world_pose_in_base)
return pose_in_base
def pose_in_base_from_name(self, name):
"""
A helper function that takes in a named data field and returns the pose
of that object in the base frame.
"""
pos_in_world = self.sim.data.get_body_xpos(name)
rot_in_world = self.sim.data.get_body_xmat(name).reshape((3, 3))
pose_in_world = T.make_pose(pos_in_world, rot_in_world)
base_pos_in_world = self.sim.data.get_body_xpos("base")
base_rot_in_world = self.sim.data.get_body_xmat("base").reshape((3, 3))
base_pose_in_world = T.make_pose(base_pos_in_world, base_rot_in_world)
world_pose_in_base = T.pose_inv(base_pose_in_world)
pose_in_base = T.pose_in_A_to_pose_in_B(pose_in_world,
world_pose_in_base)
return pose_in_base
def set_robot_joint_positions(self, jpos):
"""
Helper method to force robot joint positions to the passed values.
"""
self.sim.data.qpos[self._ref_joint_pos_indexes] = jpos
self.sim.forward()
@property
def _right_hand_pose(self):
"""
Returns eef pose in base frame of robot.
"""
return self.pose_in_base_from_name("right_hand")
@property
def _right_hand_quat(self):
"""
Returns eef quaternion in base frame of robot.
"""
return T.mat2quat(self._right_hand_orn)
@property
def _right_hand_total_velocity(self):
"""
Returns the total eef velocity (linear + angular) in the base frame
as a numpy array of shape (6,)
"""
# Use jacobian to translate joint velocities to end effector velocities.
Jp = self.sim.data.get_body_jacp("right_hand").reshape((3, -1))
Jp_joint = Jp[:, self._ref_joint_vel_indexes]
Jr = self.sim.data.get_body_jacr("right_hand").reshape((3, -1))
Jr_joint = Jr[:, self._ref_joint_vel_indexes]
eef_lin_vel = Jp_joint.dot(self._joint_velocities)
eef_rot_vel = Jr_joint.dot(self._joint_velocities)
return np.concatenate([eef_lin_vel, eef_rot_vel])
@property
def _right_hand_pos(self):
"""
Returns position of eef in base frame of robot.
"""
eef_pose_in_base = self._right_hand_pose
return eef_pose_in_base[:3, 3]
@property
def _right_hand_orn(self):
"""
Returns orientation of eef in base frame of robot as a rotation matrix.
"""
eef_pose_in_base = self._right_hand_pose
return eef_pose_in_base[:3, :3]
@property
def _right_hand_vel(self):
"""
Returns velocity of eef in base frame of robot.
"""
return self._right_hand_total_velocity[:3]
@property
def _right_hand_ang_vel(self):
"""
Returns angular velocity of eef in base frame of robot.
"""
return self._right_hand_total_velocity[3:]
@property
def _joint_positions(self):
"""
Returns a numpy array of joint positions.
Sawyer robots have 7 joints and positions are in rotation angles.
"""
return self.sim.data.qpos[self._ref_joint_pos_indexes]
@property
def joint_positions(self):
"""
Returns a numpy array of joint positions.
Sawyer robots have 7 joints and positions are in rotation angles.
"""
return copy.deepcopy(self.sim.data.qpos[self._ref_joint_pos_indexes])
@property
def joint_velocities(self):
"""
Returns a numpy array of joint velocities.
Sawyer robots have 7 joints and velocities are angular velocities.
"""
return copy.deepcopy(self.sim.data.qvel[self._ref_joint_vel_indexes])
@property
def _joint_velocities(self):
"""
Returns a numpy array of joint velocities.
Sawyer robots have 7 joints and velocities are angular velocities.
"""
return self.sim.data.qvel[self._ref_joint_vel_indexes]
@property
def _joint_velocities_dict(self):
"""
Returns a numpy array of joint velocities.
Sawyer robots have 7 joints and velocities are angular velocities.
"""
return {
'right_j0': self.sim.data.get_joint_qvel('right_j0'),
'right_j1': self.sim.data.get_joint_qvel('right_j1'),
'right_j2': self.sim.data.get_joint_qvel('right_j2'),
'right_j3': self.sim.data.get_joint_qvel('right_j3'),
'right_j4': self.sim.data.get_joint_qvel('right_j4'),
'right_j5': self.sim.data.get_joint_qvel('right_j5'),
'right_j6': self.sim.data.get_joint_qvel('right_j6'),
}
@property
def _joint_pos_dict(self):
"""
Returns a numpy array of joint velocities.
Sawyer robots have 7 joints and velocities are angular velocities.
"""
return {
'right_j0': self.sim.data.get_joint_qpos('right_j0'),
'right_j1': self.sim.data.get_joint_qpos('right_j1'),
'right_j2': self.sim.data.get_joint_qpos('right_j2'),
'right_j3': self.sim.data.get_joint_qpos('right_j3'),
'right_j4': self.sim.data.get_joint_qpos('right_j4'),
'right_j5': self.sim.data.get_joint_qpos('right_j5'),
'right_j6': self.sim.data.get_joint_qpos('right_j6'),
}
@property
def _joint_ranges(self):
"""
Returns a numpy array of joint ranges.
Sawyer robots have 7 joints with max ranges in radiants.
"""
return self.sim.model.jnt_range[self._ref_joint_ids]
def _gripper_visualization(self):
"""
Do any needed visualization here.
"""
# By default, don't do any coloring.
self.sim.model.site_rgba[self.eef_site_id] = [0., 0., 0., 0.]
def _check_contact(self):
"""
Returns True if the gripper is in contact with another object.
"""
return False
class TrajController:
def __init__(self):
self.planning_cmd_sub = message_filters.Subscriber('/planning/pos_cmd',PositionCommand)
self.vins_imu_sub = message_filters.Subscriber('/vins_estimator/imu_propagate',Odometry)
self.time_syn = message_filters.ApproximateTimeSynchronizer([self.planning_cmd_sub,self.vins_imu_sub], 10, 0.1, allow_headerless=True)
self.time_syn.registerCallback(self.call_back)
self.pos_cmd_pose_pub = rospy.Publisher('/planning/pos_cmd_pose',Odometry,queue_size=10)
self.laikago_pose_pub = rospy.Publisher('/laikago_tracker/pose',Odometry,queue_size=10)
self.cmd_handle = laikago_command_handle()
self.safe_distance = rospy.get_param('safe_distance') # m
self.replan_distance = rospy.get_param('replan_distance') # m
self.pid_x = PID(1.5, 0.0, 0.01, setpoint=0)
self.pid_y = PID(1.5, 0.0, 0.023, setpoint=0)
self.pid_yaw = PID(1.5, 0.0, 0.01, setpoint=0)
self.pid_x.output_limits = (0,1)
self.pid_y.output_limits = (-1,1)
self.pid_yaw.output_limits = (-1,1)
self.reach_goal_first_time = rospy.Time.now()
def reset_pid(self):
self.pid_x.reset()
self.pid_y.reset()
self.pid_yaw.reset()
def call_back(self,pos_cmd, vins_imu):
enable = rospy.get_param('enable_replan')
# enable = True
if enable:
# debug
pos_cmd_pos = Odometry()
pos_cmd_pos.pose.pose.position.x = pos_cmd.position.x
pos_cmd_pos.pose.pose.position.y = pos_cmd.position.y
q = transformations.quaternion_from_euler(0, 0, pos_cmd.yaw)
pos_cmd_pos.pose.pose.orientation.x = q[0]
pos_cmd_pos.pose.pose.orientation.y = q[1]
pos_cmd_pos.pose.pose.orientation.z = q[2]
pos_cmd_pos.pose.pose.orientation.w = q[3]
pos_cmd_pos.header = pos_cmd.header
self.pos_cmd_pose_pub.publish(pos_cmd_pos)
vins_imu.pose.pose.position.x = vins_imu.pose.pose.position.x - 0.2
self.laikago_pose_pub.publish(vins_imu)
# if pos_cmd.velocity.x == 0.0 and pos_cmd.velocity.y == 0.0:
# if rospy.Time.now().to_nsec() - self.reach_goal_first_time.to_nsec() > 500000000:
# rospy.set_param('enable_replan', False)
# # rospy.set_param('enable_seek_target', True)
# else:
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.forwardSpeed = self.pid_x(vins_imu.pose.pose.position.x - pos_cmd.position.x)
self.cmd_handle.cmd.sideSpeed = self.pid_y(vins_imu.pose.pose.position.y - pos_cmd.position.y)
orientation = vins_imu.pose.pose.orientation
(roll, pitch, yaw) = transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w])
# (roll, pitch, yaw) = transformations.euler_from_quaternion([
# pos_cmd.position.x - vins_imu.pose.pose.position.x,
# pos_cmd.position.y - vins_imu.pose.pose.position.y,
# 0, orientation.w])
self.cmd_handle.cmd.rotateSpeed = self.pid_yaw(yaw - pos_cmd.yaw)
self.cmd_handle.send()
self.reach_goal_first_time = rospy.Time.now()
else:
self.reset_pid()
class ManualNavigator:
def __init__(self, max_angle=45, max_setpoint_angle=12.5, scaling_factor=1000, max_collinear_offset=120000):
self.MAX_ANGLE = math.radians(max_angle)
self.MAX_SETPOINT_ANGLE = math.radians(max_setpoint_angle)
self.MAX_COLLINEAR_OFFSET = max_collinear_offset
self.MAX_ROTATIONAL_OFFSET = 100000
self.SCALING_FACTOR = scaling_factor
#self.pid = pid_class.PID()
self.pid = PID(400, 10, 0, setpoint=0)
#self.pid.update_constants()
self.pid_output = 0
self.imu = mpu.Sensors(1, 0x68)
self.remote = remote.RemoteController()
self.update_user_angle()
self.odrv = drive.OdriveController()
self.setup_odrive()
self.angle = self.dt = 0 # Values set in update_pid
self.running = False
self.sample_time = 0.001
self.update_pid()
def update_constants(self):
with open("PID.txt", 'r') as infile:
kP, kI, kD, max_I = (float(infile.readline()) for i in range(4))
if kP != self.pid.Kp or kI != self.pid.Ki or kD != self.pid.Kd:
print("New constants fixed. resetting I-val.")
#self.I = 0
self.pid.Kp = kP
self.pid.Ki = kI
self.pid.Kd = kD
#self.max_I_val = max_I
def start(self):
self.angle = self.imu.reset_angle()
self.pid.reset()
self.running = True
def stop(self):
self.running = False
self.odrv.set_collinear_offsets(0)
self.odrv.set_all_motors_speed(0)
def cleanup(self):
self.odrv.set_all_motors_speed(0)
self.odrv.set_axis_state(odrive.enums.AXIS_STATE_IDLE)
self.imu.bus.close()
def setup_odrive(self):
print("Starting calibration...")
self.odrv.calibrate()
print("Calibration finished.")
print("Setting axis state...")
self.odrv.set_axis_state(odrive.enums.AXIS_STATE_CLOSED_LOOP_CONTROL)
print("Setting control mode...")
self.odrv.set_control_mode(odrive.enums.CTRL_MODE_VELOCITY_CONTROL)
print("Done, please standby...")
time.sleep(1)
def update_user_angle(self):
main_axis_input = self.remote.get_ly_axis()
angle = main_axis_input * self.MAX_SETPOINT_ANGLE
self.pid.setpoint = angle
#self.pid.set_setpoint(angle)
def update_collinear_offset(self):
collinear_input = self.remote.get_lx_axis()
collinear_offset = collinear_input * self.MAX_COLLINEAR_OFFSET
self.odrv.set_collinear_offsets(collinear_offset)
def update_rotational_offset(self):
rotational_input = self.remote.get_rx_axis()
rotational_offset = rotational_input * self.MAX_ROTATIONAL_OFFSET
self.odrv.set_rotational_offsets(rotational_offset)
def update_pid(self):
self.angle, self.dt = self.imu.get_angle()
#self.pid.set_process_variable(self.angle, self.dt)
#self.pid_output = self.pid.get_control_variable()
self.pid_output = self.pid(self.angle)
def update_odrive_output(self):
velocity = self.pid_output * self.SCALING_FACTOR
self.odrv.set_all_motors_speed(velocity)
def main_task(self):
""" Returns: true if ok, false if not"""
self.update_user_angle()
self.update_collinear_offset()
self.update_rotational_offset()
self.update_pid()
self.update_odrive_output()
if self.fallen_over:
self.stop()
return not self.fallen_over
def print_telemetry(self):
print(f"Setpoint: {math.degrees(self.pid.setpoint)}")
print(f"Angle: {math.degrees(self.angle)}\tOutput: {self.pid_output}")
@property
def fallen_over(self):
if abs(self.imu.angle) > self.MAX_ANGLE:
print("has fallen over, ", math.degrees(self.imu.angle), "\t", math.degrees(self.MAX_ANGLE))
return True
return False
class BottyMcBotFace(object):
def __init__(self, serial_device):
self.serial_device = serial_device
self.yaw_target = 0.
self.pitch_target = 0.
self.yaw_vel = 0.
self.pitch_vel = 0.
self.trigger_start = 0
self.is_parked = False
self.configure_feather()
self.set_pid_tuning(cfg.track_kp, cfg.track_ki, cfg.track_kd)
""" Motion stuff """
def configure_feather(self):
command_string = 'C0'
for k, v in cfg.Feather_Parameter_Chars.items():
command_string += ' {}{}'.format(k, v)
self.serial_device.command(command_string)
def home(self):
print('TODO implement this plz')
def zero(self):
self.serial_device.command('G92 X0 Y0')
def enable(self):
self.serial_device.command('M17')
def disable(self):
self.serial_device.command('M84')
def trigger(self,
min_pwm=cfg.trigger_min_pwm,
max_pwm=cfg.trigger_max_pwm,
time_held_s=cfg.trigger_hold_s,
force_off=False):
""" Call this function continuously and do other stuff while it runs, will return true when it is done """
if self.trigger_start == 0 and force_off is False:
self.trigger_start = time.time()
self.serial_device.command('c1 a{}'.format(max_pwm))
elif ((time.time() - self.trigger_start) >
time_held_s) or force_off is True:
self.trigger_start = 0
self.serial_device.command('c1 a{}'.format(min_pwm))
return True
return False
def set_pid_tuning(self, kp, ki, kd):
self.pitch_pid = PID(kp / 1000000, ki / 1000000, kd / 1000000)
# self.pitch_pid.output_limits = (-0.015, 0.015)
self.yaw_pid = PID(kp / 1000000, ki / 1000000, kd / 1000000)
# self.yaw_pid.output_limits = (-0.015, 0.015)
def reset_pid(self):
self.pitch_pid.reset()
self.yaw_pid.reset()
def absolute_move(self, yaw_rads, pitch_rads, velocity_mmps=None):
""" I move da motorz """
# Filter commands if exceeding limits
if yaw_rads > cfg.yaw_travel_rads:
self.yaw_target = cfg.yaw_travel_rads
elif yaw_rads < 0:
self.yaw_target = 0
else:
self.yaw_target = yaw_rads
if pitch_rads > cfg.pitch_travel_rads:
self.pitch_target = cfg.pitch_travel_rads
elif pitch_rads < 0:
self.pitch_target = 0
else:
self.pitch_target = pitch_rads
# Send gcode
command = 'G0 X{} Y{}'.format(self.yaw_target, self.pitch_target)
if velocity_mmps is not None:
command += ' F{}'.format(velocity_mmps * 60)
self.serial_device.command(command)
def relative_move(self, yaw_rads=0, pitch_rads=0, velocity_mmps=None):
self.yaw_target += yaw_rads
self.pitch_target += pitch_rads
self.absolute_move(self.yaw_target, self.pitch_target, velocity_mmps)
def update_target(self, pitch_pixel_err, yaw_pixel_err, mult=1.0):
""" Takes in raw pixel errors and determines and sends motor commands """
# First calculate motor leads
self.get_velocities()
pitch_pixel_err += self.pitch_vel * cfg.lead_ahead_constant
yaw_pixel_err += self.yaw_vel * cfg.lead_ahead_constant
# Then run through pid with adjusted pixel targets
pitch_move_rads = self.pitch_pid(pitch_pixel_err) * mult
yaw_move_rads = self.yaw_pid(yaw_pixel_err) * mult
# Then command motors
self.relative_move(yaw_move_rads, pitch_move_rads)
if cfg.DEBUG_MODE:
print('PITCH: {}\tcomponents: {}\tlead: {}'.format(
pitch_move_rads, self.pitch_pid.components,
self.pitch_vel * cfg.lead_ahead_constant))
print('YAW: {}\tcomponents: {}\tlead: {}'.format(
yaw_move_rads, self.yaw_pid.components,
self.yaw_vel * cfg.lead_ahead_constant))
return pitch_move_rads, yaw_move_rads
def get_velocities(self):
""" Updates pitch and yaw motor velocities """
ret = self.serial_device.command('C2')
(self.yaw_vel, self.pitch_vel) = (float(k) for k in ret.split(','))
def send_gcode(self, filename):
with open(os.path.join(cfg.gcode_folder, filename)) as f:
while (True):
line = f.readline().strip('\n')
if not line:
break
if line[0] == ';':
# Comments start with semicolon
continue
self.serial_device.command(line)
@property
def is_homed(self):
raise Exception('NOT IMPLEMENTED')
@property
def xpos_mm(self):
ret = self.serial_device.command('M114')
return float(ret.split(',')[0])
@property
def ypos_mm(self):
ret = self.serial_device.command('M114')
return float(ret.split(',')[1])
class ArduinoPCR:
"""Classe com protocolos para comunicação serial."""
def __init__(self, baudrate, timeout=1, experiment: ExperimentPCR = None):
self.timeout = timeout
self.baudrate = baudrate
self.experiment: ExperimentPCR = experiment
self.cooling_experiment = ExperimentPCR('Resfriamento', 1, 25,
StepPCR('1',
std.COOLING_TEMP_C,
5))
self.pid = PID(Kp=std.KP,
Ki=std.KI,
Kd=std.KD,
output_limits=(-255, 255), sample_time=0)
# print(self.pid.tunings)
# Conferir com o nome no Gerenciador de dispositivos do windows
# caso esteja usando um arduino diferente.
self.device_type = ''
self.port_connected = None
self.serial_device: serial.Serial = None
self.is_connected = False
self.waiting_update = False
self.monitor_thread = None
self.is_running = False
self.is_waiting = True
self.current_sample_temperature = 0
self.current_lid_temperature = 0
self.current_step = ''
self.current_step_temp = 0
self.current_cycle = 0
self.elapsed_time = 0
self.is_cooling = False
self.reading = ''
self.initialize_connection()
def run_experiment(self):
sleep(1)
started_time = time()
self.elapsed_time = 0
for step in self.experiment.steps:
self.elapsed_time += int(step.duration)
self.elapsed_time *= self.experiment.n_cycles
for i in range(int(self.experiment.n_cycles)):
self.current_cycle = i + 1
for step in self.experiment.steps:
self.pid.reset()
started_step_time = time()
self.current_step = step.name
self.current_step_temp = step.temperature
set_point = int(step.temperature)
duration = int(step.duration)
self.pid.setpoint = set_point
current_time = time()
while current_time - started_step_time <= duration:
if self.is_waiting:
if not self.is_running:
# print('Experiment Cancelled')
messagebox.showinfo('Cetus PCR',
'O experimento foi '
'cancelado.')
self.serial_device.write(b'<peltier 0 0>')
self.is_waiting = False
return
output = self.pid(self.current_sample_temperature)
print(f'output= {output}')
# print(self.current_sample_temperature)
if output >= 0:
new_str = f'<peltier 0 {int(output)}>'
elif output < 0:
new_str = f'<peltier 1 {abs(int(output))}>'
self.serial_device.write(b'%a\r\n' % new_str)
self.is_waiting = False
self.elapsed_time = int(time() - started_time)
if not set_point - std.TOLERANCE < \
self.current_sample_temperature < \
set_point + std.TOLERANCE:
# Delay para atingir a temperatura desejada
difference = time() - current_time
duration += difference
# print(duration)
experiment_data_x.append(current_time - started_time)
experiment_data_y.append(self.current_sample_temperature)
experiment_data_setpoint.append(self.pid.setpoint)
current_time = time()
sleep(0.1)
self.is_cooling = False
print(f'Finish time: {time() - started_time}')
file_path = f'{self.experiment.name} - {datetime.now():%d%m%y%H%M%S}'
with open(f'experiment logs/{file_path}.csv', 'w') as outfile:
outfile.write('X,Y,Set Point\n')
for x, y, sp in zip(experiment_data_x, experiment_data_y,
experiment_data_setpoint):
outfile.write(f'{x},{y},{sp}\n')
messagebox.showinfo('Cetus PCR', f'"{self.experiment.name}" '
f'concluído.')
def serial_monitor(self):
"""Função para monitoramento da porta serial do Arduino.
Todas as informações provenientes da porta serial são exibidas
no prompt padrão do Python.
Determinadas informações também são guardadas em variáveis para
serem posteriormente exibidas para o usuário.
Esse processo deve ser rodado em outra thread para evitar a parada
do mainloop da janela principal.
"""
while self.is_connected:
try:
self.reading = self.serial_device.readline().decode()
self.reading = self.reading.strip('\r\n')
if 'tempSample' in self.reading:
self.current_sample_temperature = \
float(self.reading.split()[1])
# print(self.current_sample_temperature)
if 'tempLid' in self.reading:
self.current_lid_temperature = \
float(self.reading.split()[1])
if 'Cooling finished' in self.reading:
messagebox.showinfo('Cetus PCR', 'Rotina de resfriamento '
'concluída.')
elif self.reading == 'nextpls':
self.is_waiting = True
if 'Heat' in self.reading or 'Cooling' in self.reading:
print(f'(SM) {repr(self.reading)}')
# if not self.is_running:
# self.serial_device.write(b'<peltier 0 0>')
# if self.reading != '':
# print(f'(SM) {repr(self.reading)}')
except serial.SerialException:
messagebox.showerror('Dispositivo desconectado',
'Ocorreu um erro ao se comunicar com '
'o CetusPCR. Verifique a conexão e '
'reinicie o aplicativo.')
self.is_connected = False
self.waiting_update = True
self.is_running = False
std.hover_text = 'Cetus PCR desconectado.'
return # Return para encerrar a thread
def initialize_connection(self):
try:
ports = list_ports.comports()
if not list_ports.comports(): # Se não há nada conectado
raise serial.SerialException
for port in ports:
if self.device_type in port.description:
self.serial_device = serial.Serial(port.device,
self.baudrate,
timeout=self.timeout)
sleep(2) # Delay para esperar o sinal do arduino
self.reading = self.serial_device.readline()
if self.reading == b'Cetus is ready.\r\n':
self.is_connected = True
self.port_connected = port.device
print('Connection Successfully. '
'Initializing Serial Monitor (SM)')
break
else:
raise serial.SerialException
except serial.SerialException:
self.serial_device = None
self.is_connected = False
print('Connection Failed')
if self.is_connected:
self.monitor_thread = Thread(target=self.serial_monitor)
self.monitor_thread.start()
class GasUptakeDirector(IsDaemon):
_kind = "gas-uptake-director"
def __init__(self, name, config, config_filepath):
self.i = 0
super().__init__(name, config, config_filepath)
self.recording = False
self.temps = collections.deque(maxlen=500)
self.set_temp = 0
self.row = []
#time.sleep(30)
# initialize clients
self._heater_client = gpiozero.DigitalOutputDevice(pin=18) #yaqc.Client(38455)
self._heater_client.value = 0
self._temp_client = yaqc.Client(39001)
self._temp_client.measure(loop=True)
self._pressure_client_a = yaqc.Client(39100)
#self._pressure_client_a.set_state(gain=2, size=16)
self._pressure_client_a.measure(loop=True)
self._pressure_client_b = yaqc.Client(39101)
#self._pressure_client_b.set_state(gain=2, size=16)
self._pressure_client_b.measure(loop=True)
self._pressure_client_c = yaqc.Client(39102)
#self._pressure_client_c.set_state(gain=2, size=16)
self._pressure_client_c.measure(loop=True)
# begin looping
self._pid = PID(Kp=0.2, Ki=0.001, Kd=0.01, setpoint=0, proportional_on_measurement=True)
self._loop.create_task(self._runner())
def _connection_lost(self, peername):
super()._connection_lost(peername)
self.set_temp = 0
def begin_recording(self):
# create file
now = datetime.datetime.now()
fname = "gas-uptake_" + now.strftime("%Y-%m-%d_%H-%M-%S") + ".txt"
self.record_path = data_directory / fname
tab = "\t"
newline = "\n"
with open(self.record_path, "a") as f:
f.write("timestamp:" + tab + "'" + now.isoformat() + "'" + newline)
f.write("gas-uptake version:" + tab + "'" + __version__ + "'" + newline)
f.write("temperature units:" + tab + "'C'" + newline)
f.write("pressure units:" + tab + "'PSI'" + newline)
f.write("column:" + tab + "[")
f.write("'labtime'")
f.write(tab + "'temperature'")
for i in range(12):
f.write(tab + f"'pressure_{i}'")
f.write("]" + newline)
# finish
self.recording = True
return self.record_path.as_posix()
def stop_recording(self):
self.recording = False
def set_temperature(self, temp):
self.set_temp = temp
self._pid.reset()
async def _poll(self):
row = [time.time()]
# temperature
m = self._temp_client.get_measured()
value = m["temperature"]
row.append(value)
# pressure
measured = []
i = 0
self._last_current_readings = dict()
for client in [
self._pressure_client_a,
self._pressure_client_b,
self._pressure_client_c,
]:
m = client.get_measured()
for channel in range(4):
value = m[f"channel_{channel}"]
self._last_current_readings[i] = value
offset = self._state[f"channel_{i}_offset"]
value -= offset
value -= 4
value *= 150 / 20 # mA to PSI
if value < 0:
value = float('nan')
measured.append(value)
row.append(value)
i += 1
# append to data
self.temps.append(row[1])
self.row = row
# write to file
if self.recording:
write_row(self.record_path, row)
# PID
self._pid.setpoint = self.set_temp
duty = self._pid(row[1])
if duty >= 1:
self._heater_client.value = 1
elif duty <= 0:
self._heater_client.value = 0
else:
duty = round(duty, 2)
self._heater_client.blink(on_time=duty, off_time=10, n=1)
async def _runner(self):
while True:
self._loop.create_task(self._poll())
await asyncio.sleep(1)
def get_last_reading(self):
return self.row
def tare_pressure(self, known_value, channel_index):
"""Apply offset channel based on known pressure value.
Parameters
----------
known_value : double
Known pressure, PSI.
channel_index : int
Channel index, 0 through 11.
"""
# convert from PSI to expected mA
value = known_value * 20 / 150
value += 4
# find offset
offset = self._last_current_readings[channel_index] - value
self._state[f"channel_{channel_index}_offset"] = offset
class TrackFsm:
def __init__(self):
self.target_uv_sub = message_filters.Subscriber(
'/laikago_tracker/target_uv', Point)
self.distance_sub = message_filters.Subscriber(
'/laikago_tracker/target_distance', Float32)
self.state_sub = message_filters.Subscriber('/laikago_real/high_state',
HighState)
self.time_syn = message_filters.ApproximateTimeSynchronizer(
[self.target_uv_sub, self.distance_sub, self.state_sub],
10,
0.1,
allow_headerless=True)
self.time_syn.registerCallback(self.FSM_callback)
# state
self.FSM_state = Enum(
'state',
('INIT', 'WAIT_TARGET', 'STAND_STILL_TRACK', 'STAND_ROTATE_TRACK',
'REPLAN', 'SEEK_TARGET', 'BEFORE_SEEK'))
self.state = self.FSM_state.INIT
# contol
self.cmd_handle = laikago_command_handle()
self.RecvHighROS = HighState()
self.P = 0.0005
self.I = 0
self.D = 0.00002
self.pid_yaw = PID(self.P, self.I, self.D, setpoint=320)
self.pid_yaw.output_limits = (-0.1, 0.1)
self.pid_pitch = PID(self.P, self.I, self.D, setpoint=240)
self.pid_pitch.output_limits = (-0.1, 0.1)
self.pid_sideSpeed = PID(0.00002, 0.0, 0.0000002, setpoint=320)
self.pid_sideSpeed.output_limits = (-1.0, 1.0)
self.pid_rotateSpeed = PID(0.00005, 0.0, 0.0, setpoint=320)
self.pid_rotateSpeed.output_limits = (-1.0, 1.0)
self.safe_distance = rospy.get_param('safe_distance') # m
self.replan_distance = rospy.get_param('replan_distance') # m
self.pid_forwardSpeed = PID(1.0,
0.0,
0.0,
setpoint=-self.safe_distance)
self.pid_forwardSpeed.output_limits = (-1.0, 1.0)
self.FSM_state_enter_first_time = rospy.Time.now()
self.seek_curve_x = 0
self.FSM_state_pub = rospy.Publisher('/laikago_tracker/fsm_state',
String,
queue_size=10)
self.cmd_yaw = rospy.Publisher('/laikago_tracker/cmd_yaw',
Float32,
queue_size=10)
self.cmd_pitch = rospy.Publisher('/laikago_tracker/cmd_pitch',
Float32,
queue_size=10)
self.cmd_rotateSpeed = rospy.Publisher(
'/laikago_tracker/cmd_rotateSpeed', Float32, queue_size=10)
# rospy.Timer(rospy.Duration(1), self.FSM_callback)
def reset_pitch(self):
self.cmd_handle.reset_pitch()
self.pid_pitch.reset()
def reset_yaw(self):
self.cmd_handle.reset_yaw()
self.pid_yaw.reset()
def reset_speed(self):
self.cmd_handle.reset_speed()
self.pid_forwardSpeed.reset()
self.pid_sideSpeed.reset()
self.pid_rotateSpeed.reset()
def reset_cmd(self):
# self.reset_pitch()
self.reset_yaw()
self.reset_speed()
def change_FSM_state(self, state):
rospy.loginfo('change to state: %s', state)
self.FSM_state_pub.publish('change to state: %s' % state)
self.FSM_state_enter_first_time = rospy.Time.now()
self.state = self.FSM_state[state]
if state == 'REPLAN':
self.reset_cmd()
rospy.set_param('enable_replan', True)
elif state == 'SEEK_TARGET':
self.seek_curve_x = 0
def FSM_callback(self, target_uv, distance, laikage_Highstate):
state = self.state
change_FSM_state = self.change_FSM_state
reset_yaw = self.reset_yaw
reset_speed = self.reset_speed
reset_cmd = self.reset_cmd
FSM_state = self.FSM_state
distance = distance.data
safe_distance = self.safe_distance
tmp = 0.2
if state == FSM_state.INIT:
reset_cmd()
change_FSM_state('WAIT_TARGET')
elif state == FSM_state.WAIT_TARGET:
if distance == -1:
if rospy.Time.now().to_nsec(
) - self.FSM_state_enter_first_time.to_nsec() > 500000000:
reset_cmd()
elif distance <= safe_distance:
# reset_cmd()
change_FSM_state('STAND_STILL_TRACK')
elif distance > safe_distance:
change_FSM_state('REPLAN')
elif state == FSM_state.STAND_STILL_TRACK:
if distance == -1:
if rospy.Time.now().to_nsec(
) - self.FSM_state_enter_first_time.to_nsec() > 1000000000:
change_FSM_state('BEFORE_SEEK')
elif distance <= safe_distance:
self.FSM_state_enter_first_time = rospy.Time.now()
# make sure the robot has stopped
if laikage_Highstate.forwardSpeed == 0:
self.cmd_handle.cmd.yaw -= self.pid_yaw(target_uv.x)
self.cmd_handle.cmd.pitch -= self.pid_pitch(target_uv.y)
if self.cmd_handle.cmd.yaw < -0.6 or self.cmd_handle.cmd.yaw > 0.6:
self.pid_rotateSpeed.reset()
change_FSM_state('STAND_ROTATE_TRACK')
else:
reset_cmd()
elif distance > safe_distance:
change_FSM_state('REPLAN')
elif state == FSM_state.STAND_ROTATE_TRACK:
if distance == -1 or (320 - 0.05 * 320) < target_uv.x < (
320 + 0.05 * 320):
if rospy.Time.now().to_nsec(
) - self.FSM_state_enter_first_time.to_nsec() > 500000000:
reset_cmd()
change_FSM_state('STAND_STILL_TRACK')
elif distance > safe_distance:
change_FSM_state('REPLAN')
else:
self.FSM_state_enter_first_time = rospy.Time.now()
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.forwardSpeed = 0
self.cmd_handle.cmd.rotateSpeed += self.pid_rotateSpeed(
target_uv.x)
self.cmd_handle.cmd.sideSpeed += self.pid_sideSpeed(
target_uv.x)
elif state == FSM_state.REPLAN:
# enable_seek_target = rospy.get_param('enable_seek_target')
if distance == -1:
if rospy.Time.now().to_nsec(
) - self.FSM_state_enter_first_time.to_nsec() > 60000000000:
rospy.set_param('enable_replan', False)
change_FSM_state('BEFORE_SEEK')
elif distance <= safe_distance:
rospy.set_param('enable_replan', False)
change_FSM_state('STAND_STILL_TRACK')
else:
self.FSM_state_enter_first_time = rospy.Time.now()
return False
elif state == FSM_state.SEEK_TARGET:
if distance == -1:
self.seek_curve_x += 0.02
if 0 <= self.seek_curve_x <= 6:
self.cmd_handle.cmd.yaw = -0.8 * sin(
pi * self.seek_curve_x / 2)
if 3 <= self.seek_curve_x <= 5:
self.cmd_handle.cmd.pitch = -0.8 * cos(
pi * self.seek_curve_x / 2)
if 6 < self.seek_curve_x <= 8:
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.rotateSpeed = 0.5
if 8 < self.seek_curve_x <= 10:
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.rotateSpeed = -0.5
if 10 < self.seek_curve_x <= 12:
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.rotateSpeed = -0.5
if 12 < self.seek_curve_x <= 14:
self.cmd_handle.cmd.mode = 2
self.cmd_handle.cmd.rotateSpeed = 0.5
if self.seek_curve_x > 14:
change_FSM_state('WAIT_TARGET')
elif distance > safe_distance:
change_FSM_state('REPLAN')
elif distance <= safe_distance:
change_FSM_state('STAND_STILL_TRACK')
elif state == FSM_state.BEFORE_SEEK:
if distance == -1:
if abs(self.cmd_handle.cmd.yaw) > 0.01:
self.cmd_handle.cmd.yaw -= 0.1 * self.cmd_handle.cmd.yaw
if abs(self.cmd_handle.cmd.pitch) > 0.01:
self.cmd_handle.cmd.pitch -= 0.1 * self.cmd_handle.cmd.pitch
if abs(self.cmd_handle.cmd.yaw) <= 0.01 and abs(
self.cmd_handle.cmd.pitch) <= 0.01:
change_FSM_state('SEEK_TARGET')
elif distance > safe_distance:
change_FSM_state('REPLAN')
elif distance <= safe_distance:
change_FSM_state('STAND_STILL_TRACK')
else:
print 'Invalid state!'
# debug
self.cmd_yaw.publish(self.cmd_handle.cmd.yaw)
self.cmd_pitch.publish(self.cmd_handle.cmd.pitch)
self.cmd_rotateSpeed.publish(self.cmd_handle.cmd.rotateSpeed)
self.cmd_handle.send()
def program():
pid_x = PID(0.022,
0.0005,
0.001,
setpoint=set_point_x,
output_limits=(-100, 100),
sample_time=None)
pid_y = PID(0.01,
0.0000,
0.0005,
setpoint=set_point_y,
output_limits=(-100, 100),
sample_time=None)
pid_dist = PID(0.007,
0.0001,
0.004,
setpoint=set_point_dist,
output_limits=(-100, 100),
sample_time=None)
mv_y = 0
mv_x = 0
mv_dist = 0
x_flag = False
y_flag = False
dist_flag = False
init_flag = False
buff2 = 'No Accomplished'
wait = 1 if opt.tracking == 'ssd' else 200
_frame, bbox = search_target()
if opt.tracking == 'algo':
tracker = Tracker(algo=opt.tracking, frame=_frame, bbox=bbox)
else:
tracker = Tracker(algo=opt.tracking)
while True:
frame = cv2.undistort(frame_read.frame, cameraMat, distortCoef)
ok, bbox = tracker(frame)
if ok:
cv2.rectangle(frame, (int(bbox[0]), int(bbox[1])),
(int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3])),
(0, 255, 0), 2)
center_x = (bbox[2] / 2) + bbox[0]
center_y = (bbox[3] / 2) + bbox[1]
err_x = abs(center_x - set_point_x)
err_y = abs(center_y - set_point_y)
_, d2 = get_distance(bbox[2], bbox[3])
err_dist = abs(d2 - set_point_dist)
if not init_flag:
if not x_flag:
if (err_x < lambda_x):
x_flag = True
pid_x.reset()
buff2 = 'X Centered'
elif (err_x > lambda_x):
mv_x = pid_x(center_x)
buff = 'Controlling center X %.2f, Current Center X %.2f' % (
mv_x, center_x)
elif not y_flag:
if (err_y < lambda_y):
y_flag = True
pid_y.reset()
buff2 = buff2 + ' Y Centered'
elif (err_y > lambda_y):
mv_y = pid_y(center_y)
buff = 'Controlling center Y %.2f, Current Center Y%.2f' % (
mv_y, center_y)
init_control(x_flag, y_flag, mv_x, mv_y)
if x_flag and y_flag: init_flag = True
else:
if not dist_flag:
if (err_dist < lambda_d):
dist_flag = True
pid_dist.reset()
buff2 = buff2 + ' Dist Approached'
elif (err_dist > lambda_d):
mv_dist = pid_dist(d2)
maintain_dist(mv_dist)
if err_x > lambda_x or err_y > lambda_y:
x_flag = False
y_flag = False
init_flag = False
pid_dist.reset()
buff = 'Controlling Distance %.2f, Current Dist %.2f' % (
mv_dist, d2)
cv2.putText(
frame,
buff,
(20, 20),
cv2.FONT_HERSHEY_SIMPLEX,
1, # font scale
(200, 0, 255),
1)
cv2.putText(
frame,
buff2,
(20, int(h / 2)),
cv2.FONT_HERSHEY_SIMPLEX,
1, # font scale
(255, 255, 0),
2)
cv2.imshow('TELLO', frame)
key = cv2.waitKey(wait)
if key == 27: break
def main():
global drone
global drone_cc
global drone_ud
global drone_fb
global shutdown
drone = tellopy.Tello()
drone.connect()
drone.wait_for_connection(60.0)
drone.start_video()
drone.subscribe(drone.EVENT_FLIGHT_DATA, handler)
pid_cc = PID(0.35,0.2,0.2,setpoint=0,output_limits=(-100,100))
pid_ud = PID(0.3,0.3,0.3,setpoint=0,output_limits=(-80,80))
pid_fb = PID(0.35,0.2,0.3,setpoint=0,output_limits=(-50,50))
video = cv2.VideoWriter('test_out.mp4',-1,1,(320,240))
# drone.subscribe(drone.EVENT_VIDEO_FRAME,handler)
print("Start Running")
with tf.Session() as sess:
model_cfg, model_outputs = posenet.load_model(args.model, sess)
output_stride = model_cfg['output_stride']
try:
# threading.Thread(target=recv_thread).start()
threading.Thread(target=controller_thread).start()
container = av.open(drone.get_video_stream())
frame_count = 0
while not shutdown:
for frame in container.decode(video=0):
frame_count = frame_count + 1
# skip first 300 frames
if frame_count < 300:
continue
if frame_count %4 == 0:
im = numpy.array(frame.to_image())
im = cv2.resize(im, (320,240)) #resize frame
image = cv2.cvtColor(im, cv2.COLOR_RGB2BGR)
#image = cv2.cvtColor(numpy.array(frame.to_image()), cv2.COLOR_RGB2BGR)
input_image, display_image, output_scale = posenet.process_input(
image, scale_factor=args.scale_factor, output_stride=output_stride)
heatmaps_result, offsets_result, displacement_fwd_result, displacement_bwd_result = sess.run(
model_outputs,
feed_dict={'image:0': input_image}
)
pose_scores, keypoint_scores, keypoint_coords = posenet.decode_multi.decode_multiple_poses(
heatmaps_result.squeeze(axis=0),
offsets_result.squeeze(axis=0),
displacement_fwd_result.squeeze(axis=0),
displacement_bwd_result.squeeze(axis=0),
output_stride=output_stride,
max_pose_detections=10,
min_pose_score=0.15)
keypoint_coords *= output_scale
# TODO this isn't particularly fast, use GL for drawing and display someday...
overlay_image = posenet.draw_skel_and_kp(
display_image, pose_scores, keypoint_scores, keypoint_coords,
min_pose_score=0.15, min_part_score=0.1)
centerx = int(overlay_image.shape[1]/2)
centery = int(overlay_image.shape[0]/2)
nosex = int(keypoint_coords[0,0,1])
nosey = int(keypoint_coords[0,0,0])
ctrl_out_cc = 0
ctrl_out_ud = 0
#overlay_image = cv2.putText(overlay_image, str(nosey), (120,50), cv2.FONT_HERSHEY_SIMPLEX , 1, (55,255,45), 2)
errorx = 0
errory = 0
if keypoint_scores[0,0] > .04:
overlay_image = cv2.line(overlay_image, (centerx,centery - 10), (nosex, nosey), (255, 255, 0), 2)
errorx = nosex - centerx
errory = nosey - centery - 10
if abs(errorx) > 20:
ctrl_out_cc = pid_cc(errorx)
drone_cc = ctrl_out_cc
else:
drone_cc = 0
if abs(errory) > 16:
ctrl_out_ud = pid_ud(errory)
drone_ud = ctrl_out_ud
else:
drone_ud = 0
#out_img = cv2.putText(out_img, str(keypoint_scores[ii,kpoint]), (50,50), cv2.FONT_HERSHEY_SIMPLEX , 1, (255,255,45), 2)
else:
#reset pid
drone_cc = 0
drone_ud = 0
pid_cc.reset()
pid_ud.reset()
leftSholy = int(keypoint_coords[0,5,0])
rightSholy = int(keypoint_coords[0,6,0])
leftHipy = int(keypoint_coords[0,11,0])
rightHipy = int(keypoint_coords[0,12,0])
meanHeight = ((rightHipy - rightSholy) + (leftHipy - leftSholy))/2 #technically arbitrary
desiredHeight = 100
ctrl_out_fb = 0
errorFB = 0
if keypoint_scores[0,5] > .04 and keypoint_scores[0,6] > .04 and keypoint_scores[0,11] > .04 and keypoint_scores[0,12] > .04:
errorFB = meanHeight - desiredHeight
#error can be within +/- 15 without caring
if abs(errorFB) > 15:
ctrl_out_fb = pid_cc(errorFB)
drone_fb = ctrl_out_fb
else:
drone_fb = 0
#out_img = cv2.putText(out_img, str(keypoint_scores[ii,kpoint]), (50,50), cv2.FONT_HERSHEY_SIMPLEX , 1, (255,255,45), 2)
else:
#reset pid
drone_fb = 0
pid_fb.reset()
#don't let the hips lie
# if keypoint_scores[0,11] < .04 and keypoint_scores[0,12] < .04:
# drone_fb = -20
# drone_ud = -20
#overlay_image = cv2.putText(overlay_image, str(ctrl_out_fb), (30,110), cv2.FONT_HERSHEY_SIMPLEX , 1, (55,255,45), 2)
# overlay_image = cv2.putText(overlay_image, str(ctrl_out_ud), (30,30), cv2.FONT_HERSHEY_SIMPLEX , 1, (55,255,45), 2)
#overlay_image = cv2.putText(overlay_image, str(errory), (30,70), cv2.FONT_HERSHEY_SIMPLEX , 1, (55,255,45), 2)
cv2.imshow('posenet', overlay_image)
video.write(overlay_image)
#cv2.imshow('Original', image)
#cv2.imshow('Canny', cv2.Canny(image, 100, 200))
cv2.waitKey(1)
except KeyboardInterrupt as e:
print(e)
except Exception as e:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_exception(exc_type, exc_value, exc_traceback)
print(e)
cv2.destroyAllWindows()
video.release()
drone.quit()
exit(1)
