collision-with-point-clouds.py
Go to the documentation of this file.
1 # This examples shows how to perform collision detection between the end-effector of a robot and a point cloud depicted as a Height Field
2 # Note: this feature requires Meshcat to be installed, this can be done using
3 # pip install --user meshcat
4 
5 import pinocchio as pin
6 import hppfcl as fcl
7 import numpy as np
8 import sys
9 from os.path import dirname, join, abspath
10 
11 from pinocchio.visualize import MeshcatVisualizer
12 
13 # Load the URDF model.
14 # Conversion with str seems to be necessary when executing this file with ipython
15 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),"models")
16 
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)
21 
22 model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_model_path, mesh_dir)
23 
24 # Add point clouds
25 num_points = 5000
26 points = np.random.rand(3, num_points)
27 point_cloud_placement = pin.SE3.Identity() # Placement of the point cloud wrt the WORLD frame
28 point_cloud_placement.translation = np.array([0.2,0.2,-0.5])
29 
30 X = points[0,:]
31 Y = points[1,:]
32 Z = points[2,:]
33 
34 nx = 20
35 x_grid = np.linspace(0.,1.,nx)
36 x_half_pad = 0.5*(x_grid[1] - x_grid[0])
37 x_bins = np.digitize(X, x_grid + x_half_pad)
38 x_dim = x_grid[-1] - x_grid[0]
39 
40 ny = 20
41 y_grid = np.linspace(0.,1.,ny)
42 y_half_pad = 0.5*(y_grid[1] - y_grid[0])
43 y_bins = np.digitize(Y, y_grid + y_half_pad)
44 y_dim = y_grid[-1] - y_grid[0]
45 
46 point_bins = y_bins * nx + x_bins
47 heights = np.zeros((ny, nx))
48 np.maximum.at(heights.ravel(), point_bins, Z)
49 
50 point_cloud = fcl.BVHModelOBBRSS()
51 point_cloud.beginModel(0, num_points)
52 point_cloud.addVertices(points.T)
53 
54 height_field = fcl.HeightFieldOBBRSS(x_dim, y_dim, heights, min(Z))
55 height_field_placement = point_cloud_placement * pin.SE3(np.eye(3), 0.5*np.array([x_grid[0] + x_grid[-1], y_grid[0] + y_grid[-1], 0.]))
56 
57 go_point_cloud = pin.GeometryObject("point_cloud",0,point_cloud,point_cloud_placement)
58 go_point_cloud.meshColor = np.ones((4))
59 collision_model.addGeometryObject(go_point_cloud)
60 visual_model.addGeometryObject(go_point_cloud)
61 
62 go_height_field = pin.GeometryObject("height_field",0,height_field,height_field_placement)
63 go_height_field.meshColor = np.ones((4))
64 height_field_collision_id = collision_model.addGeometryObject(go_height_field)
65 visual_model.addGeometryObject(go_height_field)
66 
67 # Add colllision pair between the height field and the panda_hand geometry
68 panda_hand_collision_id = collision_model.getGeometryId("panda_hand_0")
69 go_panda_hand = collision_model.geometryObjects[panda_hand_collision_id]
70 go_panda_hand.geometry.buildConvexRepresentation(False)
71 go_panda_hand.geometry = go_panda_hand.geometry.convex # We need to work with the convex hull of the real mesh
72 
73 collision_pair = pin.CollisionPair(height_field_collision_id, panda_hand_collision_id)
74 collision_model.addCollisionPair(collision_pair)
75 
76 viz = MeshcatVisualizer(model, collision_model, visual_model)
77 
78 # Start a new MeshCat server and client.
79 # Note: the server can also be started separately using the "meshcat-server" command in a terminal:
80 # this enables the server to remain active after the current script ends.
81 #
82 # Option open=True pens the visualizer.
83 # Note: the visualizer can also be opened seperately by visiting the provided URL.
84 try:
85  viz.initViewer(open=True)
86 except ImportError as err:
87  print("Error while initializing the viewer. It seems you should install Python meshcat")
88  print(err)
89  sys.exit(0)
90 
91 # Load the robot in the viewer.
92 viz.loadViewerModel()
93 
94 # Display a robot configuration.
95 q0 = pin.neutral(model)
96 viz.display(q0)
97 
98 is_collision = False
99 data = model.createData()
100 collision_data = collision_model.createData()
101 while not is_collision:
102  q = pin.randomConfiguration(model)
103 
104  is_collision = pin.computeCollisions(model, data, collision_model, collision_data, q, True)
105 
106 print("Found a configuration in collision:",q)
107 viz.display(q)
108 
109 
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
Definition: cholesky.hpp:71


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29