6 import hppfcl, numpy
as np
7 from gepetto
import Color
11 gui.applyConfiguration(
12 name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()
17 if isinstance(geom, hppfcl.Capsule):
18 return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color)
19 elif isinstance(geom, hppfcl.Cylinder):
20 return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color)
21 elif isinstance(geom, hppfcl.Box):
22 w, h, d = (2.0 * geom.halfSide).tolist()
23 return gui.addBox(name, w, h, d, color)
24 elif isinstance(geom, hppfcl.Sphere):
25 return gui.addSphere(name, geom.radius, color)
26 elif isinstance(geom, hppfcl.Cone):
27 return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color)
28 elif isinstance(geom, hppfcl.Convex):
30 geom.points(geom.polygons(f)[i]).tolist()
31 for f
in range(geom.num_polygons)
34 gui.addCurve(name, pts, color)
35 gui.setCurveMode(name,
"TRIANGLES")
36 gui.setLightingMode(name,
"ON")
37 gui.setBoolProperty(name,
"BackfaceDrawing",
True)
39 elif isinstance(geom, hppfcl.ConvexBase):
40 pts = [geom.points(i).tolist()
for i
in range(geom.num_points)]
41 gui.addCurve(name, pts, color)
42 gui.setCurveMode(name,
"POINTS")
43 gui.setLightingMode(name,
"OFF")
46 msg =
"Unsupported geometry type for %s (%s)" % (
50 warnings.warn(msg, category=UserWarning, stacklevel=2)
55 gui.createGroup(group_name)
58 p = [group_name +
"/p1", group_name +
"/p2"]
59 gui.addSphere(p[0], r, Color.red)
60 gui.addSphere(p[1], r, Color.blue)
62 gui.applyConfigurations(
65 res.getNearestPoint1().tolist() + qid,
66 res.getNearestPoint2().tolist() + qid,
70 n = group_name +
"/normal" 71 gui.addArrow(n, r, 0.1, Color.green)
72 gui.applyConfiguration(
74 res.getNearestPoint1().tolist()
75 + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal)
84 if gui.nodeExists(group_name):
85 gui.setVisibility(group_name,
"ON")
87 gui.createGroup(group_name)
88 for i
in range(res.numContacts()):
89 contact = res.getContact(i)
90 n = group_name +
"/contact" +
str(i)
91 depth = contact.penetration_depth
93 gui.setFloatProperty(n,
"Size", depth)
94 gui.setFloatProperty(n,
"Radius", 0.1 * depth)
95 gui.setColor(n, color)
97 gui.addArrow(n, depth * 0.1, depth, color)
100 gui.applyConfiguration(
102 (P - depth * N / 2).tolist()
103 + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N)
108 elif gui.nodeExists(group_name):
109 gui.setVisibility(group_name,
"OFF")
def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0))
def applyConfiguration(gui, name, tf)
def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True)
def displayCollisionResult(gui, group_name, res, color=Color.green)