Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix Qualisys example #359

Merged
merged 2 commits into from
Sep 7, 2022
Merged

Fix Qualisys example #359

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f9d2df3
Setting rotation std dev
krichardsson Sep 6, 2022
012cb75
Use scipy for quaternion
krichardsson Sep 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 13 additions & 9 deletions examples/qualisys/qualisys_hl_commander.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
from threading import Thread

import qtm
from scipy.spatial.transform import Rotation

import cflib.crtp
from cflib.crazyflie import Crazyflie
Expand All @@ -51,7 +52,11 @@
rigid_body_name = 'cf'

# True: send position and orientation; False: send position only
send_full_pose = False
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
Expand Down Expand Up @@ -231,16 +236,10 @@ def send_extpose_rot_matrix(cf, x, y, z, rot):
rotaton matrix. This is going to be forwarded to the Crazyflie's
position estimator.
"""
qw = _sqrt(1 + rot[0][0] + rot[1][1] + rot[2][2]) / 2
qx = _sqrt(1 + rot[0][0] - rot[1][1] - rot[2][2]) / 2
qy = _sqrt(1 - rot[0][0] + rot[1][1] - rot[2][2]) / 2
qz = _sqrt(1 - rot[0][0] - rot[1][1] + rot[2][2]) / 2

# Normalize the quaternion
ql = math.sqrt(qx ** 2 + qy ** 2 + qz ** 2 + qw ** 2)
quat = Rotation.from_matrix(rot).as_quat()

if send_full_pose:
cf.extpos.send_extpose(x, y, z, qx / ql, qy / ql, qz / ql, qw / ql)
cf.extpos.send_extpose(x, y, z, quat[0], quat[1], quat[2], quat[3])
else:
cf.extpos.send_extpos(x, y, z)

Expand All @@ -254,6 +253,10 @@ def reset_estimator(cf):
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')

Expand Down Expand Up @@ -316,6 +319,7 @@ def run_sequence(cf, trajectory_id, duration):
qtm_wrapper.on_pose = lambda pose: send_extpose_rot_matrix(
cf, pose[0], pose[1], pose[2], pose[3])

adjust_orientation_sensitivity(cf)
activate_kalman_estimator(cf)
activate_high_level_commander(cf)
# activate_mellinger_controller(cf)
Expand Down