diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py new file mode 100644 index 000000000..2a73fb455 --- /dev/null +++ b/examples/mocap/mocap_hl_commander.py @@ -0,0 +1,238 @@ +# -*- coding: utf-8 -*- +# +# ,---------, ____ _ __ +# | ,-^-, | / __ )(_) /_______________ _____ ___ +# | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# | / ,--' | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2023 Bitcraze AB +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, in version 3. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . +""" +Example of how to connect to a motion capture system and feed the position to a +Crazyflie, using the motioncapture library. The motioncapture library supports all major mocap systems and provides +a generalized API regardless of system type. +The script uses the high level commander to upload a trajectory to fly a figure 8. + +Set the uri to the radio settings of the Crazyflie and modify the +mocap setting matching your system. +""" +import time +from threading import Thread + +import motioncapture + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.mem import MemoryElement +from cflib.crazyflie.mem import Poly4D +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger +from cflib.utils import uri_helper + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +# The host name or ip address of the mocap system +host_name = '192.168.5.21' + +# The type of the mocap system +# Valid options are: 'vicon', 'optitrack', 'optitrack_closed_source', 'qualisys', 'nokov', 'vrpn', 'motionanalysis' +mocap_system_type = 'qualisys' + +# The name of the rigid body that represents the Crazyflie +rigid_body_name = 'cf' + +# True: send position and orientation; False: send position only +send_full_pose = True + +# When using full pose, the estimator can be sensitive to noise in the orientation data when yaw is close to +/- 90 +# degrees. If this is a problem, increase orientation_std_dev a bit. The default value in the firmware is 4.5e-3. +orientation_std_dev = 8.0e-3 + +# The trajectory to fly +# See https://github.com/whoenig/uav_trajectories for a tool to generate +# trajectories + +# Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 +figure8 = [ + [1.050000, 0.000000, -0.000000, 0.000000, -0.000000, 0.830443, -0.276140, -0.384219, 0.180493, -0.000000, 0.000000, -0.000000, 0.000000, -1.356107, 0.688430, 0.587426, -0.329106, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.710000, 0.396058, 0.918033, 0.128965, -0.773546, 0.339704, 0.034310, -0.026417, -0.030049, -0.445604, -0.684403, 0.888433, 1.493630, -1.361618, -0.139316, 0.158875, 0.095799, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.620000, 0.922409, 0.405715, -0.582968, -0.092188, -0.114670, 0.101046, 0.075834, -0.037926, -0.291165, 0.967514, 0.421451, -1.086348, 0.545211, 0.030109, -0.050046, -0.068177, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.700000, 0.923174, -0.431533, -0.682975, 0.177173, 0.319468, -0.043852, -0.111269, 0.023166, 0.289869, 0.724722, -0.512011, -0.209623, -0.218710, 0.108797, 0.128756, -0.055461, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.560000, 0.405364, -0.834716, 0.158939, 0.288175, -0.373738, -0.054995, 0.036090, 0.078627, 0.450742, -0.385534, -0.954089, 0.128288, 0.442620, 0.055630, -0.060142, -0.076163, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.560000, 0.001062, -0.646270, -0.012560, -0.324065, 0.125327, 0.119738, 0.034567, -0.063130, 0.001593, -1.031457, 0.015159, 0.820816, -0.152665, -0.130729, -0.045679, 0.080444, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.700000, -0.402804, -0.820508, -0.132914, 0.236278, 0.235164, -0.053551, -0.088687, 0.031253, -0.449354, -0.411507, 0.902946, 0.185335, -0.239125, -0.041696, 0.016857, 0.016709, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.620000, -0.921641, -0.464596, 0.661875, 0.286582, -0.228921, -0.051987, 0.004669, 0.038463, -0.292459, 0.777682, 0.565788, -0.432472, -0.060568, -0.082048, -0.009439, 0.041158, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [0.710000, -0.923935, 0.447832, 0.627381, -0.259808, -0.042325, -0.032258, 0.001420, 0.005294, 0.288570, 0.873350, -0.515586, -0.730207, -0.026023, 0.288755, 0.215678, -0.148061, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa + [1.053185, -0.398611, 0.850510, -0.144007, -0.485368, -0.079781, 0.176330, 0.234482, -0.153567, 0.447039, -0.532729, -0.855023, 0.878509, 0.775168, -0.391051, -0.713519, 0.391628, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000], # noqa +] + + +class MocapWrapper(Thread): + def __init__(self, body_name): + Thread.__init__(self) + + self.body_name = body_name + self.on_pose = None + self._stay_open = True + + self.start() + + def close(self): + self._stay_open = False + + def run(self): + mc = motioncapture.connect(mocap_system_type, {'hostname': host_name}) + while self._stay_open: + mc.waitForNextFrame() + for name, obj in mc.rigidBodies.items(): + if name == self.body_name: + if self.on_pose: + pos = obj.position + self.on_pose([pos[0], pos[1], pos[2], obj.rotation]) + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def send_extpose_quat(cf, x, y, z, quat): + """ + Send the current Crazyflie X, Y, Z position and attitude as a quaternion. + This is going to be forwarded to the Crazyflie's position estimator. + """ + if send_full_pose: + cf.extpos.send_extpose(x, y, z, quat.x, quat.y, quat.z, quat.w) + else: + cf.extpos.send_extpos(x, y, z) + + +def reset_estimator(cf): + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + # time.sleep(1) + wait_for_position_estimator(cf) + + +def adjust_orientation_sensitivity(cf): + cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev) + + +def activate_kalman_estimator(cf): + cf.param.set_value('stabilizer.estimator', '2') + + # Set the std deviation for the quaternion data pushed into the + # kalman filter. The default value seems to be a bit too low. + cf.param.set_value('locSrv.extQuatStdDev', 0.06) + + +def activate_mellinger_controller(cf): + cf.param.set_value('stabilizer.controller', '2') + + +def upload_trajectory(cf, trajectory_id, trajectory): + trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] + trajectory_mem.trajectory = [] + + total_duration = 0 + for row in trajectory: + duration = row[0] + x = Poly4D.Poly(row[1:9]) + y = Poly4D.Poly(row[9:17]) + z = Poly4D.Poly(row[17:25]) + yaw = Poly4D.Poly(row[25:33]) + trajectory_mem.trajectory.append(Poly4D(duration, x, y, z, yaw)) + total_duration += duration + + trajectory_mem.write_data_sync() + cf.high_level_commander.define_trajectory(trajectory_id, 0, len(trajectory_mem.trajectory)) + return total_duration + + +def run_sequence(cf, trajectory_id, duration): + commander = cf.high_level_commander + + commander.takeoff(1.0, 2.0) + time.sleep(3.0) + relative = True + commander.start_trajectory(trajectory_id, 1.0, relative) + time.sleep(duration) + commander.land(0.0, 2.0) + time.sleep(2) + commander.stop() + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + # Connect to the mocap system + mocap_wrapper = MocapWrapper(rigid_body_name) + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + cf = scf.cf + trajectory_id = 1 + + # Set up a callback to handle data from the mocap system + mocap_wrapper.on_pose = lambda pose: send_extpose_quat(cf, pose[0], pose[1], pose[2], pose[3]) + + adjust_orientation_sensitivity(cf) + activate_kalman_estimator(cf) + # activate_mellinger_controller(cf) + duration = upload_trajectory(cf, trajectory_id, figure8) + print('The sequence is {:.1f} seconds long'.format(duration)) + reset_estimator(cf) + run_sequence(cf, trajectory_id, duration) + + mocap_wrapper.close() diff --git a/examples/qualisys/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py similarity index 100% rename from examples/qualisys/qualisys_hl_commander.py rename to examples/mocap/qualisys_hl_commander.py