panda3d_visualizer.py
Go to the documentation of this file.
1 import warnings
2 
3 from .. import pinocchio_pywrap as pin
4 from ..utils import npToTuple
5 from .base_visualizer import BaseVisualizer
6 
7 try:
8  import hppfcl
9  WITH_HPP_FCL_BINDINGS = True
10 except ImportError:
11  WITH_HPP_FCL_BINDINGS = False
12 
14  """
15  A Pinocchio display using panda3d engine.
16  """
17 
18  def initViewer(self, viewer=None, load_model=False): # pylint: disable=arguments-differ
19  """Init the viewer by attaching to / creating a GUI viewer."""
20  self.visual_group = None
21  self.collision_group = None
22  self.display_visuals = False
23  self.display_collisions = False
24  self.viewer = viewer
25 
26  from panda3d_viewer import Viewer as Panda3dViewer
27  if viewer is None:
28  self.viewer = Panda3dViewer(window_title="python-pinocchio")
29 
30  if load_model:
31  self.loadViewerModel(group_name=self.model.name)
32 
33  def loadViewerModel(self, group_name, color=None): # pylint: disable=arguments-differ
34  """Create a group of nodes displaying the robot meshes in the viewer."""
35  self.visual_group = group_name + "/visuals"
36  self.collision_group = group_name + "/collisions"
37 
38  self.viewer.append_group(self.visual_group)
39  self.viewer.append_group(self.collision_group)
40 
41  def append(root, obj):
42  geom = obj.geometry
43  if WITH_HPP_FCL_BINDINGS and isinstance(geom, hppfcl.ShapeBase):
44  # append a primitive geometry
45  if isinstance(geom, hppfcl.Capsule):
46  r, l = geom.radius, 2 * geom.halfLength
47  self.viewer.append_capsule(root, obj.name, r, l)
48  elif isinstance(geom, hppfcl.Cylinder):
49  r, l = geom.radius, 2 * geom.halfLength
50  self.viewer.append_cylinder(root, obj.name, r, l)
51  elif isinstance(geom, hppfcl.Box):
52  size = npToTuple(2. * geom.halfSide)
53  self.viewer.append_box(root, obj.name, size)
54  elif isinstance(geom, hppfcl.Sphere):
55  self.viewer.append_sphere(root, obj.name, geom.radius)
56  else:
57  msg = "Unsupported geometry type for %s (%s)" % (
58  obj.name, type(geom))
59  warnings.warn(msg, category=UserWarning, stacklevel=2)
60  return
61  else:
62  # append a mesh
63  scale = npToTuple(obj.meshScale)
64  self.viewer.append_mesh(root, obj.name, obj.meshPath, scale)
65 
66  if obj.overrideMaterial:
67  rgba = npToTuple(obj.meshColor)
68  path = obj.meshTexturePath
69  self.viewer.set_material(root, obj.name, rgba, path)
70  elif color is not None:
71  self.viewer.set_material(root, obj.name, color)
72 
73  self.displayVisuals(False)
74  self.displayCollisions(False)
75 
76  for obj in self.visual_model.geometryObjects:
77  append(self.visual_group, obj)
78 
79  for obj in self.collision_model.geometryObjects:
80  append(self.collision_group, obj)
81 
82  self.displayVisuals(True)
83 
84  def getViewerNodeName(self, geometry_object, geometry_type):
85  """Return the name of the geometry object inside the viewer."""
86  if geometry_type is pin.GeometryType.VISUAL:
87  return self.visual_group + '/' + geometry_object.name
88  elif geometry_type is pin.GeometryType.COLLISION:
89  return self.collision_group + '/' + geometry_object.name
90 
91  def display(self, q = None):
92  """Display the robot at configuration q in the viewer by placing all the bodies."""
93  if q is not None:
94  pin.forwardKinematics(self.model, self.data, q)
95 
96  def move(group, model, data):
97  pin.updateGeometryPlacements(self.model, self.data, model, data)
98  name_pose_dict = {}
99  for obj in model.geometryObjects:
100  oMg = data.oMg[model.getGeometryId(obj.name)]
101  x, y, z, qx, qy, qz, qw = pin.SE3ToXYZQUATtuple(oMg)
102  name_pose_dict[obj.name] = (x, y, z), (qw, qx, qy, qz)
103  self.viewer.move_nodes(group, name_pose_dict)
104 
105  if self.display_visuals:
106  move(self.visual_group, self.visual_model, self.visual_data)
107 
108  if self.display_collisions:
109  move(self.collision_group, self.collision_model,
110  self.collision_data)
111 
112  def displayCollisions(self, visibility):
113  """Set whether to display collision objects or not."""
114  self.viewer.show_group(self.collision_group, visibility)
115  self.display_collisions = visibility
116 
117  def displayVisuals(self, visibility):
118  """Set whether to display visual objects or not."""
119  self.viewer.show_group(self.visual_group, visibility)
120  self.display_visuals = visibility
121 
122 __all__ = ['Panda3dVisualizer']
result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Definition: fusion.hpp:24
def getViewerNodeName(self, geometry_object, geometry_type)
def initViewer(self, viewer=None, load_model=False)


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