capsule-approximation.py
Go to the documentation of this file.
1 """
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
5 """
6 
7 import numpy as np
8 import scipy.optimize as optimize
9 
10 import hppfcl
11 
12 """
13 Capsule definition
14 a, b: the two extremities of the capsule segment
15 r: radius of the capsule
16 """
17 
18 EPSILON = 1e-8
19 CONSTRAINT_INFLATION_RATIO = 5e-3
20 
21 
22 def capsule_volume(a, b, r):
23  return np.linalg.norm(b - a) * np.pi * r ** 2 + 4 / 3 * np.pi * r ** 3
24 
25 
27  ap = p - a
28  ab = b - a
29  t = ap.dot(ab) / ab.dot(ab)
30  t = np.clip(t, 0, 1)
31  p_witness = a[None, :] + (b - a)[None, :] * t[:, None]
32  dist = np.linalg.norm(p - p_witness, axis=1).max()
33  return dist
34 
35 
36 def pca_approximation(vertices):
37  mean = vertices.mean(axis=0)
38  vertices -= mean
39  u, s, vh = np.linalg.svd(vertices, full_matrices=True)
40  components = vh
41  pca_proj = vertices.dot(components.T)
42  vertices += mean
43 
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()
47  return a0, b0, radius
48 
49 
50 def capsule_approximation(vertices):
51  a0, b0, r0 = pca_approximation(vertices)
52  constraint_inflation = CONSTRAINT_INFLATION_RATIO * r0
53  x0 = np.array(list(a0) + list(b0) + [r0])
54  constraint_cap = lambda x: distance_points_segment(vertices, x[:3], x[3:6]) - x[6]
55  capsule_vol = lambda x: capsule_volume(x[:3], x[3:6], x[6])
56  constraint = optimize.NonlinearConstraint(
57  constraint_cap, lb=-np.inf, ub=-constraint_inflation
58  )
59  res = optimize.minimize(capsule_vol, x0, constraints=constraint)
60  res_constraint = constraint_cap(res.x)
61  assert (
62  res_constraint <= 1e-4
63  ), "The computed solution is invalid, a vertex is at a distance {:.5f} of the capsule.".format(
64  res_constraint
65  )
66  a, b, r = res.x[:3], res.x[3:6], res.x[6]
67  return a, b, r
68 
69 def approximate_mesh(filename, lMg):
70  mesh_loader = hppfcl.MeshLoader()
71  mesh = mesh_loader.load(filename, np.ones(3))
72  vertices = np.array([ lMg * mesh.vertices(i) for i in range(mesh.num_vertices) ])
73  assert vertices.shape == (mesh.num_vertices, 3)
74  a, b, r = capsule_approximation(vertices)
75  return a, b, r
76 
77 def parse_urdf(infile, outfile):
78  from lxml import etree
79 
80  tree = etree.parse(infile)
81 
82  def get_path(fn):
83  if fn.startswith('package://'):
84  relpath = fn[len('package://'):]
85  import os
86  for rospath in os.environ['ROS_PACKAGE_PATH'].split(':'):
87  abspath = os.path.join(rospath, relpath)
88  if os.path.isfile(abspath):
89  return abspath
90  raise ValueError("Could not find " + fn)
91  return fn
92 
93  def get_transform(origin):
94  from pinocchio import SE3, rpy
95  _xyz = [ float(v) for v in origin.attrib.get('xyz', '0 0 0').split(' ') ]
96  _rpy = [ float(v) for v in origin.attrib.get('rpy', '0 0 0').split(' ') ]
97  return SE3 (rpy.rpyToMatrix(*_rpy), np.array(_xyz))
98 
99  def set_transform(origin, a, b):
100  from pinocchio import rpy, Quaternion
101  length = np.linalg.norm(b-a)
102  z = (b - a) / length
103  R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix()
104  origin.attrib['xyz'] = " ".join([str(v) for v in ((a+b)/2) ])
105  origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R) ])
106 
107  from tqdm import tqdm
108  for mesh in tqdm(tree.xpath('/robot/link/collision/geometry/mesh'), desc="Generating capsules"):
109  geom = mesh.getparent()
110  coll = geom.getparent()
111  link = coll.getparent()
112  if coll.find('origin') is None:
113  o = etree.Element("origin")
114  o.tail = geom.tail
115  coll.insert(0, o)
116  origin = coll.find('origin')
117  lMg = get_transform(origin)
118 
119  meshfile = get_path(mesh.attrib['filename'])
120  import os
121  name = os.path.basename(meshfile)
122  # Generate capsule
123  a, b, radius = approximate_mesh (meshfile, lMg)
124  length = np.linalg.norm(b-a)
125 
126  set_transform(origin, a, b)
127 
128  mesh.tag = "cylinder"
129  mesh.attrib.pop('filename')
130  mesh.attrib['radius'] = str(radius)
131  mesh.attrib['length'] = str(length)
132  coll.attrib['name'] = name
133 
134  if link.find('collision_checking') is None:
135  link.append(etree.Element('collision_checking'))
136  collision_checking = link.find('collision_checking')
137  collision_checking.append(etree.Element('capsule'))
138  collision_checking[-1].attrib['name'] = name
139 
140  tree.write(outfile)
141 
142 if __name__ == "__main__":
143  # Example for a single capsule
144  #filename = "mesh.obj"
145  #mesh_loader = hppfcl.MeshLoader()
146  #mesh = mesh_loader.load(filename, np.ones(3))
147  #vertices = mesh.vertices()
148  #a, b, r = capsule_approximation(vertices)
149 
150  # Example for a whole URDF model
151  # This path refers to Pinocchio source code but you can define your own directory here.
152  pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
153  urdf_filename = pinocchio_model_dir + "models/example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf"
154  parse_urdf(urdf_filename, "ur5_gripper_with_capsules.urdf")
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
Definition: cholesky.hpp:71
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
def approximate_mesh(filename, lMg)
def parse_urdf(infile, outfile)
def distance_points_segment(p, a, b)
def capsule_approximation(vertices)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28