class Transform(object):
'''Class to represent transform operations. Note that the operations
provided are isolated from each other, in the sense that the sequence of
operations is always: rotation, scale and translation. That means that an
operation add to itself and the final outcome will always be in the
following order: accumulated scale, accumulated rotation and accumulated
translation.'''
def __init__(self):
self.quaternion = Quaternion((1, 0, 0, 0))
self.translation = Vector3(0.0, 0.0, 0.0)
self.scale_factor = 1.0
def scale(self, scale):
self.scale_factor *= scale
def rotation_quaternion(self, vector, angle):
if not vector.length():
return None
c = math.cos(angle / 2.0)
v = math.sin(angle / 2.0) * vector.normalized()
q = Quaternion((c, v.x, v.y, v.z))
q.normalize()
return q
def rotate(self, vector, angle):
q = self.rotation_quaternion(vector, angle)
if not q:
return
self.quaternion = q * self.quaternion
self.quaternion.normalize()
def set_rotation(self, vector, angle):
q = self.rotation_quaternion(vector, angle)
if not q:
return
self.quaternion = q
def set_euler(self, roll, pitch, yaw):
self.quaternion = Quaternion((roll, pitch, yaw))
self.quaternion.normalize()
def translate(self, d):
self.translation += d
def mat4(self):
s = self.scale_factor
m = self.quaternion.dcm
d = self.translation
return (c_float * 16)(
s * m.a.x, s * m.b.x, s * m.c.x, 0,
s * m.a.y, s * m.b.y, s * m.c.y, 0,
s * m.a.z, s * m.b.z, s * m.c.z, 0,
d.x, d.y, d.z, 1
)
def apply(self, v):
v = self.quaternion.transform(v) * self.scale_factor
v += self.translation
return v
def rotation_quaternion(self, vector, angle):
if not vector.length():
return None
c = math.cos(angle / 2.0)
v = math.sin(angle / 2.0) * vector.normalized()
q = Quaternion((c, v.x, v.y, v.z))
q.normalize()
return q
def rotation_quaternion(self, vector, angle):
if not vector.length():
return None
c = math.cos(angle / 2.0)
v = math.sin(angle / 2.0) * vector.normalized()
q = Quaternion((c, v.x, v.y, v.z))
q.normalize()
return q
def test_conversion(self):
"""
Tests forward and backward conversions
"""
for q in self.quaternions:
# quaternion -> euler -> quaternion
q0 = q
e = Quaternion(q.q).euler
q1 = Quaternion(e)
assert q0.close(q1)
# quaternion -> dcm (Matrix3) -> quaternion
q0 = q
dcm = Quaternion(q.q).dcm
q1 = Quaternion(dcm)
assert q0.close(q1)
def gimbal_controller(dcm_estimated, dcm_demanded, chan1):
'''return the delta to chan1 and chan2'''
quat_est = Quaternion(dcm_estimated)
quat_dem = Quaternion(dcm_demanded)
quatErr = quat_division(quat_dem, quat_est)
if quatErr[0] >= 0.0:
scaler = 2.0
else:
scaler = -2.0
bf_rates = scaler * Vector3(quatErr[1], quatErr[2], quatErr[3])
yaw_joint_rad = radians((chan1 - ROTATIONS['level'][0]) * YAW_SCALE)
chan1_change = degrees(bf_rates.z) * YAW_SCALE
chan2_change = degrees(bf_rates.x * sin(yaw_joint_rad) +
bf_rates.y * cos(yaw_joint_rad)) / PITCH_SCALE
return (chan1_change, chan2_change)
def get_vicon_pose(self, object_name, segment_name):
# get position in mm. Coordinates are in NED
vicon_pos = self.vicon.get_segment_global_translation(
object_name, segment_name)
if vicon_pos is None:
# Object is not in view
return None, None, None, None
vicon_quat = Quaternion(
self.vicon.get_segment_global_quaternion(object_name,
segment_name))
pos_ned = Vector3(vicon_pos * 0.001)
euler = vicon_quat.euler
roll, pitch, yaw = euler[0], euler[1], euler[2]
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
return pos_ned, roll, pitch, yaw
def _helper_test_constructor(self, q, euler, dcm):
"""
Helper function for constructor test
Calls constructor for the quaternion from q euler and dcm and checks
if the resulting converions are equivalent to the arguments.
The test for the euler angles is weak as the solution is not unique
:param q: quaternion 4x1, [w, x, y, z]
:param euler: Vector3(roll, pitch, yaw), needs to be equivalent to q
:param q: Matrix3, needs to be equivalent to q
"""
# construct q from a Quaternion
quaternion_instance = Quaternion(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
# construct q from a QuaternionBase
quaternion_instance = QuaternionBase(q)
q_test = Quaternion(quaternion_instance)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(quaternion_instance)
assert q_test.dcm.close(dcm)
q_test = Quaternion(quaternion_instance)
q_euler = Quaternion(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
# construct q from a quaternion
q_test = Quaternion(q)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(q)
assert q_test.dcm.close(dcm)
q_test = Quaternion(q)
q_euler = Quaternion(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
# # construct q from a euler angles
q_test = Quaternion(euler)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(euler)
assert q_test.dcm.close(dcm)
q_test = Quaternion(euler)
q_euler = Quaternion(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
# # construct q from dcm (Matrix3 instance)
q_test = Quaternion(dcm)
np.testing.assert_almost_equal(q_test.q, q)
q_test = Quaternion(dcm)
assert q_test.dcm.close(dcm)
q_test = Quaternion(dcm)
q_euler = Quaternion(q_test.euler)
assert (np.allclose(q_test.euler, euler)
or np.allclose(q_test.q, q_euler.q))
def _all_quaternions(self):
"""Generate quaternions from all euler angles"""
return [Quaternion(e) for e in self._all_angles()]
def main_loop():
'''loop getting vicon data'''
global vicon
name = vicon.get_subject_name(0)
segname = vicon.get_subject_root_segment_name(name)
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
gps_period_ms = 1000 // args.gps_rate
while True:
if args.send_zero:
time.sleep(0.05)
else:
vicon.get_frame()
now = time.time()
now_ms = int(now * 1000)
if args.rate is not None:
dt = now - last_msg_time
if dt < 1.0 / args.rate:
continue
last_msg_time = now
# get position as ENU mm
if args.send_zero:
pos_enu = [0.0, 0.0, 0.0]
else:
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = [pos_enu[0] * 0.001, pos_enu[1] * 0.001, -pos_enu[2] * 0.001]
# get orientation in euler, converting to ArduPilot conventions
if args.send_zero:
quat = [0.0, 0.0, 0.0, 1.0]
else:
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw -= math.pi * 0.5
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
if args.debug > 0:
print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" %
(pos_ned[0], pos_ned[1], pos_ned[2], math.degrees(roll),
math.degrees(pitch), math.degrees(yaw)))
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and not args.gps_only:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0,
0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(args.target_sysid,
int(origin[0] * 1.0e7),
int(origin[1] * 1.0e7),
int(origin[2] * 1.0e3), time_us)
last_origin_send = now
if args.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(origin[0], origin[1],
pos_ned[1], pos_ned[0])
gps_alt = origin[2] - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [(pos_ned[0] - last_gps_pos[0]) / gps_dt,
(pos_ned[1] - last_gps_pos[1]) / gps_dt,
(pos_ned[2] - last_gps_pos[2]) / gps_dt]
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if args.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel[0], gps_vel[1], gps_vel[2], 0.2,
1.0, 1.0, args.gps_nsats, yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
# send VISION_POSITION_ESTIMATE
if not args.gps_only:
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(time_us,
pos_ned[0],
pos_ned[1],
pos_ned[2],
roll,
pitch,
yaw,
force_mavlink1=True)
def __init__(self):
self.quaternion = Quaternion((1, 0, 0, 0))
self.translation = Vector3(0.0, 0.0, 0.0)
self.scale_factor = 1.0
def thread_loop(self):
'''background processing'''
name = None
last_pos = None
last_frame_num = None
while True:
vicon = self.vicon
if vicon is None:
time.sleep(0.1)
continue
if not name:
vicon.get_frame()
name = vicon.get_subject_name(0)
if name is None:
continue
segname = vicon.get_subject_root_segment_name(name)
print("Connected to subject '%s' segment '%s'" %
(name, segname))
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
frame_rate = vicon.get_frame_rate()
frame_dt = 1.0 / frame_rate
last_rate = time.time()
frame_count = 0
print("Vicon frame rate %.1f" % frame_rate)
if self.vicon_settings.gps_rate > 0:
gps_period_ms = 1000 // self.vicon_settings.gps_rate
time.sleep(0.01)
vicon.get_frame()
now = time.time()
now_ms = int(now * 1000)
frame_num = vicon.get_frame_number()
frame_count += 1
if now - last_rate > 0.1:
rate = frame_count / (now - last_rate)
self.actual_frame_rate = 0.9 * self.actual_frame_rate + 0.1 * rate
last_rate = now
frame_count = 0
self.vel_filter.set_cutoff_frequency(
self.actual_frame_rate, self.vicon_settings.vel_filter_hz)
# get position in mm
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = Vector3(pos_enu[1] * 0.001, pos_enu[0] * 0.001,
-pos_enu[2] * 0.001)
if last_frame_num is None or frame_num - last_frame_num > 100 or frame_num <= last_frame_num:
last_frame_num = frame_num
last_pos = pos_ned
continue
dt = (frame_num - last_frame_num) * frame_dt
vel = (pos_ned - last_pos) * (1.0 / dt)
last_pos = pos_ned
last_frame_num = frame_num
filtered_vel = self.vel_filter.apply(vel)
if self.vicon_settings.vision_rate > 0:
dt = now - last_msg_time
if dt < 1.0 / self.vicon_settings.vision_rate:
continue
last_msg_time = now
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw += math.radians(self.vicon_settings.yaw_offset)
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
self.pos = pos_ned
self.att = [
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
]
self.frame_count += 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and self.vicon_settings.vision_rate > 0:
for m, t, _ in self.master:
# send a heartbeat msg
m.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0, 0, 0)
# send origin at 1Hz
m.mav.set_gps_global_origin_send(
t, int(self.vicon_settings.origin_lat * 1.0e7),
int(self.vicon_settings.origin_lon * 1.0e7),
int(self.vicon_settings.origin_alt * 1.0e3), time_us)
last_origin_send = now
if self.vicon_settings.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(
self.vicon_settings.origin_lat,
self.vicon_settings.origin_lon, pos_ned.y, pos_ned.x)
gps_alt = self.vicon_settings.origin_alt - pos_ned.z
gps_dt = now - last_gps_send
gps_vel = filtered_vel
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if self.vicon_settings.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel.x, gps_vel.y, gps_vel.z, 0.2,
1.0, 1.0, self.vicon_settings.gps_nsats,
yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
self.gps_count += 1
if self.vicon_settings.vision_rate > 0:
# send VISION_POSITION_ESTIMATE
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(
time_us,
pos_ned.x,
pos_ned.y,
pos_ned.z,
roll,
pitch,
yaw,
force_mavlink1=True)
self.vision_count += 1
def set_euler(self, roll, pitch, yaw):
self.quaternion = Quaternion((roll, pitch, yaw))
self.quaternion.normalize()
def __init__(self):
self.quaternion = Quaternion((1, 0, 0, 0))
self.translation = Vector3(0.0, 0.0, 0.0)
self.scale_factor = 1.0
def quaternion_to_AP(quaternion):
'''convert pybullet quaternion to ArduPilot quaternion'''
return Quaternion(
[quaternion[3], quaternion[0], -quaternion[1], -quaternion[2]])
def set_euler(self, roll, pitch, yaw):
self.quaternion = Quaternion((roll, pitch, yaw))
self.quaternion.normalize()
def SetEuler(self, roll, pitch, yaw, callback=None):
self.desired_quaternion = Quaternion((roll, pitch, yaw))
self.attitude_callback = callback
self.actuation_state = 'attitude'
self.angvel = Vector3(1, 0, 0), 0
self.StartActuation()
def thread_loop(self):
'''background processing'''
name = None
while True:
vicon = self.vicon
if vicon is None:
time.sleep(0.1)
continue
if not name:
vicon.get_frame()
name = vicon.get_subject_name(0)
if name is None:
continue
segname = vicon.get_subject_root_segment_name(name)
print("Connected to subject '%s' segment '%s'" %
(name, segname))
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
last_origin_send = now
now_ms = int(now * 1000)
last_gps_send_ms = now_ms
last_gps_send = now
if self.vicon_settings.gps_rate > 0:
gps_period_ms = 1000 // self.vicon_settings.gps_rate
time.sleep(0.01)
vicon.get_frame()
mav = self.master
now = time.time()
now_ms = int(now * 1000)
if self.vicon_settings.vision_rate > 0:
dt = now - last_msg_time
if dt < 1.0 / self.vicon_settings.vision_rate:
continue
last_msg_time = now
# get position in mm
pos_enu = vicon.get_segment_global_translation(name, segname)
if pos_enu is None:
continue
# convert to NED meters
pos_ned = [
pos_enu[1] * 0.001, pos_enu[0] * 0.001, -pos_enu[2] * 0.001
]
# get orientation in euler, converting to ArduPilot conventions
quat = vicon.get_segment_global_quaternion(name, segname)
q = Quaternion([quat[3], quat[0], quat[1], quat[2]])
euler = q.euler
roll, pitch, yaw = euler[2], euler[1], euler[0]
yaw -= math.pi * 0.5
yaw = math.radians(mavextra.wrap_360(math.degrees(yaw)))
self.pos = pos_ned
self.att = [
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
]
self.frame_count += 1
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and self.vicon_settings.vision_rate > 0:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
0, 0, 0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(
self.target_system,
int(self.vicon_settings.origin_lat * 1.0e7),
int(self.vicon_settings.origin_lon * 1.0e7),
int(self.vicon_settings.origin_alt * 1.0e3), time_us)
last_origin_send = now
if self.vicon_settings.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(
self.vicon_settings.origin_lat,
self.vicon_settings.origin_lon, pos_ned[1], pos_ned[0])
gps_alt = self.vicon_settings.origin_alt - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [(pos_ned[0] - last_gps_pos[0]) / gps_dt,
(pos_ned[1] - last_gps_pos[1]) / gps_dt,
(pos_ned[2] - last_gps_pos[2]) / gps_dt]
last_gps_pos = pos_ned
gps_week, gps_week_ms = get_gps_time(now)
if self.vicon_settings.gps_nsats >= 6:
fix_type = 3
else:
fix_type = 1
mav.mav.gps_input_send(time_us, 0, 0, gps_week_ms, gps_week,
fix_type, int(gps_lat * 1.0e7),
int(gps_lon * 1.0e7), gps_alt, 1.0, 1.0,
gps_vel[0], gps_vel[1], gps_vel[2], 0.2,
1.0, 1.0, self.vicon_settings.gps_nsats,
yaw_cd)
last_gps_send_ms = (now_ms // gps_period_ms) * gps_period_ms
last_gps_send = now
self.gps_count += 1
if self.vicon_settings.vision_rate > 0:
# send VISION_POSITION_ESTIMATE
# we force mavlink1 to avoid the covariances which seem to make the packets too large
# for the mavesp8266 wifi bridge
mav.mav.global_vision_position_estimate_send(
time_us,
pos_ned[0],
pos_ned[1],
pos_ned[2],
roll,
pitch,
yaw,
force_mavlink1=True)
self.vision_count += 1