def __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.pose = pose.pose
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
return co
def __make_sphere(name, pose, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.pose = pose.pose
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
return co
def __make_box(name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.pose = pose.pose
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
return co
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
)
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.pose = pose.pose
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, "__len__"):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, "indices"):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0],
face.indices[1],
face.indices[2],
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
pyassimp.release(scene)
return co
