def disabled_t_e_s_t_s():
return
assert b.value == 1
d = utmath.ScalarDouble(1.0)
assert d.value == 1.0
q = utmath.test_quat()
assert q.x() == 0 and q.y() == 0 and q.z() == 0 and q.w() == 1
m = utmath.test_mat33()
print m
print np.identity(3)
assert np.all(m == np.identity(3))
# from vector
p = utmath.Pose(q, m[0, :])
q1 = utmath.Quaternion(m[0, :], 1.0)
# from matrix
m1 = np.identity(4)
p1 = utmath.Pose(m1)
#test accessors
assert np.all(p1.translation() == np.asarray([0, 0, 0]))
assert np.all(p1.rotation().toVector() == np.array([0, 0, 0, -1]))
# needs proper verification
p2 = p * p1
p3 = p.invert()
v = p3 * np.asarray([1., 2., 3.])
def test_basic_datatypes():
"test basic data types: Vector2-8, Matrix33-44, Pose, Quaternion"
b = utmath.ScalarInt(1)
assert b.value == 1
d = utmath.ScalarDouble(1.0)
assert d.value == 1.0
q = utmath.Quaternion()
assert q.x() == 0 and q.y() == 0 and q.z() == 0 and q.w() == 1
m = utmath.Matrix33d.identity()
assert np.all(m == np.identity(3))
# from vector
p = utmath.Pose(q, np.asarray(m)[0, :])
q1 = utmath.Quaternion(np.asarray(m)[0, :], 1.0)
# from matrix
m1 = np.identity(4)
p1 = utmath.Pose(m1)
#test accessors
assert np.all(p1.translation() == np.asarray([0., 0., 0.]))
assert np.all(p1.rotation().toVector() == np.array([0., 0., 0., -1.]))
# needs proper verification
p2 = p * p1
p3 = p.invert()
v = p3 * utmath.Vector3d(1., 2., 3.)
def pull_cb(ts):
global pullsrc_ctr
from ubitrack.core import math, measurement
import numpy as np
pullsrc_ctr += 1
p = math.Pose(math.Quaternion(), np.array([1, 2, 3]))
return measurement.Pose(ts, p)
def __iter__(self):
if not self.check_input():
raise StopIteration()
rcls = self.make_record_class()
parent_fields = self.input_fieldnames
absolute_orientation_inv = self.absolute_orientation.invert()
for record in self.recordsource:
attrs = dict((k, getattr(record, k)) for k in parent_fields)
attrs['haptic_pose'] = self.forward_kinematics.calculate_pose(
record.jointangles, record.gimbalangles, record=record)
# HIP target pose in HDorigin
hiptarget_rotation = math.Quaternion(
(absolute_orientation_inv * record.externaltracker_pose *
self.tooltip_offset).rotation())
ht_pose_no_trans = math.Pose(hiptarget_rotation,
np.array([0, 0, 0]))
# re-orient zrefaxis_calib using hiptarget pose
zrefaxis = ht_pose_no_trans * self.zrefaxis_calib
# zrefaxis = hiptarget_rotation.transformVector(self.zrefaxis_calib)
attrs['zrefaxis'] = zrefaxis / np.linalg.norm(zrefaxis)
yield rcls(**attrs)
def calculate_pose(self, joint_angles, gimbal_angles, record=None):
jacf = self.jointangle_correction_factors
gacf = self.gimbalangle_correction_factors
l1 = self.joint_lengths[0]
l2 = self.joint_lengths[1]
calx = self.origin_calib[0]
caly = self.origin_calib[1]
calz = self.origin_calib[2]
O1 = joint_angles[0]
O2 = joint_angles[1]
O3 = joint_angles[2]
O4 = gimbal_angles[0]
O5 = gimbal_angles[1]
O6 = gimbal_angles[2]
k1 = jacf[0, 1]
m1 = jacf[0, 2]
k2 = jacf[1, 1]
m2 = jacf[1, 2]
k3 = jacf[2, 1]
m3 = jacf[2, 2]
k4 = gacf[0, 1]
m4 = gacf[0, 2]
k5 = gacf[1, 1]
m5 = gacf[1, 2]
k6 = gacf[2, 1]
m6 = gacf[2, 2]
# calculate translation
trans = np.array(
[calx - (l1*cos(O2*k2 + m2) + l2*sin(O3*k3 + m3))*sin(O1*k1 + m1),
caly + l1*sin(O2*k2 + m2) - l2*cos(O3*k3 + m3) + l2,
calz - l1 + (l1*cos(O2*k2 + m2) + l2*sin(O3*k3 + m3))*cos(O1*k1 + m1)]
)
# calculate rotation of arm
rot = np.array([[-(-(-sin(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4) + sin(O4*k4 + m4)*cos(O1*k1 + m1))*sin(O5*k5 + m5) + sin(O1*k1 + m1)*sin(O3*k3 + m3)*cos(O5*k5 + m5))*sin(O6*k6 + m6) + (sin(O1*k1 + m1)*sin(O4*k4 + m4)*cos(O3*k3 + m3) + cos(O1*k1 + m1)*cos(O4*k4 + m4))*cos(O6*k6 + m6),
(-(-sin(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4) + sin(O4*k4 + m4)*cos(O1*k1 + m1))*sin(O5*k5 + m5) + sin(O1*k1 + m1)*sin(O3*k3 + m3)*cos(O5*k5 + m5))*cos(O6*k6 + m6) + (sin(O1*k1 + m1)*sin(O4*k4 + m4)*cos(O3*k3 + m3) + cos(O1*k1 + m1)*cos(O4*k4 + m4))*sin(O6*k6 + m6),
(-sin(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4) + sin(O4*k4 + m4)*cos(O1*k1 + m1))*cos(O5*k5 + m5) + sin(O1*k1 + m1)*sin(O3*k3 + m3)*sin(O5*k5 + m5)],
[-(-sin(O3*k3 + m3)*sin(O5*k5 + m5)*cos(O4*k4 + m4) + cos(O3*k3 + m3)*cos(O5*k5 + m5))*sin(O6*k6 + m6) - sin(O3*k3 + m3)*sin(O4*k4 + m4)*cos(O6*k6 + m6),
(-sin(O3*k3 + m3)*sin(O5*k5 + m5)*cos(O4*k4 + m4) + cos(O3*k3 + m3)*cos(O5*k5 + m5))*cos(O6*k6 + m6) - sin(O3*k3 + m3)*sin(O4*k4 + m4)*sin(O6*k6 + m6),
sin(O3*k3 + m3)*cos(O4*k4 + m4)*cos(O5*k5 + m5) + sin(O5*k5 + m5)*cos(O3*k3 + m3)],
[-(-(sin(O1*k1 + m1)*sin(O4*k4 + m4) + cos(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4))*sin(O5*k5 + m5) - sin(O3*k3 + m3)*cos(O1*k1 + m1)*cos(O5*k5 + m5))*sin(O6*k6 + m6) + (sin(O1*k1 + m1)*cos(O4*k4 + m4) - sin(O4*k4 + m4)*cos(O1*k1 + m1)*cos(O3*k3 + m3))*cos(O6*k6 + m6),
(-(sin(O1*k1 + m1)*sin(O4*k4 + m4) + cos(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4))*sin(O5*k5 + m5) - sin(O3*k3 + m3)*cos(O1*k1 + m1)*cos(O5*k5 + m5))*cos(O6*k6 + m6) + (sin(O1*k1 + m1)*cos(O4*k4 + m4) - sin(O4*k4 + m4)*cos(O1*k1 + m1)*cos(O3*k3 + m3))*sin(O6*k6 + m6),
(sin(O1*k1 + m1)*sin(O4*k4 + m4) + cos(O1*k1 + m1)*cos(O3*k3 + m3)*cos(O4*k4 + m4))*cos(O5*k5 + m5) - sin(O3*k3 + m3)*sin(O5*k5 + m5)*cos(O1*k1 + m1)]])
return math.Pose(math.Quaternion.fromMatrix(rot).normalize(), trans)
def test_measurement_poselist():
"test measurement poselist"
poses = utmath.PoseList()
for i in range(5):
poses.push_back(
utmath.Pose(utmath.Quaternion(0.0, 0.0, 0.0, 1.0),
utmath.Vector3d(1.0, 2.0, 3.0)))
m = measurement.PoseList(measurement.now(), poses)
result = m.get()
assert len(result) == 5
element = result[0]
assert np.all(element.translation() == np.array([1.0, 2.0, 3.0]))
def test_measurement_pose():
"test measurement Pose"
i = measurement.Pose()
assert i.invalid()
assert i.get() == None
m = np.array([[1., 2., 3., 4.], [1., 2., 3., 4.], [1., 2., 3., 4.],
[1., 2., 3., 4.]])
p = utmath.Pose(m)
b = measurement.Pose(123, p)
p1 = b.get()
assert np.all(p1.toVector() == p.toVector())
assert b.time() == 123
assert b.invalid() == False
def test_create_quaternion():
m = np.array([[1., 2., 3., 4.], [1., 2., 3., 4.], [1., 2., 3., 4.],
[1., 2., 3., 4.]])
p = utmath.Pose(m)
axis, angle = p.rotation().toAxisAngle()
q = utmath.Quaternion(axis, angle)
assert str(q) == str(p.rotation())
#axis angle initialization not working
v = np.array([1., 0., 0.])
q = utmath.Quaternion(v, 0.)
assert np.all(q.toVector() == np.array([
0.,
0.,
0.,
1.,
]))
def loadCalibrationFiles(root_dir):
if isinstance(root_dir, unicode):
root_dir = root_dir.encode(sys.getdefaultencoding())
fname = os.path.join(root_dir, "phantom_jointangle_correction.calib")
if os.path.isfile(fname):
phantom_jointangle_calib = util.readCalibMeasurementMatrix3x3(fname)
log.info(
"Phantom Joint Angle Calibration\n%s" %
(phantom_jointangle_calib), )
else:
log.warn("Phantom Joint Angle Calibration NOT FOUND")
phantom_jointangle_calib = np.array([0.0, 1.0, 0.0] * 3).reshape(
(3, 3))
phantom_gimbalangle_calib = np.array([0.0, 1.0, 0.0] * 3).reshape((3, 3))
fname = os.path.join(root_dir, "absolute_orientation_calibration.calib")
if os.path.isfile(fname):
externaltracker_to_device = util.readCalibMeasurementPose(fname)
log.info("OptiTrack to Device Transform\n%s" %
(externaltracker_to_device, ))
else:
log.warn("Absolute Orientation Calibration NOT FOUND")
externaltracker_to_device = math.Pose(math.Quaternion(),
np.array([0.0, 0.0, 0.0]))
fname = os.path.join(root_dir, "tooltip_calibration.calib")
if os.path.isfile(fname):
tooltip_calib = util.readCalibMeasurementPosition(fname)
log.info("Tooltip Calibration\n%s" % (tooltip_calib, ))
else:
log.warn("Tooltip Calibration NOT FOUND")
tooltip_calib = np.array([0.0, 0.0, 0.0])
return dict(
phantom_jointangle_calib=phantom_jointangle_calib,
phantom_gimbalangle_calib=phantom_gimbalangle_calib,
externaltracker_to_device=externaltracker_to_device,
tooltip_calib=tooltip_calib,
)
def test_create_quaternion(self):
m = np.array([[1., 2., 3., 4.], [1., 2., 3., 4.], [1., 2., 3., 4.],
[1., 2., 3., 4.]])
p = utmath.Pose(m)
axis, angle = p.rotation().toAxisAngle()
q = utmath.Quaternion(axis, angle)
self.assertTrue(
str(q) == str(p.rotation()),
"quaternion created from axis angle is not equal to the original")
#axis angle initialization not working
v = np.array([1, 0, 0])
q = utmath.Quaternion(v, 0.)
print q.toVector()
self.assertTrue(np.all(q.toVector() == np.array([
0.,
0.,
0.,
1.,
])), "quaternion created from axis angle has wrong data")
def calculate_pose(self, joint_angles, gimbal_angles, record=None):
jacf = self.jointangle_correction_factors
gacf = self.gimbalangle_correction_factors
# current infrastructure only allows pre-calibrated platform sensors
# XXXX bad design !!!!
S1_ = 0.0
S2_ = 0.0
S3_ = 0.0
if hasattr(record, "scale_platform_sensors"):
S1_ = record.scale_platform_sensors[0]
S2_ = record.scale_platform_sensors[1]
S3_ = record.scale_platform_sensors[2]
else:
log.warn(
"Missing attributes 'scale_platform_sensors' on datasource.")
l1 = self.joint_lengths[0]
l2 = self.joint_lengths[1]
O1 = joint_angles[0]
O2 = joint_angles[1]
O3 = joint_angles[2]
O4 = gimbal_angles[0]
O5 = gimbal_angles[1]
O6 = gimbal_angles[2]
k1 = jacf[0, 1]
m1 = jacf[0, 2]
k2 = jacf[1, 1]
m2 = jacf[1, 2]
k3 = jacf[2, 1]
m3 = jacf[2, 2]
k4 = gacf[0, 1]
m4 = gacf[0, 2]
k5 = gacf[1, 1]
m5 = gacf[1, 2]
k6 = gacf[2, 1]
m6 = gacf[2, 2]
O1_ = O1 * k1 + m1
O2_ = O2 * k2 + m2
O3_ = O3 * k3 + m3
O4_ = O4 * k4 + m4
O5_ = O5 * k5 + m5
O6_ = O6 * k6 + m6
# calculate translation
trans = np.array([
S1_ + l1 * cos(O1_) * cos(O2_) + l2 * cos(O1_) * cos(O2_ + O3_),
S2_ + l1 * sin(O1_) * cos(O2_) + l2 * sin(O1_) * cos(O2_ + O3_),
S3_ - l1 * sin(O2_) - l2 * sin(O2_ + O3_)
])
# calculate rotation of arm
rot = np.array(
[[
-(sin(O4_) * sin(O5_) * cos(O6_) + sin(O6_) * cos(O4_)) *
sin(O1_) + (sin(O4_) * sin(O6_) * sin(O2_ + O3_) -
sin(O5_) * sin(O2_ + O3_) * cos(O4_) * cos(O6_) +
cos(O5_) * cos(O6_) * cos(O2_ + O3_)) * cos(O1_),
(sin(O4_) * sin(O5_) * cos(O6_) + sin(O6_) * cos(O4_)) *
cos(O1_) + (sin(O4_) * sin(O6_) * sin(O2_ + O3_) -
sin(O5_) * sin(O2_ + O3_) * cos(O4_) * cos(O6_) +
cos(O5_) * cos(O6_) * cos(O2_ + O3_)) * sin(O1_),
sin(O4_) * sin(O6_) * cos(O2_ + O3_) -
sin(O5_) * cos(O4_) * cos(O6_) * cos(O2_ + O3_) -
sin(O2_ + O3_) * cos(O5_) * cos(O6_)
],
[(sin(O4_) * sin(O5_) * sin(O6_) - cos(O4_) * cos(O6_)) * sin(O1_)
+ (sin(O4_) * sin(O2_ + O3_) * cos(O6_) +
sin(O5_) * sin(O6_) * sin(O2_ + O3_) * cos(O4_) -
sin(O6_) * cos(O5_) * cos(O2_ + O3_)) * cos(O1_),
-(sin(O4_) * sin(O5_) * sin(O6_) - cos(O4_) * cos(O6_)) *
cos(O1_) + (sin(O4_) * sin(O2_ + O3_) * cos(O6_) +
sin(O5_) * sin(O6_) * sin(O2_ + O3_) * cos(O4_) -
sin(O6_) * cos(O5_) * cos(O2_ + O3_)) * sin(O1_),
sin(O4_) * cos(O6_) * cos(O2_ + O3_) +
sin(O5_) * sin(O6_) * cos(O4_) * cos(O2_ + O3_) +
sin(O6_) * sin(O2_ + O3_) * cos(O5_)],
[(sin(O5_) * cos(O2_ + O3_) +
sin(O2_ + O3_) * cos(O4_) * cos(O5_)) * cos(O1_) +
sin(O1_) * sin(O4_) * cos(O5_),
(sin(O5_) * cos(O2_ + O3_) +
sin(O2_ + O3_) * cos(O4_) * cos(O5_)) * sin(O1_) -
sin(O4_) * cos(O1_) * cos(O5_),
-sin(O5_) * sin(O2_ + O3_) + cos(O4_) * cos(O5_) * cos(O2_ + O3_)
]])
return math.Pose(math.Quaternion.fromMatrix(rot.T).normalize(), trans)
def calculate_pose(self, joint_angles, gimbal_angles, record=None):
jacf = self.jointangle_correction_factors
gacf = self.gimbalangle_correction_factors
l1 = self.joint_lengths[0]
l2 = self.joint_lengths[1]
O1 = joint_angles[0]
O2 = joint_angles[1]
O3 = joint_angles[2]
O4 = gimbal_angles[0]
O5 = gimbal_angles[1]
O6 = gimbal_angles[2]
k1 = jacf[0, 1]
m1 = jacf[0, 2]
k2 = jacf[1, 1]
m2 = jacf[1, 2]
k3 = jacf[2, 1]
m3 = jacf[2, 2]
k4 = gacf[0, 1]
m4 = gacf[0, 2]
k5 = gacf[1, 1]
m5 = gacf[1, 2]
k6 = gacf[2, 1]
m6 = gacf[2, 2]
O1_ = O1 * k1 + m1
O2_ = O2 * k2 + m2
O3_ = O3 * k3 + m3
O4_ = O4 * k4 + m4
O5_ = O5 * k5 + m5
O6_ = O6 * k6 + m6
# calculate translation
trans = np.array([
l1 * cos(O1_) * cos(O2_) + l2 * cos(O1_) * cos(O2_ + O3_),
l1 * sin(O1_) * cos(O2_) + l2 * sin(O1_) * cos(O2_ + O3_),
l1 * sin(O2_) - l2 * sin(O2_ + O3_)
])
# calculate rotation of arm
rot = np.array(
[[
-(sin(O4_) * sin(O5_) * cos(O6_) + sin(O6_) * cos(O4_)) *
sin(O1_) + (sin(O4_) * sin(O6_) * sin(O2_ + O3_) -
sin(O5_) * sin(O2_ + O3_) * cos(O4_) * cos(O6_) +
cos(O5_) * cos(O6_) * cos(O2_ + O3_)) * cos(O1_),
(sin(O4_) * sin(O5_) * cos(O6_) + sin(O6_) * cos(O4_)) *
cos(O1_) + (sin(O4_) * sin(O6_) * sin(O2_ + O3_) -
sin(O5_) * sin(O2_ + O3_) * cos(O4_) * cos(O6_) +
cos(O5_) * cos(O6_) * cos(O2_ + O3_)) * sin(O1_),
sin(O4_) * sin(O6_) * cos(O2_ + O3_) -
sin(O5_) * cos(O4_) * cos(O6_) * cos(O2_ + O3_) -
sin(O2_ + O3_) * cos(O5_) * cos(O6_)
],
[(sin(O4_) * sin(O5_) * sin(O6_) - cos(O4_) * cos(O6_)) * sin(O1_)
+ (sin(O4_) * sin(O2_ + O3_) * cos(O6_) +
sin(O5_) * sin(O6_) * sin(O2_ + O3_) * cos(O4_) -
sin(O6_) * cos(O5_) * cos(O2_ + O3_)) * cos(O1_),
-(sin(O4_) * sin(O5_) * sin(O6_) - cos(O4_) * cos(O6_)) *
cos(O1_) + (sin(O4_) * sin(O2_ + O3_) * cos(O6_) +
sin(O5_) * sin(O6_) * sin(O2_ + O3_) * cos(O4_) -
sin(O6_) * cos(O5_) * cos(O2_ + O3_)) * sin(O1_),
sin(O4_) * cos(O6_) * cos(O2_ + O3_) +
sin(O5_) * sin(O6_) * cos(O4_) * cos(O2_ + O3_) +
sin(O6_) * sin(O2_ + O3_) * cos(O5_)],
[(sin(O5_) * cos(O2_ + O3_) +
sin(O2_ + O3_) * cos(O4_) * cos(O5_)) * cos(O1_) +
sin(O1_) * sin(O4_) * cos(O5_),
(sin(O5_) * cos(O2_ + O3_) +
sin(O2_ + O3_) * cos(O4_) * cos(O5_)) * sin(O1_) -
sin(O4_) * cos(O1_) * cos(O5_),
-sin(O5_) * sin(O2_ + O3_) + cos(O4_) * cos(O5_) * cos(O2_ + O3_)
]])
return math.Pose(math.Quaternion.fromMatrix(rot.T).normalize(), trans)
def calculate_pose(self, joint_angles, gimbal_angles, record=None):
jacf = self.jointangle_correction_factors
gacf = self.gimbalangle_correction_factors
l1 = self.joint_lengths[0]
l2 = self.joint_lengths[1]
calx = self.origin_calib[0]
caly = self.origin_calib[1]
calz = self.origin_calib[2]
O1_ = joint_angles[0]
O2_ = joint_angles[1]
O3_ = joint_angles[2]
O4_ = gimbal_angles[0]
O5_ = gimbal_angles[1]
O6_ = gimbal_angles[2]
j1 = jacf[0, 0]
k1 = jacf[0, 1]
m1 = jacf[0, 2]
j2 = jacf[1, 0]
k2 = jacf[1, 1]
m2 = jacf[1, 2]
j3 = jacf[2, 0]
k3 = jacf[2, 1]
m3 = jacf[2, 2]
j4 = gacf[0, 0]
k4 = gacf[0, 1]
m4 = gacf[0, 2]
j5 = gacf[1, 0]
k5 = gacf[1, 1]
m5 = gacf[1, 2]
j6 = gacf[2, 0]
k6 = gacf[2, 1]
m6 = gacf[2, 2]
O1 = j1*O1_**2 + O1_ * k1 + m1
O2 = j2*O2_**2 + O2_ * k2 + m2
O3 = j3*O3_**2 + O3_ * k3 + m3
O4 = j4*O4_**2 + O4_ * k4 + m4
O5 = j5*O5_**2 + O5_ * k5 + m5
O6 = j6*O6_**2 + O6_ * k6 + m6
sO1, sO2, sO3 = sin(O1), sin(O2), sin(O3)
sO4, sO5, sO6 = sin(O4), sin(O5), sin(O6)
cO1, cO2, cO3 = cos(O1), cos(O2), cos(O3)
cO4, cO5, cO6 = cos(O4), cos(O5), cos(O6)
# calculate translation
trans = np.array(
[calx - sO1*(cO2*l1 + l2*sO3),
-cO3*l2 + caly + l1*sO2 + l2,
cO1*(cO2*l1 + l2*sO3) + calz - l1]
)
# calculate rotation of arm
rot = np.array([[cO6*(cO1*cO4 + cO3*sO1*sO4) + sO6*(-cO5*sO1*sO3 - sO5*(-cO1*sO4 + cO3*cO4*sO1)),
cO6*(cO5*sO1*sO3 + sO5*(-cO1*sO4 + cO3*cO4*sO1)) + sO6*(cO1*cO4 + cO3*sO1*sO4),
cO5*(cO1*sO4 - cO3*cO4*sO1) + sO1*sO3*sO5],
[-cO6*sO3*sO4 + sO6*(-cO3*cO5 + cO4*sO3*sO5),
cO6*(cO3*cO5 - cO4*sO3*sO5) - sO3*sO4*sO6,
cO3*sO5 + cO4*cO5*sO3],
[cO6*(-cO1*cO3*sO4 + cO4*sO1) + sO6*(cO1*cO5*sO3 - sO5*(-cO1*cO3*cO4 - sO1*sO4)),
cO6*(-cO1*cO5*sO3 + sO5*(-cO1*cO3*cO4 - sO1*sO4)) + sO6*(-cO1*cO3*sO4 + cO4*sO1),
-cO1*sO3*sO5 + cO5*(cO1*cO3*cO4 + sO1*sO4)]])
return math.Pose(math.Quaternion.fromMatrix(rot).normalize(), trans)
def position3d_to_pose(pos):
return math.Pose(math.Quaternion(), pos)
def pull_cb(ts):
from ubitrack.core import math, measurement
import numpy as np
p = math.Pose(math.Quaternion(), np.array([1., 2., 3.],
dtype=np.double))
return measurement.Pose(ts, p)
