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