viewer.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2019 CNRS
4 # Author: Joseph Mirabel
5 
6 import warnings
7 
8 import numpy as np
9 from gepetto import Color
10 
11 import hppfcl
12 
13 
14 def applyConfiguration(gui, name, tf):
15  gui.applyConfiguration(
16  name, tf.getTranslation().tolist() + tf.getQuatRotation().coeffs().tolist()
17  )
18 
19 
20 def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0)):
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):
33  pts = [
34  geom.points(geom.polygons(f)[i]).tolist()
35  for f in range(geom.num_polygons)
36  for i in range(3)
37  ]
38  gui.addCurve(name, pts, color)
39  gui.setCurveMode(name, "TRIANGLES")
40  gui.setLightingMode(name, "ON")
41  gui.setBoolProperty(name, "BackfaceDrawing", True)
42  return 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")
48  return True
49  else:
50  msg = "Unsupported geometry type for %s (%s)" % (name, type(geom))
51  warnings.warn(msg, category=UserWarning, stacklevel=2)
52  return False
53 
54 
55 def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True):
56  gui.createGroup(group_name)
57  r = 0.01
58  if closest_points:
59  p = [group_name + "/p1", group_name + "/p2"]
60  gui.addSphere(p[0], r, Color.red)
61  gui.addSphere(p[1], r, Color.blue)
62  qid = [0, 0, 0, 1]
63  gui.applyConfigurations(
64  p,
65  [
66  res.getNearestPoint1().tolist() + qid,
67  res.getNearestPoint2().tolist() + qid,
68  ],
69  )
70  if normal:
71  n = group_name + "/normal"
72  gui.addArrow(n, r, 0.1, Color.green)
73  gui.applyConfiguration(
74  n,
75  res.getNearestPoint1().tolist()
76  + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), res.normal)
77  .coeffs()
78  .tolist(),
79  )
80  gui.refresh()
81 
82 
83 def displayCollisionResult(gui, group_name, res, color=Color.green):
84  if res.isCollision():
85  if gui.nodeExists(group_name):
86  gui.setVisibility(group_name, "ON")
87  else:
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
93  if gui.nodeExists(n):
94  gui.setFloatProperty(n, "Size", depth)
95  gui.setFloatProperty(n, "Radius", 0.1 * depth)
96  gui.setColor(n, color)
97  else:
98  gui.addArrow(n, depth * 0.1, depth, color)
99  N = contact.normal
100  P = contact.pos
101  gui.applyConfiguration(
102  n,
103  (P - depth * N / 2).tolist()
104  + hppfcl.Quaternion.FromTwoVectors(np.array([1, 0, 0]), N)
105  .coeffs()
106  .tolist(),
107  )
108  gui.refresh()
109  elif gui.nodeExists(group_name):
110  gui.setVisibility(group_name, "OFF")
hppfcl.viewer.applyConfiguration
def applyConfiguration(gui, name, tf)
Definition: viewer.py:14
doxygen_xml_parser.type
type
Definition: doxygen_xml_parser.py:882
hppfcl.viewer.displayCollisionResult
def displayCollisionResult(gui, group_name, res, color=Color.green)
Definition: viewer.py:83
hppfcl.viewer.displayShape
def displayShape(gui, name, geom, color=(0.9, 0.9, 0.9, 1.0))
Definition: viewer.py:20
hppfcl.viewer.displayDistanceResult
def displayDistanceResult(gui, group_name, res, closest_points=True, normal=True)
Definition: viewer.py:55
str
const char * str()
Definition: doxygen_xml_parser.py:882


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:15