def _axis(pose, length=2): """Axis primitive""" q = Quaternion(pose[3], pose[4], pose[5], pose[6]) bild = ".color cornflower blue" + os.linesep bild += _sphere(pose, 0.3) + os.linesep bild += ".color 1 0 0" + os.linesep c = [length, 0, 0] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) bild += ".color 1 1 0" + os.linesep c = [0, length, 0] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) bild += ".color 0 0 1" + os.linesep c = [0, 0, length] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) return bild
def test_rotation(self): q = Quaternion(0.707106781, 0.0, 0.707106781, 0.0) v = [1.0, 0.0, 0.0] v2 = q.rotate(v) assert_almost_equals(0.0, v2[0]) assert_almost_equals(0.0, v2[1]) assert_almost_equals(-1.0, v2[2])
def test_rotation(self): q = Quaternion(0.707106781, 0.0, 0.707106781, 0.0) v = [1.0, 0.0, 0.0] v2 = q.rotate(v) assert_almost_equals(0.0, v2[0]) assert_almost_equals(0.0, v2[1]) assert_almost_equals(-1.0, v2[2])
def _axis(pose, length=2): """Axis primitive""" q = Quaternion(pose[3], pose[4], pose[5], pose[6]) bild = ".color cornflower blue" + os.linesep bild += _sphere(pose, 0.3) + os.linesep bild += ".color 1 0 0" + os.linesep c = [length, 0, 0] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) bild += ".color 1 1 0" + os.linesep c = [0, length, 0] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) bild += ".color 0 0 1" + os.linesep c = [0, 0, length] end = q.rotate(c) bild += ".arrow %f %f %f %f %f %f%s" % (pose[0], pose[1], pose[2], pose[0] + end[0], pose[1] + end[1], pose[2] + end[2], os.linesep) return bild