mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-28 09:58:23 +08:00
[natnet] use scipy rotation module instead of pyquaternion (#3492)
scipy is already part of the dependencies and can be installed from system packages which is not the case for pyquaternion only available with pip
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
904eefb0fe
commit
eabbbe249c
@@ -46,9 +46,9 @@ The edges of the indoor test areas are denoted like as in this diagram:
|
|||||||
far
|
far
|
||||||
+──────────────────────────+
|
+──────────────────────────+
|
||||||
│ │
|
│ │
|
||||||
│ ⊙ ⊙ │
|
│ ⊙ ⊙ │
|
||||||
│ ⊙ + ⊙ │
|
│ ⊙ + ⊙ │
|
||||||
│ ⊙ ⊙ │
|
│ ⊙ ⊙ │
|
||||||
left │ │ right
|
left │ │ right
|
||||||
│ │
|
│ │
|
||||||
│ │
|
│ │
|
||||||
@@ -107,7 +107,7 @@ as long as the --long_edge argument is set correspondingly.
|
|||||||
│ │
|
│ │
|
||||||
│ │
|
│ │
|
||||||
│ y │
|
│ y │
|
||||||
│ ⊙──· x │
|
│ ⊙──· x │
|
||||||
left │ │ / │ right
|
left │ │ / │ right
|
||||||
│ │/ │
|
│ │/ │
|
||||||
│ · │
|
│ · │
|
||||||
@@ -125,7 +125,7 @@ around x:
|
|||||||
│ │
|
│ │
|
||||||
│ y^ │
|
│ y^ │
|
||||||
│ | │
|
│ | │
|
||||||
│ z⊙──►x │
|
│ z⊙──►x │
|
||||||
left │ │ right
|
left │ │ right
|
||||||
│ │
|
│ │
|
||||||
│ │
|
│ │
|
||||||
@@ -147,17 +147,16 @@ rigid-body in motive must be known (that's when the quaternion is initialized to
|
|||||||
|
|
||||||
|
|
||||||
import sys
|
import sys
|
||||||
|
import typing
|
||||||
from os import path, getenv
|
from os import path, getenv
|
||||||
from time import time, sleep
|
from time import time, sleep
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from pyquaternion import Quaternion as Quat
|
from scipy.spatial.transform import Rotation
|
||||||
from collections import deque
|
from collections import deque
|
||||||
import argparse
|
import argparse
|
||||||
|
|
||||||
# import NatNet client
|
# import NatNet client
|
||||||
from NatNetClient import NatNetClient
|
from NatNetClient import NatNetClient
|
||||||
import DataDescriptions
|
|
||||||
import MoCapData
|
|
||||||
|
|
||||||
# if PAPARAZZI_HOME not set, then assume the tree containing this
|
# if PAPARAZZI_HOME not set, then assume the tree containing this
|
||||||
# file is a reasonable substitute
|
# file is a reasonable substitute
|
||||||
@@ -166,32 +165,11 @@ sys.path.append(PPRZ_HOME + "/var/lib/python")
|
|||||||
from pprzlink.ivy import IvyMessagesInterface
|
from pprzlink.ivy import IvyMessagesInterface
|
||||||
from pprzlink.message import PprzMessage
|
from pprzlink.message import PprzMessage
|
||||||
|
|
||||||
# parse args
|
|
||||||
parser = argparse.ArgumentParser(
|
|
||||||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
|
||||||
epilog=ADDITIONAL_HELP,
|
|
||||||
)
|
|
||||||
parser.add_argument('-le', '--long_edge', required=True, dest='long_edge', choices=['left', 'far', 'right', 'near'], help="Side of the test area that the long edge of the calibration tool was pointing at")
|
|
||||||
group = parser.add_mutually_exclusive_group(required=True)
|
|
||||||
group.add_argument('-xs', '--x_side', dest='x_side', choices=['left', 'far', 'right', 'near'], help="Side that the x/east axis of the resulting ENU frame should point to.")
|
|
||||||
group.add_argument('-xa', '--x_angle_from_right', dest='x_angle', type=float, help="Right-hand rotation in degrees of the x/east-axis around the up-axis, where 0.0 means x/east is pointing to the right")
|
|
||||||
parser.add_argument('-up', '--up_axis', dest='up_axis', choices=['y_up', 'z_up'], default='y_up', help="Optitrack Up axis: y_up or z_up.")
|
|
||||||
parser.add_argument('-an', '--ac_nose', required=True, dest='ac_nose', choices=['left', 'far', 'right', 'near'], help="Side of the test area that the nose of the drone was pointing at when definition the rigid body")
|
|
||||||
parser.add_argument('-ac', action='append', nargs=2,
|
|
||||||
metavar=('rigid_id','ac_id'), required=True, help='pair of rigid body and A/C id (multiple possible)')
|
|
||||||
|
|
||||||
parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port")
|
ac_id_type = typing.Any # The type used to identify AC, probably str
|
||||||
parser.add_argument('-s', '--server', dest='server', default="127.0.0.1", help="NatNet server IP address")
|
ac_dicts_type = typing.Tuple[typing.Dict[ac_id_type,int], # Link between user AC id and internal id (an int)
|
||||||
parser.add_argument('-m', '--multicast_addr', dest='multicast', default="239.255.42.99", help="NatNet server multicast address")
|
typing.Dict[int,typing.Optional[int]], # Last timestamp per AC
|
||||||
parser.add_argument('-dp', '--data_port', dest='data_port', type=int, default=1511, help="NatNet server data socket UDP port")
|
typing.Dict[int,deque]] # List of known positions per AC (stored in a queue)
|
||||||
parser.add_argument('-cp', '--command_port', dest='command_port', type=int, default=1510, help="NatNet server command socket UDP port")
|
|
||||||
parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
|
|
||||||
parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="transmit frequency")
|
|
||||||
parser.add_argument('-gr', '--ground_ref', dest='ground_ref', action='store_true', help="also send the GROUND_REF message")
|
|
||||||
parser.add_argument('-vs', '--vel_samples', dest='vel_samples', default=4, type=int, help="amount of samples to compute velocity (should be greater than 2)")
|
|
||||||
parser.add_argument('-rg', '--remote_gps', dest='rgl_msg', action='store_true', help="use the old REMOTE_GPS_LOCAL message")
|
|
||||||
parser.add_argument('-sm', '--small', dest='small_msg', action='store_true', help="enable the EXTERNAL_POSE_SMALL message instead of the full")
|
|
||||||
parser.add_argument('-o', '--old_natnet', dest='old_natnet', action='store_true', help="Change the NatNet version to 2.9")
|
|
||||||
|
|
||||||
def process_args(args):
|
def process_args(args):
|
||||||
if args.ac is None:
|
if args.ac is None:
|
||||||
@@ -211,16 +189,16 @@ def process_args(args):
|
|||||||
# Rotation for Z up if needed
|
# Rotation for Z up if needed
|
||||||
if args.up_axis == 'y_up':
|
if args.up_axis == 'y_up':
|
||||||
# x far, y up, z right -> x far, y left, z up
|
# x far, y up, z right -> x far, y left, z up
|
||||||
q_z_up = Quat(axis=[1., 0., 0.], angle=np.pi/2.)
|
q_z_up = Rotation.from_rotvec(np.pi/2*np.array([1., 0., 0.]))
|
||||||
else:
|
else:
|
||||||
q_z_up = Quat(axis=[1., 0., 0.], angle=0.)
|
q_z_up = Rotation.identity()
|
||||||
|
|
||||||
### axes rotation definitions ###
|
### axes rotation definitions ###
|
||||||
# Ground plane calibration correction
|
# Ground plane calibration correction
|
||||||
ground_plane_correction = {'left': -90., 'far': 180., 'right': 90., 'near': 0.}
|
ground_plane_correction = {'left': -90., 'far': 180., 'right': 90., 'near': 0.}
|
||||||
q_ground_plane = Quat(
|
q_ground_plane = Rotation.from_rotvec(
|
||||||
axis=[0., 0., 1.],
|
np.deg2rad(ground_plane_correction[args.long_edge]) * np.array([0., 0., 1.])
|
||||||
angle=np.deg2rad(ground_plane_correction[args.long_edge]))
|
)
|
||||||
|
|
||||||
# Desired x/East axis orientation
|
# Desired x/East axis orientation
|
||||||
if args.x_side is not None:
|
if args.x_side is not None:
|
||||||
@@ -229,9 +207,8 @@ def process_args(args):
|
|||||||
else:
|
else:
|
||||||
x_angle = -args.x_angle # angle from right side, right-hand rotation
|
x_angle = -args.x_angle # angle from right side, right-hand rotation
|
||||||
|
|
||||||
q_x_correction = Quat(
|
q_x_correction = Rotation.from_rotvec(
|
||||||
axis=[0., 0., 1.],
|
np.deg2rad(x_angle) * np.array([0., 0., 1.])
|
||||||
angle=np.deg2rad(x_angle)
|
|
||||||
)
|
)
|
||||||
|
|
||||||
# Total axis system rotation
|
# Total axis system rotation
|
||||||
@@ -239,15 +216,15 @@ def process_args(args):
|
|||||||
|
|
||||||
# Attitude Quaternion correction
|
# Attitude Quaternion correction
|
||||||
nose_correction = {'left': 90., 'far': 0., 'right': -90., 'near': 180.}
|
nose_correction = {'left': 90., 'far': 0., 'right': -90., 'near': 180.}
|
||||||
q_nose_correction = Quat(
|
q_nose_correction = Rotation.from_rotvec(
|
||||||
axis=[0., 0., 1.],
|
np.deg2rad(nose_correction[args.ac_nose] + x_angle) * np.array([0., 0., 1.])
|
||||||
angle=np.deg2rad(nose_correction[args.ac_nose] + x_angle)
|
|
||||||
)
|
)
|
||||||
|
|
||||||
return id_dict, timestamp, period, track, q_total, q_nose_correction
|
return id_dict, timestamp, period, track, q_total, q_nose_correction
|
||||||
|
|
||||||
# store track function
|
# store track function
|
||||||
def store_track(ac_id, pos, t):
|
def store_track(ac_dicts:ac_dicts_type, ac_id, pos, t):
|
||||||
|
id_dict,_,track = ac_dicts
|
||||||
if ac_id in id_dict.keys():
|
if ac_id in id_dict.keys():
|
||||||
track[ac_id].append((pos, t))
|
track[ac_id].append((pos, t))
|
||||||
if len(track[ac_id]) > args.vel_samples:
|
if len(track[ac_id]) > args.vel_samples:
|
||||||
@@ -255,7 +232,7 @@ def store_track(ac_id, pos, t):
|
|||||||
|
|
||||||
# compute velocity from track
|
# compute velocity from track
|
||||||
# returns zero if not enough samples
|
# returns zero if not enough samples
|
||||||
def compute_velocity(ac_id):
|
def compute_velocity(track, ac_id):
|
||||||
vel = [ 0., 0., 0. ]
|
vel = [ 0., 0., 0. ]
|
||||||
if len(track[ac_id]) >= args.vel_samples:
|
if len(track[ac_id]) >= args.vel_samples:
|
||||||
nb = -1
|
nb = -1
|
||||||
@@ -279,18 +256,19 @@ def compute_velocity(ac_id):
|
|||||||
vel[2] /= nb
|
vel[2] /= nb
|
||||||
return vel
|
return vel
|
||||||
|
|
||||||
def performTransformation( pos, vel, quat ):
|
def performTransformation(q_total, q_nose_correction, pos, vel, quat):
|
||||||
pos = q_total.rotate([pos[0], pos[1], pos[2]])
|
pos = q_total.apply(pos)
|
||||||
vel = q_total.rotate([vel[0], vel[1], vel[2]])
|
vel = q_total.apply(vel)
|
||||||
|
|
||||||
# Rotate the attitude delta to the new frame
|
# Rotate the attitude delta to the new frame
|
||||||
quat = Quat(real=quat[3], imaginary=[quat[0], quat[1], quat[2]])
|
quat = Rotation.from_quat(quat)
|
||||||
quat = (q_total * quat * q_total.inverse) * q_nose_correction
|
quat = (q_total * quat * q_total.inv()) * q_nose_correction
|
||||||
quat = quat.elements[[1, 2, 3, 0]].tolist()
|
quat = quat.as_quat().tolist() # Scalar last
|
||||||
|
|
||||||
return pos, vel, quat
|
return pos, vel, quat
|
||||||
|
|
||||||
def receiveRigidBodyList( rigid_body_data, stamp ):
|
def receiveRigidBodyList(ac_dicts:ac_dicts_type, period:float, rigid_body_data, stamp, q_total, q_nose_correction):
|
||||||
|
id_dict,timestamp,track = ac_dicts
|
||||||
for rigid_body in rigid_body_data.rigid_body_list:
|
for rigid_body in rigid_body_data.rigid_body_list:
|
||||||
if not rigid_body.tracking_valid:
|
if not rigid_body.tracking_valid:
|
||||||
# skip if rigid body is not valid
|
# skip if rigid body is not valid
|
||||||
@@ -299,22 +277,22 @@ def receiveRigidBodyList( rigid_body_data, stamp ):
|
|||||||
i = str(rigid_body.id_num)
|
i = str(rigid_body.id_num)
|
||||||
if i not in id_dict.keys():
|
if i not in id_dict.keys():
|
||||||
continue
|
continue
|
||||||
|
|
||||||
pos = rigid_body.pos
|
pos = rigid_body.pos
|
||||||
quat = rigid_body.rot
|
quat = rigid_body.rot
|
||||||
|
|
||||||
store_track(i, pos, stamp)
|
store_track(ac_dicts, i, pos, stamp)
|
||||||
if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
|
if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
|
||||||
if timestamp[i] is None:
|
if timestamp[i] is None:
|
||||||
timestamp[i] = stamp
|
timestamp[i] = stamp
|
||||||
continue # too early for next message
|
continue # too early for next message
|
||||||
timestamp[i] = stamp
|
timestamp[i] = stamp
|
||||||
|
|
||||||
vel = compute_velocity(i)
|
vel = compute_velocity(track, i)
|
||||||
|
|
||||||
# Rotate position, velocity and attitude according to the quaternions
|
# Rotate position, velocity and attitude according to the quaternions
|
||||||
# found above
|
# found above
|
||||||
pos, vel, quat = performTransformation(pos, vel, quat)
|
pos, vel, quat = performTransformation(q_total, q_nose_correction, pos, vel, quat)
|
||||||
|
|
||||||
# Check which message to send
|
# Check which message to send
|
||||||
if args.rgl_msg:
|
if args.rgl_msg:
|
||||||
@@ -331,7 +309,7 @@ def receiveRigidBodyList( rigid_body_data, stamp ):
|
|||||||
# convert quaternion to psi euler angle
|
# convert quaternion to psi euler angle
|
||||||
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
|
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
|
||||||
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
|
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
|
||||||
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
|
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / np.pi
|
||||||
ivy.send(msg)
|
ivy.send(msg)
|
||||||
elif args.small_msg:
|
elif args.small_msg:
|
||||||
# First check if position fits in message
|
# First check if position fits in message
|
||||||
@@ -372,23 +350,17 @@ def receiveRigidBodyList( rigid_body_data, stamp ):
|
|||||||
gr['timestamp'] = stamp
|
gr['timestamp'] = stamp
|
||||||
ivy.send(gr)
|
ivy.send(gr)
|
||||||
|
|
||||||
run_test_cases = '--test' in sys.argv
|
|
||||||
|
|
||||||
if not run_test_cases:
|
|
||||||
# parse arguments
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
# start ivy interface
|
#################### Entry point ####################
|
||||||
if args.ivy_bus is not None:
|
|
||||||
ivy = IvyMessagesInterface("natnet2ivy", ivy_bus=args.ivy_bus)
|
|
||||||
else:
|
|
||||||
ivy = IvyMessagesInterface("natnet2ivy")
|
|
||||||
|
|
||||||
# start natnet interface
|
def main_loop(args, ivy):
|
||||||
|
id_dict, timestamp, period, track, q_total, q_nose_correction = process_args(args)
|
||||||
|
|
||||||
natnet = NatNetClient()
|
natnet = NatNetClient()
|
||||||
natnet.set_server_address(args.server)
|
natnet.set_server_address(args.server)
|
||||||
natnet.set_client_address('0.0.0.0')
|
natnet.set_client_address('0.0.0.0')
|
||||||
natnet.rigid_body_list_listener = receiveRigidBodyList
|
natnet.rigid_body_list_listener = lambda l,s : receiveRigidBodyList((id_dict,timestamp,track),period,l,s,q_total,q_nose_correction)
|
||||||
|
|
||||||
if args.verbose:
|
if args.verbose:
|
||||||
natnet.set_print_level(1) # print all frames
|
natnet.set_print_level(1) # print all frames
|
||||||
@@ -397,10 +369,8 @@ if not run_test_cases:
|
|||||||
if args.old_natnet:
|
if args.old_natnet:
|
||||||
natnet.set_nat_net_version(2,9)
|
natnet.set_nat_net_version(2,9)
|
||||||
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
|
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
|
||||||
|
|
||||||
try:
|
try:
|
||||||
# Start up the streaming client.
|
|
||||||
# This will run perpetually, and operate on a separate thread.
|
|
||||||
id_dict, timestamp, period, track, q_total, q_nose_correction = process_args(args)
|
|
||||||
is_running = natnet.run()
|
is_running = natnet.run()
|
||||||
if not is_running:
|
if not is_running:
|
||||||
print("Natnet error: Could not start streaming client.")
|
print("Natnet error: Could not start streaming client.")
|
||||||
@@ -423,85 +393,136 @@ if not run_test_cases:
|
|||||||
ivy.stop()
|
ivy.stop()
|
||||||
exit(-1)
|
exit(-1)
|
||||||
|
|
||||||
|
def test_case(args, i, case):
|
||||||
else:
|
if (case['xs'] == 'right') or (case['xs'] == 'far'):
|
||||||
#%% test cases
|
args.x_side = case['xs']
|
||||||
# Idea: position a drone at 90deg yaw to the right, and 15deg nose down.
|
args.x_angle = None
|
||||||
# Then run through all settings and check output. Because it's a
|
else:
|
||||||
# combined rotation, it should show all problems with transformations
|
args.x_angle = case['xs']
|
||||||
#
|
args.x_side = None
|
||||||
# Correcteness of the NatNet pos and NatNet quaternion for each case have
|
|
||||||
# been experimentally verified. The TargetPos and TargetQuat are relatively
|
args.up_axis = case['up_axis']
|
||||||
# easy to generate manually.
|
args.long_edge = case['long_edge']
|
||||||
# Test flights still outstanding for setting xa to a numeric value
|
args.ac_nose = case['ac_nose']
|
||||||
|
|
||||||
args = argparse.Namespace()
|
id_dict, timestamp, period, track, q_total, q_nose_correction = process_args(args)
|
||||||
|
|
||||||
args.freq = 10
|
pos = np.array(case['NatNetPos'], dtype=float)
|
||||||
args.ac = [(1,1)]
|
quat = np.array(case['NatNetQuat'], dtype=float) # convert to scalar last
|
||||||
|
quat = quat[[1,2,3,0]]
|
||||||
# in testcases, quaternions are scalar last
|
vel = np.array([0, 0, 0], dtype=float)
|
||||||
test_cases =\
|
|
||||||
[
|
posTarget = np.array(case['TargetPos'], dtype=float)
|
||||||
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
quatTarget = np.array(case['TargetQuat'], dtype=float)
|
||||||
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
|
||||||
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
posOut, _, quatOut = performTransformation(q_total,q_nose_correction, pos, vel, quat)
|
||||||
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
quatOut = np.array(quatOut)
|
||||||
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
quatOut = quatOut[[3,0,1,2]]
|
||||||
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
|
||||||
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
posError = np.linalg.norm(posTarget - posOut)
|
||||||
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
|
||||||
|
|
||||||
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
# quatError = Quat.absolute_distance(Quat(quatTarget), Quat(quatOut))
|
||||||
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
quatErrorQuat = Rotation.from_quat(quatTarget).inv()*Rotation.from_quat(quatOut)
|
||||||
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
quatError = quatErrorQuat.magnitude()
|
||||||
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
|
||||||
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
|
||||||
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
|
||||||
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
|
||||||
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
|
||||||
|
|
||||||
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
thresh = 3e-3
|
||||||
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
assert posError < thresh, f"Failed position case {i}. Should be {posTarget}, but is {posOut}. Error {posError}"
|
||||||
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
assert quatError < thresh, f"Failed quaternion case {i}. Should be {quatTarget}, but is {quatOut}. Error {quatError}"
|
||||||
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
|
||||||
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
if __name__ == '__main__':
|
||||||
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
import argparse
|
||||||
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
|
||||||
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] }
|
# parse args
|
||||||
]
|
parser = argparse.ArgumentParser(
|
||||||
|
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||||||
|
epilog=ADDITIONAL_HELP,
|
||||||
|
)
|
||||||
|
parser.add_argument('-le', '--long_edge', required=True, dest='long_edge', choices=['left', 'far', 'right', 'near'], help="Side of the test area that the long edge of the calibration tool was pointing at")
|
||||||
|
group = parser.add_mutually_exclusive_group(required=True)
|
||||||
|
group.add_argument('-xs', '--x_side', dest='x_side', choices=['left', 'far', 'right', 'near'], help="Side that the x/east axis of the resulting ENU frame should point to.")
|
||||||
|
group.add_argument('-xa', '--x_angle_from_right', dest='x_angle', type=float, help="Right-hand rotation in degrees of the x/east-axis around the up-axis, where 0.0 means x/east is pointing to the right")
|
||||||
|
parser.add_argument('-up', '--up_axis', dest='up_axis', choices=['y_up', 'z_up'], default='y_up', help="Optitrack Up axis: y_up or z_up.")
|
||||||
|
parser.add_argument('-an', '--ac_nose', required=True, dest='ac_nose', choices=['left', 'far', 'right', 'near'], help="Side of the test area that the nose of the drone was pointing at when definition the rigid body")
|
||||||
|
parser.add_argument('-ac', action='append', nargs=2,
|
||||||
|
metavar=('rigid_id','ac_id'), required=True, help='pair of rigid body and A/C id (multiple possible)')
|
||||||
|
|
||||||
|
parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port")
|
||||||
|
parser.add_argument('-s', '--server', dest='server', default="127.0.0.1", help="NatNet server IP address")
|
||||||
|
parser.add_argument('-m', '--multicast_addr', dest='multicast', default="239.255.42.99", help="NatNet server multicast address")
|
||||||
|
parser.add_argument('-dp', '--data_port', dest='data_port', type=int, default=1511, help="NatNet server data socket UDP port")
|
||||||
|
parser.add_argument('-cp', '--command_port', dest='command_port', type=int, default=1510, help="NatNet server command socket UDP port")
|
||||||
|
parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
|
||||||
|
parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="transmit frequency")
|
||||||
|
parser.add_argument('-gr', '--ground_ref', dest='ground_ref', action='store_true', help="also send the GROUND_REF message")
|
||||||
|
parser.add_argument('-vs', '--vel_samples', dest='vel_samples', default=4, type=int, help="amount of samples to compute velocity (should be greater than 2)")
|
||||||
|
parser.add_argument('-rg', '--remote_gps', dest='rgl_msg', action='store_true', help="use the old REMOTE_GPS_LOCAL message")
|
||||||
|
parser.add_argument('-sm', '--small', dest='small_msg', action='store_true', help="enable the EXTERNAL_POSE_SMALL message instead of the full")
|
||||||
|
parser.add_argument('-o', '--old_natnet', dest='old_natnet', action='store_true', help="Change the NatNet version to 2.9")
|
||||||
|
|
||||||
for i, case in enumerate(test_cases):
|
run_test_cases = '--test' in sys.argv
|
||||||
if (case['xs'] == 'right') or (case['xs'] == 'far'):
|
|
||||||
args.x_side = case['xs']
|
if not run_test_cases:
|
||||||
args.x_angle = None
|
# parse arguments
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
# start ivy interface
|
||||||
|
if args.ivy_bus is not None:
|
||||||
|
ivy = IvyMessagesInterface("natnet2ivy", ivy_bus=args.ivy_bus)
|
||||||
else:
|
else:
|
||||||
args.x_angle = case['xs']
|
ivy = IvyMessagesInterface("natnet2ivy")
|
||||||
args.x_side = None
|
|
||||||
|
|
||||||
args.up_axis = case['up_axis']
|
|
||||||
args.long_edge = case['long_edge']
|
|
||||||
args.ac_nose = case['ac_nose']
|
|
||||||
|
|
||||||
id_dict, timestamp, period, track, q_total, q_nose_correction = process_args(args)
|
|
||||||
|
|
||||||
pos = np.array(case['NatNetPos'], dtype=float)
|
|
||||||
quat = np.array(case['NatNetQuat'], dtype=float) # convert to scalar last
|
|
||||||
quat = quat[[1,2,3,0]]
|
|
||||||
vel = np.array([0, 0, 0], dtype=float)
|
|
||||||
|
|
||||||
posTarget = np.array(case['TargetPos'], dtype=float)
|
|
||||||
quatTarget = np.array(case['TargetQuat'], dtype=float)
|
|
||||||
|
|
||||||
posOut, _, quatOut = performTransformation(pos, vel, quat)
|
|
||||||
quatOut = np.array(quatOut)
|
|
||||||
quatOut = quatOut[[3,0,1,2]]
|
|
||||||
|
|
||||||
posError = np.linalg.norm(posTarget - posOut)
|
|
||||||
quatError = Quat.absolute_distance(Quat(quatTarget), Quat(quatOut))
|
|
||||||
|
|
||||||
thresh = 3e-3
|
# start natnet interface
|
||||||
assert posError < thresh, f"Failed position case {i}. Should be {posTarget}, but is {posOut}. Error {posError}"
|
main_loop(args, ivy)
|
||||||
assert quatError < thresh, f"Failed quaternion case {i}. Should be {quatTarget}, but is {quatOut}. Error {quatError}"
|
|
||||||
|
|
||||||
print(f"Passed all {i+1} test cases")
|
|
||||||
|
else:
|
||||||
|
#%% test cases
|
||||||
|
# Idea: position a drone at 90deg yaw to the right, and 15deg nose down.
|
||||||
|
# Then run through all settings and check output. Because it's a
|
||||||
|
# combined rotation, it should show all problems with transformations
|
||||||
|
#
|
||||||
|
# Correcteness of the NatNet pos and NatNet quaternion for each case have
|
||||||
|
# been experimentally verified. The TargetPos and TargetQuat are relatively
|
||||||
|
# easy to generate manually.
|
||||||
|
# Test flights still outstanding for setting xa to a numeric value
|
||||||
|
|
||||||
|
args = argparse.Namespace()
|
||||||
|
|
||||||
|
args.freq = 10
|
||||||
|
args.ac = [(1,1)]
|
||||||
|
|
||||||
|
# in testcases, quaternions are scalar last
|
||||||
|
test_cases =\
|
||||||
|
[
|
||||||
|
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
{'xs': 'right', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-2, -1, 0.1], 'TargetQuat': [ -.7, 0.1, -0.1, 0.7 ]},
|
||||||
|
|
||||||
|
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
{'xs': 'far', 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-1, 2, 0.1], 'TargetQuat': [ 0, 0, -0.14, 0.99 ] },
|
||||||
|
|
||||||
|
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ -.7, -0.1, 0.7, 0.1 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 0.1, -2], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [-.7, 0.1, 0.7, 0.1 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'y_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, 0.1, 1 ], 'NatNetQuat': [.99, 0, 0, -0.14 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'far', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ -.7, -0.1, -0.1, 0.7 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'right', 'ac_nose': 'right', 'NatNetPos': [-1, 2, 0.1], 'NatNetQuat': [ .99, 0.14, 0, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'far', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ -.7, 0.1, -0.1, 0.7 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] },
|
||||||
|
{'xs': -27., 'up_axis': 'z_up', 'long_edge': 'near', 'ac_nose': 'right', 'NatNetPos': [-2, -1, 0.1], 'NatNetQuat': [ .99, 0, 0.14, 0 ], 'TargetPos': [-1.33,-1.8,.1], 'TargetQuat': [ -.844, .12,-0.073, .517 ] }
|
||||||
|
]
|
||||||
|
|
||||||
|
for i, case in enumerate(test_cases):
|
||||||
|
test_case(args, i, case)
|
||||||
|
|
||||||
|
print(f"Passed all {i+1} test cases")
|
||||||
|
|||||||
Reference in New Issue
Block a user