def approximate_mesh(filename, lMg): mesh_loader = hppfcl.MeshLoader() mesh = mesh_loader.load(filename, np.ones(3)) vertices = np.array([ lMg * mesh.vertices(i) for i in range(mesh.num_vertices) ]) assert vertices.shape == (mesh.num_vertices, 3) a, b, r = capsule_approximation(vertices) return a, b, r
def __init__( self, urdfString=None, urdfFilename=None, fov=np.radians((60., 90.)), # fov = np.radians((49.5, 60)), geoms=["arm_3_link_0"], ): if isinstance(fov, str): self.fov = fov fov_fcl = hppfcl.MeshLoader().load( hpp.rostools.retrieve_resource(fov)) else: # build FOV tetahedron dx, dy = np.sin(fov) pts = hppfcl.StdVec_Vec3f() self.fov = [ (0, 0, 0), (dx, dy, 1), (-dx, dy, 1), (-dx, -dy, 1), (dx, -dy, 1), ] pts.extend([np.array(e) for e in self.fov]) self.fov.append(self.fov[1]) fov_fcl = hppfcl.BVHModelOBBRSS() fov_fcl.beginModel(4, 5) fov_fcl.addSubModel(pts, _tetahedron_tris) fov_fcl.endModel() self.cos_angle_thr = np.cos(np.radians(70)) if urdfFilename is None: assert urdfString is not None # Pinocchio does not allow to build a GeometryModel from a XML string. urdfFilename = '/tmp/tmp.urdf' with open(urdfFilename, 'w') as f: f.write(urdfString) self.model, self.gmodel = pinocchio.buildModelsFromUrdf( hpp.rostools.retrieve_resource(urdfFilename), root_joint=pinocchio.JointModelPlanar(), geometry_types=pinocchio.GeometryType.COLLISION) id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame") parent_frame = self.model.frames[id_parent_frame] go = pinocchio.GeometryObject("field_of_view", id_parent_frame, parent_frame.parent, fov_fcl, parent_frame.placement) self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model) for n in geoms: assert self.gmodel.existGeometryName(n) self.gmodel.addCollisionPair( pinocchio.CollisionPair(self.gmodel.getGeometryId(n), self.gid_field_of_view)) self.data = pinocchio.Data(self.model) self.gdata = pinocchio.GeometryData(self.gmodel)
def addDriller(self, mesh, frame_name, fMm): if not isinstance(fMm, pinocchio.SE3): fMm = pinocchio.XYZQUATToSE3(fMm) id_parent_frame = self.model.getFrameId(frame_name) parent_frame = self.model.frames[id_parent_frame] go = pinocchio.GeometryObject("driller", id_parent_frame, parent_frame.parent, hppfcl.MeshLoader().load(hpp.rostools.retrieve_resource(mesh)), parent_frame.placement * fMm) self.gmodel.addGeometryObject(go, self.model) self.gmodel.addCollisionPair(pinocchio.CollisionPair(self.gid_field_of_view, self.gmodel.ngeoms-1)) self.gdata = pinocchio.GeometryData(self.gmodel)
def generate_srdf(bvh_file, N, output): loader = hppfcl.MeshLoader () bvh = loader.load(bvh_file) handle = """ <handle name="{name}" clearance="{clearance}"> <position xyz="{xyz}" xyzw="{xyzw}"/> <link name="{link}" /> <mask>{mask}</mask> </handle>""" mask = "1 1 1 1 1 1" clearance = 0.01 link = "cylinder" output.write("""<robot name="cylinder">""") for ih in range(N): it = random.randint(0,bvh.num_tris-1) tri = bvh.tri_indices(it) ws = [ random.random() for _ in range(3) ] wt = sum(ws) ps = [ bvh.vertices(i) for i in tri ] p = sum((wi/wt*pi for wi,pi in zip(ws,ps))) x = -np.cross(ps[1]-ps[0], ps[2]-ps[0]) x /= np.linalg.norm(x) p -= 0.05*x quat = eigenpy.Quaternion.FromTwoVectors(np.array([1,0,0]), x) output.write(handle.format( name="handle_"+str(ih), clearance=clearance, xyz = " ".join([ str(v) for v in p ]), xyzw = " ".join([ str(quat[i]) for i in range(4) ]), link = link, mask=mask)) output.write("""</robot>""")