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 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))
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 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_orient_with_matrix(self):
coords = make_cascoords()
rotation_matrix = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
coords.orient_with_matrix(rotation_matrix, wrt='local')
coords.orient_with_matrix(rotation_matrix, wrt='parent')
coords.orient_with_matrix(rotation_matrix, wrt='world')
wrt = make_coords().rotate(np.pi / 2.0, 'z')
coords.orient_with_matrix(rotation_matrix, wrt=wrt)
def trimesh2sdf(mesh, dim_grid=100, padding_grid=5):
"""Convert trimesh to signed distance function.
Parameters
----------
mesh : trimesh.base.Trimesh
mesh object.
dim_grid : int
dimension of the GridSDF.
This value is used for a not primitive mesh.
padding_grid : int
number of padding.
Returns
-------
sdf : skrobot.sdf.SignedDistanceFunction
converted signed distance function.
"""
if not ('file_path' in mesh.metadata
or 'shape' in mesh.metadata):
raise ValueError("Input mesh doesn't contain valid metadata"
" for converting SDF.")
is_loaded_mesh = 'file_path' in mesh.metadata
if is_loaded_mesh:
file_path = mesh.metadata['file_path']
sdf = GridSDF.from_objfile(file_path, dim_grid=dim_grid,
padding_grid=padding_grid)
else:
# process primtives
shape = mesh.metadata['shape']
if shape == 'box':
extents = mesh.metadata['extents']
sdf = BoxSDF([0, 0, 0], extents)
elif shape == 'cylinder':
height = mesh.metadata['height']
radius = mesh.metadata['radius']
sdf = CylinderSDF([0, 0, 0], radius=radius, height=height)
elif shape == 'sphere':
radius = mesh.metadata['radius']
sdf = SphereSDF([0, 0, 0], radius)
else:
msg = "primtive type {0} is not supported".format(shape)
raise ValueError(msg)
origin = mesh.metadata["origin"]
rotation_matrix = origin[:3, :3]
translation = origin[:3, 3]
sdf.coords = make_cascoords(pos=translation, rot=rotation_matrix)
return sdf
