1 import meshcat.geometry
as g
2 import pinocchio
as pin
5 from os.path import dirname, join, abspath
13 rotate = pin.utils.rotate
14 R = rotate(
"x", xyzrpy[3]) @ rotate(
"y", xyzrpy[4]) @ rotate(
"z", xyzrpy[5])
15 p = np.array(xyzrpy[:3])
20 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
22 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
23 mesh_dir = pinocchio_model_dir
25 urdf_path = join(model_path,
"panda_description/urdf/panda.urdf")
26 srdf_path = join(model_path,
"panda_description/srdf/panda.srdf")
28 robot, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
29 data = robot.createData()
34 [0.40, 0.0, 0.30, np.pi / 2, 0, 0],
35 [-0.08, -0.0, 0.75, np.pi / 2, 0, 0],
36 [0.23, -0.0, 0.04, np.pi / 2, 0, 0],
40 color = [1.0, 0.2, 0.2, 1.0]
41 rad, length = 0.1, 0.4
42 for i, xyzrpy
in enumerate(oMobs):
43 obs = pin.GeometryObject.CreateCapsule(rad, length)
44 obs.meshColor = np.array(
47 obs.name =
"obs%d" % i
50 collision_model.addGeometryObject(obs)
51 visual_model.addGeometryObject(obs)
54 collision_model.addAllCollisionPairs()
55 pin.removeCollisionPairs(robot, collision_model, srdf_path)
59 nbodies = collision_model.ngeoms - nobs
60 robotBodies = range(nbodies)
61 envBodies = range(nbodies, nbodies + nobs)
62 for a, b
in itertools.product(robotBodies, envBodies):
63 collision_model.addCollisionPair(pin.CollisionPair(a, b))
67 collision_data = pin.GeometryData(collision_model)
68 visual_data = pin.GeometryData(visual_model)
73 viz.initViewer(open=
True)
74 except ImportError
as err:
76 "Error while initializing the viewer. It seems you should install Python meshcat"
84 q0 = (robot.upperPositionLimit.T + robot.lowerPositionLimit.T) / 2
86 frame = robot.getFrameId(
96 verts, faces = pin.reachableWorkspaceWithCollisionsHull(
97 robot, collision_model, q0, horizon, frame, n_samples, facet_dims
105 from CGAL.CGAL_Polyhedron_3
import Polyhedron_3
107 except ModuleNotFoundError:
108 print(
"To compute non convex Polytope CGAL library needs to be installed.")
112 return v.x(), v.y(), v.z()
126 Compute the alpha shape of a set of points. (Code thanks to A. Skuric)
127 Retrieved from http://blog.thehumangeo.com/2014/05/12/drawing-boundaries-in-python/
129 :param coords : Coordinates of points
130 :param alpha: List of alpha values to influence the gooeyness of the border. Smaller numbers don't fall inward as much as larger numbers.
131 Too large, and you lose everything!
132 :return: Shapely.MultiPolygons which is the hull of the input set of points
135 bbox_diag = np.linalg.norm(np.max(coords, 0) - np.min(coords, 0))
136 alpha_value = bbox_diag / 5
138 alpha_value = np.mean(alpha)
140 points = [Point_3(pt[0], pt[1], pt[2])
for pt
in coords]
143 a = alpha_wrap_3(points, alpha_value, 0.01, Q)
144 alpha_shape_vertices = np.array(
148 alpha_shape_faces = np.array(
152 return alpha_shape_vertices, alpha_shape_faces
154 verts = pin.reachableWorkspaceWithCollisions(
155 robot, collision_model, q0, horizon, frame, n_samples, facet_dims
161 verts = faces.reshape(-1, 3)
162 faces = np.arange(len(verts)).reshape(-1, 3)
164 print(
"------------------- Display Vertex")
168 poly = g.TriangularMeshGeometry(vertices=verts, faces=faces)
169 viz.viewer[
"poly"].set_object(
170 poly, g.MeshBasicMaterial(color=0x000000, wireframe=
True, linewidth=12, opacity=0.2)
175 viz.viewer[
"poly"].set_object(
177 g.MeshBasicMaterial(color=0x000000, wireframe=
True, linewidth=2, opacity=0.2),