2 Copyright (c) 2020 INRIA
3 Inspired from Antonio El Khoury PhD: https://tel.archives-ouvertes.fr/file/index/docid/833019/filename/thesis.pdf
4 Section 3.8.1 Computing minimum bounding capsules
14 a, b: the two extremities of the capsule segment
15 r: radius of the capsule
19 CONSTRAINT_INFLATION_RATIO = 5e-3
23 return np.linalg.norm(b - a) * np.pi * r**2 + 4 / 3 * np.pi * r**3
29 t = ap.dot(ab) / ab.dot(ab)
31 p_witness = a[
None, :] + (b - a)[
None, :] * t[:,
None]
32 dist = np.linalg.norm(p - p_witness, axis=1).
max()
37 mean = vertices.mean(axis=0)
39 u, s, vh = np.linalg.svd(vertices, full_matrices=
True)
41 pca_proj = vertices.dot(components.T)
44 a0 = mean + components[0] * (pca_proj[:, 0].
min() - EPSILON)
45 b0 = mean + components[0] * (pca_proj[:, 0].
max() + EPSILON)
46 radius = np.linalg.norm(pca_proj[:, 1:], axis=1).
max()
52 constraint_inflation = CONSTRAINT_INFLATION_RATIO * r0
53 x0 = np.array(list(a0) + list(b0) + [r0])
56 constraint = optimize.NonlinearConstraint(
57 constraint_cap, lb=-np.inf, ub=-constraint_inflation
59 res = optimize.minimize(capsule_vol, x0, constraints=constraint)
60 res_constraint = constraint_cap(res.x)
62 res_constraint <= 1e-4
63 ),
"The computed solution is invalid, a vertex is at a distance {:.5f} of the capsule.".format(
66 a, b, r = res.x[:3], res.x[3:6], res.x[6]
71 mesh_loader = hppfcl.MeshLoader()
72 mesh = mesh_loader.load(filename, np.ones(3))
73 vertices = np.array([lMg * mesh.vertices(i)
for i
in range(mesh.num_vertices)])
74 assert vertices.shape == (mesh.num_vertices, 3)
80 from lxml
import etree
82 tree = etree.parse(infile)
85 if fn.startswith(
"package://"):
86 relpath = fn[len(
"package://") :]
89 for rospath
in os.environ[
"ROS_PACKAGE_PATH"].split(
":"):
90 abspath = os.path.join(rospath, relpath)
91 if os.path.isfile(abspath):
93 raise ValueError(
"Could not find " + fn)
96 def get_transform(origin):
97 from pinocchio
import SE3, rpy
99 _xyz = [float(v)
for v
in origin.attrib.get(
"xyz",
"0 0 0").split(
" ")]
100 _rpy = [float(v)
for v
in origin.attrib.get(
"rpy",
"0 0 0").split(
" ")]
101 return SE3(rpy.rpyToMatrix(*_rpy), np.array(_xyz))
103 def set_transform(origin, a, b):
104 from pinocchio
import rpy, Quaternion
106 length = np.linalg.norm(b - a)
108 R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix()
109 origin.attrib[
"xyz"] =
" ".join([str(v)
for v
in ((a + b) / 2)])
110 origin.attrib[
"rpy"] =
" ".join([str(v)
for v
in rpy.matrixToRpy(R)])
112 from tqdm
import tqdm
115 tree.xpath(
"/robot/link/collision/geometry/mesh"), desc=
"Generating capsules"
117 geom = mesh.getparent()
118 coll = geom.getparent()
119 link = coll.getparent()
120 if coll.find(
"origin")
is None:
121 o = etree.Element(
"origin")
124 origin = coll.find(
"origin")
125 lMg = get_transform(origin)
127 meshfile = get_path(mesh.attrib[
"filename"])
130 name = os.path.basename(meshfile)
133 length = np.linalg.norm(b - a)
135 set_transform(origin, a, b)
137 mesh.tag =
"cylinder"
138 mesh.attrib.pop(
"filename")
139 mesh.attrib[
"radius"] = str(radius)
140 mesh.attrib[
"length"] = str(length)
141 coll.attrib[
"name"] = name
143 if link.find(
"collision_checking")
is None:
144 link.append(etree.Element(
"collision_checking"))
145 collision_checking = link.find(
"collision_checking")
146 collision_checking.append(etree.Element(
"capsule"))
147 collision_checking[-1].attrib[
"name"] = name
152 if __name__ ==
"__main__":
162 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
165 +
"models/example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf"
167 parse_urdf(urdf_filename,
"ur5_gripper_with_capsules.urdf")