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)