def _get_meshes_from_pyassimp_node(node, scale): meshes=[] tf = node.transformation # Match behavior of RViz for loading collada data # See rviz mesh_loader.cpp buildMesh() lines 227-236 pnode = node.parent while pnode is not None and hasattr(pnode, 'parent'): # Don't convert to y-up orientation, which is what the root node in # Assimp does if pnode.parent is not None and hasattr(pnode.parent, 'parent'): tf = np.dot(pnode.transformation, tf) pnode = pnode.parent for m in node.meshes: mesh = Mesh() for face in m.faces: triangle = MeshTriangle() triangle.vertex_indices = [face[0], face[1], face[2]] mesh.triangles.append(triangle) for vertex in m.vertices: point = Point() vertex_h=np.dot(tf,np.hstack((vertex, [1]))) point.x = vertex_h[0] * scale[0] point.y = vertex_h[1] * scale[1] point.z = vertex_h[2] * scale[2] mesh.vertices.append(point) meshes.append(mesh) for c in node.children: meshes.extend( _get_meshes_from_pyassimp_node(c,scale)) return meshes
def _make_mesh(self, name, path, scale, pos, quat=None, frame_id=None): if not use_pyassimp: raise RuntimeError('pyassimp is broken, cannot load meshes') scene = pyassimp.load(path) if not scene.meshes: pyassimp.release(scene) raise RuntimeError('Unable to load mesh "{}"'.format(path)) mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if isinstance(face, np.ndarray): indices = face.tolist() else: indices = face.indices if len(indices) == 3: triangle.vertex_indices = list(indices) mesh.triangles.append(triangle) 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) pyassimp.release(scene) return self._add_command(name, self._make_pose(pos, quat), mesh=mesh, frame_id=frame_id)
def makeMesh(self, name, pose, filename): if not use_pyassimp: rospy.logerr( 'pyassimp is broken on your platform, cannot load meshes') return scene = pyassimp.load(filename) if not scene.meshes: rospy.logerr('Unable to load mesh') return mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [ face.indices[0], face.indices[1], face.indices[2] ] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] point.y = vertex[1] point.z = vertex[2] mesh.vertices.append(point) pyassimp.release(scene) o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = self._fixed_frame o.id = name o.meshes.append(mesh) o.mesh_poses.append(pose) o.operation = o.ADD return o
def make_mesh(self, co, pose, filename, scale=(1.0, 1.0, 1.0)): try: scene = pyassimp.load(filename) except AssimpError as e: rospy.logerr("Assimp error: %s", e) return False if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [ face.indices[0], face.indices[1], face.indices[2] ] mesh.triangles.append(triangle) 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) if not isinstance(co.meshes, list): co.meshes = [] co.meshes += [mesh] if not isinstance(co.mesh_poses, list): co.mesh_poses = [] co.mesh_poses += [pose.pose] pyassimp.release(scene) return co
def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)): co = CollisionObject() scene = pyassimp.load(filename) if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]] mesh.triangles.append(triangle) 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] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def make_mesh(name, pose, filename, scale=(1, 1, 1)): ''' returns a CollisionObject with the mesh defined in the path. mostly copied from https://github.com/ros-planning/moveit_commander/blob/kinetic-devel/src/moveit_commander/planning_scene_interface.py ''' print("Creating mesh with file from", filename) co = CollisionObject() scene = pyassimp.load(filename) if not scene.meshes: raise MoveItCommanderException("There are no meshes in the file") co.operation = CollisionObject.ADD co.id = name co.header = pose.header mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face) == 3: triangle.vertex_indices = [face[0], face[1], face[2]] mesh.triangles.append(triangle) 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] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def stl_to_mesh(filename, scale=(1, 1, 1)): mesh = Mesh() scene = pyassimp.load(filename) 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) pyassimp.release(scene) return mesh
def __add_mesh(self, name, pose, filename, scale=(1, 1, 1)): """ This method is based on the __make_mesh() method of the moveit_commander.PlanningSceneInterface class :return: mesh: the mesh message corresponding to the mesh object """ co = moveit_msgs.msg.CollisionObject() # pyassimp scene loading if pyassimp is False: raise moveit_commander.exception.MoveItCommanderException( "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt" ) scene = pyassimp.load(filename) if not scene.meshes or len(scene.meshes) == 0: raise moveit_commander.exception.MoveItCommanderException( "There are no meshes in the file") if len(scene.meshes[0].faces) == 0: raise moveit_commander.exception.MoveItCommanderException( "There are no faces in the mesh") co.operation = moveit_msgs.msg.CollisionObject.ADD co.id = name co.header = pose.header # populating mesh message 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 moveit_commander.exception.MoveItCommanderException( "Unable to build triangles from mesh due to mesh object structure" ) for vertex in scene.meshes[0].vertices: point = geometry_msgs.msg.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] co.mesh_poses = [pose.pose] self.scene._pub_co.publish(co) return mesh
def make_mesh(self, co, pose, filename, scale=(1.0, 1.0, 1.0)): try: scene = pyassimp.load(filename) except AssimpError as e: rospy.logerr("Assimp error: %s", e) return False 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") 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) if not isinstance(co.meshes, list): co.meshes = [] co.meshes += [mesh] if not isinstance(co.mesh_poses, list): co.mesh_poses = [] co.mesh_poses += [pose.pose] pyassimp.release(scene) 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 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] co.mesh_poses = [pose.pose] pyassimp.release(scene) return co
def make_mesh(co, name, pose, filename, scale=(1, 1, 1)): #print("make_mesh(name=%s filename=%s)" % (name, filename)) 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 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) if not isinstance(co.meshes, list): co.meshes = [] co.meshes += [mesh] if not isinstance(co.mesh_poses, list): co.mesh_poses = [] co.mesh_poses += [pose.pose] pyassimp.release(scene) return co
def make_mesh(path, scale): """Load a mesh from disk and convert to a message. Arguments: path {str} -- path to the mesh file on disk scale {list} -- mesh scale x,y,z Returns: Mesh -- mesh message """ rospy.logdebug('Make mesh: %s, scale: %.3f,%.3f,%.3f', path, *scale) mesh = trimesh.load(path, force='mesh') return Mesh( vertices=[Point(*vertex) for vertex in mesh.vertices * scale], triangles=[ MeshTriangle(vertex_indices=indices) for indices in mesh.faces ])
def to_Mesh(klampt_mesh): """From a Klampt Geometry3D or TriangleMesh to a ROS Mesh""" from klampt import Geometry3D,TriangleMesh from shape_msgs.msg import MeshTriangle if isinstance(klampt_mesh,Geometry3D): mesh = klampt_mesh.getTriangleMesh() T = klampt_mesh.getCurrentTransform() if T != se3.identity(): mesh.transform(*T) klampt_mesh = mesh ros_mesh = Mesh() for i in range(len(klampt_mesh.vertices)//3): k = i*3 ros_mesh.vertices.append(Vector3(klampt_mesh.vertices[k],klampt_mesh.vertices[k+1],klampt_mesh.vertices[k+2])) for i in range(len(klampt_mesh.indices)//3): k = i*3 ros_mesh.triangles.append(MeshTriangle([klampt_mesh.indices[k],klampt_mesh.indices[k+1],klampt_mesh.indices[k+2]])) return ros_mesh
def to_shape_msgs_mesh(vertices, faces): """ Converts a set of vertices and faces to a ROS shape_msgs Mesh message """ mesh_msg = Mesh() for face in faces: triangle = MeshTriangle() triangle.vertex_indices = face mesh_msg.triangles.append(triangle) for vertex in vertices: pt = Point() pt.x = vertex[0] pt.y = vertex[1] pt.z = vertex[2] mesh_msg.vertices.append(pt) return mesh_msg
def create_mesh(name, pose, filename, scale=(1, 1, 1), frame_id='/world_frame'): co = CollisionObject() scene = pyassimp.load(str(exo.Tools.parsePath(filename))) if not scene.meshes or len(scene.meshes) == 0: raise Exception("There are no meshes in the file") if len(scene.meshes[0].faces) == 0: raise Exception("There are no faces in the mesh") co.operation = CollisionObject.ADD co.id = name co.header.frame_id = frame_id 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 Exception("Unable to build triangles from mesh") 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] co.mesh_poses = [pose] pyassimp.release(scene) return co
def makeMesh(self, name, ps, filename): """ Make a mesh collision object :param name: Name of the object :param ps: A pose stamped object pose message :param filename: The mesh file to load """ if not use_pyassimp: rospy.logerr("pyassimp is broken on your platform, cannot load meshes") return scene = pyassimp.load(filename) if not scene.meshes: rospy.logerr("Unable to load mesh") return mesh = Mesh() for face in scene.meshes[0].faces: triangle = MeshTriangle() if len(face.indices) == 3: triangle.vertex_indices = [ face.indices[0], face.indices[1], face.indices[2], ] mesh.triangles.append(triangle) for vertex in scene.meshes[0].vertices: point = Point() point.x = vertex[0] point.y = vertex[1] point.z = vertex[2] mesh.vertices.append(point) pyassimp.release(scene) o = CollisionObject() o.header.stamp = rospy.Time.now() o.header.frame_id = ps.header.frame_id o.id = name o.meshes.append(mesh) o.mesh_poses.append(ps.pose) o.operation = o.ADD return o
def import_hull(xml_file): e = xml.etree.ElementTree.parse(xml_file).getroot() mesh = Mesh() for vertice_entry in e.findall('vertice'): vertice = Point() vertice.x = float(vertice_entry.get('x')) vertice.y = float(vertice_entry.get('y')) vertice.z = float(vertice_entry.get('z')) mesh.vertices.append(vertice) for triangle_entry in e.findall('triangle'): triangle = MeshTriangle() triangle.vertex_indices[0] = int(triangle_entry.get('id0')) triangle.vertex_indices[1] = int(triangle_entry.get('id1')) triangle.vertex_indices[2] = int(triangle_entry.get('id2')) mesh.triangles.append(triangle) return mesh