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
