1 import meshcat.geometry
as g
2 import pinocchio
as pin
4 from os.path import dirname, join, abspath
11 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
13 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
14 mesh_dir = pinocchio_model_dir
16 urdf_filename =
"panda.urdf"
17 urdf_model_path = join(join(model_path,
"panda_description/urdf"), urdf_filename)
19 robot, collision_model, visual_model = pin.buildModelsFromUrdf(
20 urdf_model_path, mesh_dir
22 data = robot.createData()
27 viz.initViewer(open=
True)
28 except ImportError
as err:
30 "Error while initializing the viewer. It seems you should install Python meshcat"
38 q0 = (robot.upperPositionLimit.T + robot.lowerPositionLimit.T) / 2
40 frame = robot.getFrameId(
50 verts, faces = pin.reachableWorkspaceHull(
51 robot, q0, horizon, frame, n_samples, facet_dims
59 from CGAL.CGAL_Polyhedron_3
import Polyhedron_3
61 except ModuleNotFoundError:
62 print(
"To compute non convex Polytope CGAL library needs to be installed.")
66 return v.x(), v.y(), v.z()
80 Compute the alpha shape of a set of points. (Code thanks to A. Skuric)
81 Retrieved from http://blog.thehumangeo.com/2014/05/12/drawing-boundaries-in-python/
83 :param coords : Coordinates of points
84 :param alpha: List of alpha values to influence the gooeyness of the border. Smaller numbers don't fall inward as much as larger numbers.
85 Too large, and you lose everything!
86 :return: Shapely.MultiPolygons which is the hull of the input set of points
89 bbox_diag = np.linalg.norm(np.max(coords, 0) - np.min(coords, 0))
90 alpha_value = bbox_diag / 5
92 alpha_value = np.mean(alpha)
94 points = [Point_3(pt[0], pt[1], pt[2])
for pt
in coords]
97 a = alpha_wrap_3(points, alpha_value, 0.01, Q)
99 alpha_shape_vertices = np.array(
102 alpha_shape_faces = np.array(
106 return alpha_shape_vertices, alpha_shape_faces
108 verts = pin.reachableWorkspace(robot, q0, horizon, frame, n_samples, facet_dims)
113 verts = faces.reshape(-1, 3)
114 faces = np.arange(len(verts)).reshape(-1, 3)
116 print(
"------------------- Display Vertex")
120 poly = g.TriangularMeshGeometry(vertices=verts, faces=faces)
121 viz.viewer[
"poly"].set_object(
122 poly, g.MeshBasicMaterial(color=0x000000, wireframe=
True, linewidth=12, opacity=0.2)
127 viz.viewer[
"poly"].set_object(
129 g.MeshBasicMaterial(color=0x000000, wireframe=
True, linewidth=2, opacity=0.2),