def test_rotation_matrix(self): testing.assert_almost_equal( rotation_matrix(pi, [1, 1, 1]), np.array([[-0.33333333, 0.66666667, 0.66666667], [0.66666667, -0.33333333, 0.66666667], [0.66666667, 0.66666667, -0.33333333]])) testing.assert_almost_equal(rotation_matrix(2 * pi, [1, 1, 1]), np.eye(3)) testing.assert_almost_equal(rotation_matrix(pi / 3, [1, 0, 0]), np.array([[1.0, 0.0, 0.0], [0.0, 0.5, -0.866025], [0.0, 0.866025, 0.5]]), decimal=5) testing.assert_almost_equal(rotation_matrix(pi / 3, [0, 1, 0]), np.array([[0.5, 0.0, 0.866025], [0.0, 1.0, 0.0], [-0.866025, 0.0, 0.5]]), decimal=5) testing.assert_almost_equal(rotation_matrix(pi / 3, [0, 0, 1]), np.array([[0.5, -0.866025, 0.0], [0.866025, 0.5, 0.0], [0.0, 0.0, 1.0]]), decimal=5)
def rotate(self, theta, axis, wrt='local'): """Rotate this coordinate. Rotate this coordinate relative to axis by theta radians with respect to wrt. Parameters ---------- theta : float radian axis : str or numpy.ndarray 'x', 'y', 'z' or vector wrt : str or Coordinates Returns ------- self """ if isinstance(axis, list) or isinstance(axis, np.ndarray): return self.rotate_with_matrix(rotation_matrix(theta, axis), wrt) if isinstance(axis, np.ndarray) and axis.shape == (3, 3): return self.rotate_with_matrix(theta, wrt) if wrt == 'local' or wrt == self: self.rotation = rotate_matrix(self.rotation, theta, axis) return self.newcoords(self.rotation, self.translation) elif wrt == 'parent' or wrt == self.parent: self.rotation = rotate_matrix(self.rotation, theta, axis) return self.newcoords(self.rotation, self.translation) else: return self.rotate_with_matrix(rotation_matrix(theta, axis), wrt)
def test_dissoc(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0], name='a') b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2], name='b') a.assoc(b) a.dissoc(b) testing.assert_almost_equal( b.worldrot(), rotation_matrix(pi / 3, 'y'), decimal=5) a.rotate(pi / 2.0, 'z') testing.assert_almost_equal( a.worldrot(), [[2.22044605e-16, -1.00000000e+00, 0.00000000e+00], [5.00000000e-01, 1.11022302e-16, -8.66025404e-01], [8.66025404e-01, 1.92296269e-16, 5.00000000e-01]], decimal=5) testing.assert_almost_equal( b.worldrot(), rotation_matrix(pi / 3, 'y'), decimal=5) testing.assert_almost_equal( a.worldpos(), [0.1, 0, 0]) testing.assert_almost_equal( b.worldpos(), [0.1, 0, 0.2])
def test_transform(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0]) b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2]) a.assoc(b) testing.assert_almost_equal( b.copy_worldcoords().transform( make_cascoords(pos=(-0.1, -0.2, -0.3)), 'local').translation, (-0.20980762113533155, -0.1999999999999999, 0.13660254037844383)) testing.assert_almost_equal( b.copy_worldcoords().transform( make_cascoords(pos=(-0.1, -0.2, -0.3)), 'world').translation, (0, -0.2, -0.1)) c = make_coords(pos=(-0.2, -0.3, -0.4)).rotate(pi / 2, 'y') b.transform( make_cascoords(pos=(-0.1, -0.2, -0.3)), c) testing.assert_almost_equal( b.translation, (-0.3, 0.15980762113533148, 0.32320508075688775)) testing.assert_almost_equal( b.copy_worldcoords().translation, (-0.2, -0.2, 0.3)) wrts = ['local', 'world', 'parent'] for wrt in wrts: b.transform( make_cascoords(pos=(-0.1, -0.2, -0.3)), wrt=wrt)
def test_assoc(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0], name='a') b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2], name='b') child = a.assoc(b) self.assertEqual(b, child) child = a.assoc(b, force=True) self.assertEqual(b, child) testing.assert_almost_equal( b.worldrot(), [[0.5, 0, 0.866025], [0, 1.0, 0.0], [-0.866025, 0, 0.5]], decimal=5) a.rotate(pi / 2.0, 'z') testing.assert_almost_equal( a.worldrot(), [[2.22044605e-16, -1.00000000e+00, 0.00000000e+00], [5.00000000e-01, 1.11022302e-16, -8.66025404e-01], [8.66025404e-01, 1.92296269e-16, 5.00000000e-01]], decimal=5) testing.assert_almost_equal( b.worldrot(), [[0.75, -0.5, -0.433013], [0.625, 0.75, 0.216506], [0.216506, -0.433013, 0.875]], decimal=5) testing.assert_almost_equal(a.worldpos(), [0.1, 0, 0]) testing.assert_almost_equal(b.worldpos(), [-0.07320508, -0.08660254, 0.05]) c = make_cascoords() with self.assertRaises(RuntimeError): c.assoc(b) with self.assertRaises(TypeError): c.assoc(b, relative_coords=1, force=True)
def rotate(self, theta, axis=None, wrt='local'): """Rotate this coordinate by given theta and axis. Parameters ---------- theta : float radian wrt : string or skrobot.coordinates.Coordinates Returns ------- self : skrobot.coordinates.Coordinates """ if isinstance(axis, list) or isinstance(axis, np.ndarray): self.rotate_with_matrix(rotation_matrix(theta, axis), wrt) elif axis is None or axis is False: self.rotate_with_matrix(theta, wrt) elif wrt == 'local' or wrt == self: self.rotation = rotate_matrix(self.rotation, theta, axis) elif wrt == 'parent' or wrt == 'world': self.rotation = rotate_matrix(self.rotation, theta, axis, True) elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1 self.rotate_with_matrix(rotation_matrix(theta, axis), wrt) else: raise ValueError('wrt {} not supported'.format(wrt)) return self.newcoords(self.rotation, self.translation)
def test_assoc(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0], name='a') b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2], name='b') a.assoc(b) testing.assert_almost_equal( b.worldrot(), [[0.5, 0, 0.866025], [0, 1.0, 0.0], [-0.866025, 0, 0.5]], decimal=5) a.rotate(pi / 2.0, 'z') testing.assert_almost_equal( a.worldrot(), [[2.22044605e-16, -1.00000000e+00, 0.00000000e+00], [5.00000000e-01, 1.11022302e-16, -8.66025404e-01], [8.66025404e-01, 1.92296269e-16, 5.00000000e-01]], decimal=5) testing.assert_almost_equal( b.worldrot(), [[0.75, -0.5, -0.433013], [0.625, 0.75, 0.216506], [0.216506, -0.433013, 0.875]], decimal=5) testing.assert_almost_equal( a.worldpos(), [0.1, 0, 0]) testing.assert_almost_equal( b.worldpos(), [-0.07320508, -0.08660254, 0.05])
def test_worldcoords(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0], name='a') b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2], name='b', parent=a) original_id = hex(id(b.worldcoords())) a.rotate(pi / 2.0, 'z') self.assertEqual(original_id, hex(id(b.worldcoords())))
def rotate(self, theta, axis=None, wrt='local'): """Rotate this coordinate by given theta and axis. This coordinate system is rotated relative to theta radians around the `axis` axis. Note that this function does not change a position of this coordinate. If you want to rotate this coordinates around with world frame, you can use `transform` function. Please see examples. Parameters ---------- theta : float relartive rotation angle in radian. axis : str or None or numpy.ndarray axis of rotation. The value of `axis` is represented as `wrt` frame. wrt : str or skrobot.coordinates.Coordinates Returns ------- self : skrobot.coordinates.Coordinates Examples -------- >>> from skrobot.coordinates import Coordinates >>> from numpy import pi >>> c = Coordinates() >>> c.translate((1.0, 0, 0)) >>> c.rotate(pi / 2.0, 'z', wrt='local') >>> c.translation array([1., 0., 0.]) >>> c.transform(Coordinates().rotate(np.pi / 2.0, 'z'), wrt='world') >>> c.translation array([0., 1., 0.]) """ if isinstance(axis, list) or isinstance(axis, np.ndarray): self.rotate_with_matrix(rotation_matrix(theta, axis), wrt) elif axis is None or axis is False: self.rotate_with_matrix(theta, wrt) elif wrt == 'local' or wrt == self: self.rotation = rotate_matrix(self.rotation, theta, axis) elif wrt == 'parent' or wrt == 'world': self.rotation = rotate_matrix(self.rotation, theta, axis, True) elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1 self.rotate_with_matrix(rotation_matrix(theta, axis), wrt) else: raise ValueError('wrt {} not supported'.format(wrt)) return self.newcoords(self.rotation, self.translation)
def test_changed(self): a = make_cascoords(rot=rotation_matrix(pi / 3, 'x'), pos=[0.1, 0, 0]) b = make_cascoords(rot=rotation_matrix(pi / 3, 'y'), pos=[0.1, 0, 0.2]) self.assertEqual(a._changed, True) self.assertEqual(b._changed, True) a.assoc(b) self.assertEqual(a._changed, False) self.assertEqual(b._changed, True) a.rotate(pi / 2.0, 'z') self.assertEqual(a._changed, True) self.assertEqual(b._changed, True) b.worldrot() self.assertEqual(a._changed, False) self.assertEqual(b._changed, False)
def test_quaternion_from_axis_angle(self): q = quaternion_from_axis_angle(0.1, [1, 0, 0]) testing.assert_almost_equal( q, matrix2quaternion(rotation_matrix(0.1, [1, 0, 0])))