def forward_kon_to_lcm(self):
self.read_input()
msg = crazyflie_input_t()
msg.input[0] = (self.sliders.get(0,0)/127.0)*(INPUT_MAX-INPUT_MIN)*.1+INPUT_MIN
msg.input[1] = (self.sliders.get(1,0)/127.0)*(INPUT_MAX-INPUT_MIN)*.1+INPUT_MIN
msg.input[2] = (self.sliders.get(2,0)/127.0)*(INPUT_MAX-INPUT_MIN)+INPUT_MIN
msg.input[3] = (self.sliders.get(3,0)/127.0)*(INPUT_MAX-INPUT_MIN)+INPUT_MIN
msg.offset = (self.sliders.get(4,0)/127.0)*(INPUT_MAX-INPUT_MIN)+INPUT_MIN
msg.type = INPUT_TYPE
if IS_EXTRA_INPUT:
self.lc.publish('crazyflie_extra_input', msg.encode())
else:
self.lc.publish('crazyflie_input', msg.encode())
tvlqr_play = self.sliders.get(41)
if not(self._tvlqr_counting) and tvlqr_play==127:
self._hover = False
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
msg = crazyflie_state_estimator_commands_t()
msg.tvlqr_counting = True
self.lc.publish('crazyflie_state_estimator_commands', msg.encode())
self._tvlqr_counting = True
tvlqr_stop = self.sliders.get(45)
if self._tvlqr_counting and tvlqr_stop==127:
self._hover = True
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
msg = crazyflie_state_estimator_commands_t()
msg.tvlqr_counting = False
self.lc.publish('crazyflie_state_estimator_commands', msg.encode())
self._tvlqr_counting = False
reset_stop_all = self.sliders.get(43)
if reset_stop_all==127:
msg = crazyflie_controller_commands_t()
msg.is_running = True
self.lc.publish('crazyflie_controller_commands', msg.encode())
self._is_running = True
stop_all = self.sliders.get(45)
if stop_all==127:
msg = crazyflie_controller_commands_t()
msg.is_running = False
self.lc.publish('crazyflie_controller_commands', msg.encode())
self._is_running = False
hover = self.sliders.get(60)
if hover==127 and (time.time()-self._last_hover_update)>.5:
self._hover = not(self._hover)
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
def get_control_input(self, xhat=None):
if self._pos_control:
if not self._is_running:
return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
if self._listen_to_lcm or not xhat:
control_input = list(self._latest_control_input)
else:
control_input = np.dot(
self._K.get('postilqr'),
np.array(xhat).transpose() -
(self._xhat_desired + np.array([
self._extra_control_input[0], self.
_extra_control_input[1], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]))).tolist()
control_input[6] += NOMINAL_W2 - 15.0
if self._reset_xhat_desired:
if self._go_to_start:
self._xhat_desired = np.array(XHAT_START).transpose()
self._go_to_start = False
else:
self._xhat_desired = np.array(
[xhat[0], xhat[1], xhat[2], 0, 0, 0, 0, 0, 0, 0, 0,
0]).transpose()
self._reset_xhat_desired = False
if self._hover:
control_input = np.dot(
self._K.get('postilqr'),
np.array(xhat).transpose() - self._xhat_desired).tolist()
control_input[6] += NOMINAL_W2 - 15.0
if self._listen_to_extra_input:
control_input[6] += self._extra_control_input[4]
if self._publish_to_lcm:
msg = crazyflie_positioninput_t()
msg.input = control_input
self._control_input_lc.publish('crazyflie_input', msg.encode())
return control_input
if not self._is_running:
return [
0.0, 0.0, 0.0, 0.0, 0.0,
MODES.get(self._control_input_type, 1)
]
if self._listen_to_lcm or not xhat:
control_input = list(self._latest_control_input)
else:
thrust_input = np.dot(
self._K.get(self._control_input_type),
np.array(xhat).transpose() - self._xhat_desired).tolist()
control_input = thrust_input + [
0.0, MODES.get(self._control_input_type, 1)
]
if self._reset_xhat_desired:
if self._go_to_start:
self._xhat_desired = np.array(XHAT_START).transpose()
self._go_to_start = False
else:
self._xhat_desired = np.array(
[xhat[0], xhat[1], xhat[2], 0, 0, 0, 0, 0, 0, 0, 0,
0]).transpose()
self._reset_xhat_desired = False
if self._hover:
xhat_error = np.array(xhat).transpose() - self._xhat_desired
thrust_input = np.dot(self._K.get('tilqr'), xhat_error).tolist()
thrust_input[0] += NOMINAL_W2 - 15
thrust_input[1] += NOMINAL_W2 - 15
thrust_input[2] += NOMINAL_W2 - 15
thrust_input[3] += NOMINAL_W2 - 15
control_input = thrust_input + [0.0, MODES.get('omegasqu', 2)]
if self._listen_to_extra_input:
assert control_input[5] == self._extra_control_input[
5], 'The extra input is not of the right type'
control_input[0] += self._extra_control_input[0]
control_input[1] += self._extra_control_input[1]
control_input[2] += self._extra_control_input[2]
control_input[3] += self._extra_control_input[3]
control_input[4] += self._extra_control_input[4]
if self._publish_to_lcm:
msg = crazyflie_input_t()
msg.input = control_input[0:4]
msg.offset = control_input[4]
msg.type = self._control_input_type
self._control_input_lc.publish('crazyflie_input', msg.encode())
return control_input
def get_control_input(self, xhat=None):
if self._pos_control:
if not self._is_running:
return [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
if self._listen_to_lcm or not xhat:
control_input = list(self._latest_control_input)
else:
control_input = np.dot(self._K.get('postilqr'),np.array(xhat).transpose()-(self._xhat_desired+np.array([self._extra_control_input[0], self._extra_control_input[1], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))).tolist()
control_input[6] += NOMINAL_W2 - 15.0
if self._reset_xhat_desired:
if self._go_to_start:
self._xhat_desired = np.array(XHAT_START).transpose()
self._go_to_start = False
else:
self._xhat_desired = np.array([xhat[0], xhat[1], xhat[2], 0, 0, 0, 0, 0, 0, 0, 0, 0]).transpose()
self._reset_xhat_desired = False
if self._hover:
control_input = np.dot(self._K.get('postilqr'),np.array(xhat).transpose()-self._xhat_desired).tolist()
control_input[6] += NOMINAL_W2 - 15.0
if self._listen_to_extra_input:
control_input[6] += self._extra_control_input[4]
if self._publish_to_lcm:
msg = crazyflie_positioninput_t()
msg.input = control_input
self._control_input_lc.publish('crazyflie_input', msg.encode())
return control_input
if not self._is_running:
return [0.0, 0.0, 0.0, 0.0, 0.0, MODES.get(self._control_input_type,1)]
if self._listen_to_lcm or not xhat:
control_input = list(self._latest_control_input)
else:
thrust_input = np.dot(self._K.get(self._control_input_type),np.array(xhat).transpose()-self._xhat_desired).tolist()
control_input = thrust_input + [0.0, MODES.get(self._control_input_type,1)]
if self._reset_xhat_desired:
if self._go_to_start:
self._xhat_desired = np.array(XHAT_START).transpose()
self._go_to_start = False
else:
self._xhat_desired = np.array([xhat[0], xhat[1], xhat[2], 0, 0, 0, 0, 0, 0, 0, 0, 0]).transpose()
self._reset_xhat_desired = False
if self._hover:
xhat_error = np.array(xhat).transpose()-self._xhat_desired
thrust_input = np.dot(self._K.get('tilqr'),xhat_error).tolist()
thrust_input[0] += NOMINAL_W2 - 15
thrust_input[1] += NOMINAL_W2 - 15
thrust_input[2] += NOMINAL_W2 - 15
thrust_input[3] += NOMINAL_W2 - 15
control_input = thrust_input + [0.0, MODES.get('omegasqu',2)]
if self._listen_to_extra_input:
assert control_input[5] == self._extra_control_input[5], 'The extra input is not of the right type'
control_input[0] += self._extra_control_input[0]
control_input[1] += self._extra_control_input[1]
control_input[2] += self._extra_control_input[2]
control_input[3] += self._extra_control_input[3]
control_input[4] += self._extra_control_input[4]
if self._publish_to_lcm:
msg = crazyflie_input_t()
msg.input = control_input[0:4]
msg.offset = control_input[4]
msg.type = self._control_input_type
self._control_input_lc.publish('crazyflie_input', msg.encode())
return control_input
def forward_kon_to_lcm(self):
self.read_input()
msg = crazyflie_input_t()
msg.input[0] = (self.sliders.get(0, 0) /
127.0) * (INPUT_MAX - INPUT_MIN) * .1 + INPUT_MIN
msg.input[1] = (self.sliders.get(1, 0) /
127.0) * (INPUT_MAX - INPUT_MIN) * .1 + INPUT_MIN
msg.input[2] = (self.sliders.get(2, 0) /
127.0) * (INPUT_MAX - INPUT_MIN) + INPUT_MIN
msg.input[3] = (self.sliders.get(3, 0) /
127.0) * (INPUT_MAX - INPUT_MIN) + INPUT_MIN
msg.offset = (self.sliders.get(4, 0) / 127.0) * (INPUT_MAX -
INPUT_MIN) + INPUT_MIN
msg.type = INPUT_TYPE
if IS_EXTRA_INPUT:
self.lc.publish('crazyflie_extra_input', msg.encode())
else:
self.lc.publish('crazyflie_input', msg.encode())
tvlqr_play = self.sliders.get(41)
if not (self._tvlqr_counting) and tvlqr_play == 127:
self._hover = False
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
msg = crazyflie_state_estimator_commands_t()
msg.tvlqr_counting = True
self.lc.publish('crazyflie_state_estimator_commands', msg.encode())
self._tvlqr_counting = True
tvlqr_stop = self.sliders.get(45)
if self._tvlqr_counting and tvlqr_stop == 127:
self._hover = True
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
msg = crazyflie_state_estimator_commands_t()
msg.tvlqr_counting = False
self.lc.publish('crazyflie_state_estimator_commands', msg.encode())
self._tvlqr_counting = False
reset_stop_all = self.sliders.get(43)
if reset_stop_all == 127:
msg = crazyflie_controller_commands_t()
msg.is_running = True
self.lc.publish('crazyflie_controller_commands', msg.encode())
self._is_running = True
stop_all = self.sliders.get(45)
if stop_all == 127:
msg = crazyflie_controller_commands_t()
msg.is_running = False
self.lc.publish('crazyflie_controller_commands', msg.encode())
self._is_running = False
hover = self.sliders.get(60)
if hover == 127 and (time.time() - self._last_hover_update) > .5:
self._hover = not (self._hover)
msg = crazyflie_hover_commands_t()
msg.hover = self._hover
self.lc.publish('crazyflie_hover_commands', msg.encode())
self._last_hover_update = time.time()
