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