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])))
