5 import pinocchio
as pin
9 from os.path import dirname, join, abspath
15 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
17 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
18 mesh_dir = pinocchio_model_dir
19 urdf_filename =
"panda.urdf"
20 urdf_model_path = join(join(model_path,
"panda_description/urdf"), urdf_filename)
22 model, collision_model, visual_model = pin.buildModelsFromUrdf(
23 urdf_model_path, mesh_dir
28 points = np.random.rand(3, num_points)
29 point_cloud_placement = (
32 point_cloud_placement.translation = np.array([0.2, 0.2, -0.5])
39 x_grid = np.linspace(0.0, 1.0, nx)
40 x_half_pad = 0.5 * (x_grid[1] - x_grid[0])
41 x_bins = np.digitize(X, x_grid + x_half_pad)
42 x_dim = x_grid[-1] - x_grid[0]
45 y_grid = np.linspace(0.0, 1.0, ny)
46 y_half_pad = 0.5 * (y_grid[1] - y_grid[0])
47 y_bins = np.digitize(Y, y_grid + y_half_pad)
48 y_dim = y_grid[-1] - y_grid[0]
50 point_bins = y_bins * nx + x_bins
51 heights = np.zeros((ny, nx))
52 np.maximum.at(heights.ravel(), point_bins, Z)
54 point_cloud = fcl.BVHModelOBBRSS()
55 point_cloud.beginModel(0, num_points)
56 point_cloud.addVertices(points.T)
58 height_field = fcl.HeightFieldOBBRSS(x_dim, y_dim, heights,
min(Z))
59 height_field_placement = point_cloud_placement * pin.SE3(
60 np.eye(3), 0.5 * np.array([x_grid[0] + x_grid[-1], y_grid[0] + y_grid[-1], 0.0])
63 go_point_cloud = pin.GeometryObject(
64 "point_cloud", 0, point_cloud_placement, point_cloud
66 go_point_cloud.meshColor = np.ones((4))
67 collision_model.addGeometryObject(go_point_cloud)
68 visual_model.addGeometryObject(go_point_cloud)
70 go_height_field = pin.GeometryObject(
71 "height_field", 0, height_field_placement, height_field
73 go_height_field.meshColor = np.ones((4))
74 height_field_collision_id = collision_model.addGeometryObject(go_height_field)
75 visual_model.addGeometryObject(go_height_field)
78 panda_hand_collision_id = collision_model.getGeometryId(
"panda_hand_0")
79 go_panda_hand = collision_model.geometryObjects[panda_hand_collision_id]
80 go_panda_hand.geometry.buildConvexRepresentation(
False)
81 go_panda_hand.geometry = (
82 go_panda_hand.geometry.convex
85 collision_pair = pin.CollisionPair(height_field_collision_id, panda_hand_collision_id)
86 collision_model.addCollisionPair(collision_pair)
96 viz.initViewer(open=
True)
97 except ImportError
as err:
99 "Error while initializing the viewer. It seems you should install Python meshcat"
105 viz.loadViewerModel()
108 q0 = pin.neutral(model)
112 data = model.createData()
113 collision_data = collision_model.createData()
114 while not is_collision:
115 q = pin.randomConfiguration(model)
117 is_collision = pin.computeCollisions(
118 model, data, collision_model, collision_data, q,
True
121 print(
"Found a configuration in collision:", q)