def test_rotate(self):
self.assertApprox(rotate('x', pi / 2), np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.'))
self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.matrix(list(range(3))).T * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
def test_rotate(self):
self.assertApprox(rotate('x', pi / 2),
np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.'))
self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.matrix(list(range(3))).T * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
def test_rotate(self):
self.assertApprox(rotate('x', pi / 2), np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]))
self.assertApprox(rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi))
m = rotate('x', pi / 3).dot(rotate('y', pi / 5)).dot(rotate('y', pi / 7))
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.array(list(range(3))) * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
self.assertApprox(rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
def test_rotate(self):
self.assertApprox(
rotate('x', pi / 2),
np.array([[1., 0., 0.], [0., 0., -1.], [0., 1., 0.]]))
self.assertApprox(
rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi))
m = rotate('x', pi / 3).dot(rotate('y',
pi / 5)).dot(rotate('y', pi / 7))
self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
rpy = np.array(list(range(3))) * pi / 2
self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
self.assertApprox(
rpyToMatrix(rpy),
rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
try:
rotate('toto', 10.)
except ValueError:
self.assertTrue(True)
else:
self.assertTrue(False)
try:
rotate('w', 10.)
except ValueError:
self.assertTrue(True)
else:
self.assertTrue(False)