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