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>""")
