9 from gepetto
import Color
15 gui.applyConfiguration(
16 name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()
21 if isinstance(geom, hppfcl.Capsule):
22 return gui.addCapsule(name, geom.radius, 2.0 * geom.halfLength, color)
23 elif isinstance(geom, hppfcl.Cylinder):
24 return gui.addCylinder(name, geom.radius, 2.0 * geom.halfLength, color)
25 elif isinstance(geom, hppfcl.Box):
26 w, h, d = (2.0 * geom.halfSide).tolist()
27 return gui.addBox(name, w, h, d, color)
28 elif isinstance(geom, hppfcl.Sphere):
29 return gui.addSphere(name, geom.radius, color)
30 elif isinstance(geom, hppfcl.Cone):
31 return gui.addCone(name, geom.radius, 2.0 * geom.halfLength, color)
32 elif isinstance(geom, hppfcl.Convex):
34 geom.points(geom.polygons(f)[i]).tolist()
35 for f
in range(geom.num_polygons)
38 gui.addCurve(name, pts, color)
39 gui.setCurveMode(name,
"TRIANGLES")
40 gui.setLightingMode(name,
"ON")
41 gui.setBoolProperty(name,
"BackfaceDrawing",
True)
43 elif isinstance(geom, hppfcl.ConvexBase):
44 pts = [geom.points(i).tolist()
for i
in range(geom.num_points)]
45 gui.addCurve(name, pts, color)
46 gui.setCurveMode(name,
"POINTS")
47 gui.setLightingMode(name,
"OFF")
50 msg =
"Unsupported geometry type for %s (%s)" % (name,
type(geom))
51 warnings.warn(msg, category=UserWarning, stacklevel=2)
56 gui.createGroup(group_name)
59 p = [group_name +
"/p1", group_name +
"/p2"]
60 gui.addSphere(p[0], r, Color.red)
61 gui.addSphere(p[1], r, Color.blue)
63 gui.applyConfigurations(
66 res.getNearestPoint1().tolist() + qid,
67 res.getNearestPoint2().tolist() + qid,
71 n = group_name +
"/normal"
72 gui.addArrow(n, r, 0.1, Color.green)
73 gui.applyConfiguration(
75 res.getNearestPoint1().tolist()
76 + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal)
85 if gui.nodeExists(group_name):
86 gui.setVisibility(group_name,
"ON")
88 gui.createGroup(group_name)
89 for i
in range(res.numContacts()):
90 contact = res.getContact(i)
91 n = group_name +
"/contact" +
str(i)
92 depth = contact.penetration_depth
94 gui.setFloatProperty(n,
"Size", depth)
95 gui.setFloatProperty(n,
"Radius", 0.1 * depth)
96 gui.setColor(n, color)
98 gui.addArrow(n, depth * 0.1, depth, color)
101 gui.applyConfiguration(
103 (P - depth * N / 2).tolist()
104 + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N)
109 elif gui.nodeExists(group_name):
110 gui.setVisibility(group_name,
"OFF")