planning_scene_utils.py
Go to the documentation of this file.
1 from moveit_msgs.msg import CollisionObject
2 from geometry_msgs.msg import Pose, Point
3 from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
4 import pyexotica as exo
5 
6 try:
7  from pyassimp import pyassimp
8 except:
9  try:
10  import pyassimp
11  except:
12  raise Exception("Failed to import pyassimp")
13 
14 
15 def create_pose(position, orientation, frame='/world_frame'):
16  pose = Pose()
17  pose.position.x = position[0]
18  pose.position.y = position[1]
19  pose.position.z = position[2]
20  pose.orientation.x = orientation[0]
21  pose.orientation.y = orientation[1]
22  pose.orientation.z = orientation[2]
23  pose.orientation.w = orientation[3]
24  return pose
25 
26 
27 def create_sphere(name, pose, radius, frame_id='/world_frame'):
28  co = CollisionObject()
29  co.operation = CollisionObject.ADD
30  co.id = name
31  co.header.frame_id = frame_id
32  sphere = SolidPrimitive()
33  sphere.type = SolidPrimitive.SPHERE
34  sphere.dimensions = [radius]
35  co.primitives = [sphere]
36  co.primitive_poses = [pose]
37  return co
38 
39 
40 def create_box(name, pose, size, frame_id='/world_frame'):
41  co = CollisionObject()
42  co.operation = CollisionObject.ADD
43  co.id = name
44  co.header.frame_id = frame_id
45  box = SolidPrimitive()
46  box.type = SolidPrimitive.BOX
47  box.dimensions = list(size)
48  co.primitives = [box]
49  co.primitive_poses = [pose]
50  return co
51 
52 
53 def create_mesh(name, pose, filename, scale=(1, 1, 1),
54  frame_id='/world_frame'):
55  co = CollisionObject()
56  scene = pyassimp.load(str(exo.Tools.parsePath(filename)))
57  if not scene.meshes or len(scene.meshes) == 0:
58  raise Exception("There are no meshes in the file")
59  if len(scene.meshes[0].faces) == 0:
60  raise Exception("There are no faces in the mesh")
61  co.operation = CollisionObject.ADD
62  co.id = name
63  co.header.frame_id = frame_id
64 
65  mesh = Mesh()
66  first_face = scene.meshes[0].faces[0]
67  if hasattr(first_face, '__len__'):
68  for face in scene.meshes[0].faces:
69  if len(face) == 3:
70  triangle = MeshTriangle()
71  triangle.vertex_indices = [face[0], face[1], face[2]]
72  mesh.triangles.append(triangle)
73  elif hasattr(first_face, 'indices'):
74  for face in scene.meshes[0].faces:
75  if len(face.indices) == 3:
76  triangle = MeshTriangle()
77  triangle.vertex_indices = [face.indices[0],
78  face.indices[1],
79  face.indices[2]]
80  mesh.triangles.append(triangle)
81  else:
82  raise Exception(
83  "Unable to build triangles from mesh")
84  for vertex in scene.meshes[0].vertices:
85  point = Point()
86  point.x = vertex[0]*scale[0]
87  point.y = vertex[1]*scale[1]
88  point.z = vertex[2]*scale[2]
89  mesh.vertices.append(point)
90  co.meshes = [mesh]
91  co.mesh_poses = [pose]
92  pyassimp.release(scene)
93  return co
94 
95 
96 def create_plane(name, pose, normal=(0, 0, 1), offset=0,
97  frame_id='/world_frame'):
98  co = CollisionObject()
99  co.operation = CollisionObject.ADD
100  co.id = name
101  co.header.frame_id = frame_id
102  p = Plane()
103  p.coef = list(normal)
104  p.coef.append(offset)
105  co.planes = [p]
106  co.plane_poses = [pose]
107  return co
def create_pose(position, orientation, frame='/world_frame')
def create_mesh(name, pose, filename, scale=(1, 1, 1), frame_id='/world_frame')
def create_plane(name, pose, normal=(0, 0, 1), offset=0, frame_id='/world_frame')
def create_sphere(name, pose, radius, frame_id='/world_frame')
def create_box(name, pose, size, frame_id='/world_frame')


exotica_python
Author(s):
autogenerated on Mon Feb 22 2021 03:33:27