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)