3 from ..
import pinocchio_pywrap_default
as pin
4 from ..utils
import npToTuple
5 from .base_visualizer
import BaseVisualizer
12 WITH_HPP_FCL_BINDINGS =
True
14 WITH_HPP_FCL_BINDINGS =
False
19 A Pinocchio display using panda3d engine.
23 """Init the viewer by attaching to / creating a GUI viewer."""
30 from panda3d_viewer
import Viewer
as Panda3dViewer
33 self.
viewer = Panda3dViewer(window_title=
"python-pinocchio")
39 """Create a group of nodes displaying the robot meshes in the viewer."""
48 if WITH_HPP_FCL_BINDINGS
and isinstance(geom, hppfcl.ShapeBase):
50 if isinstance(geom, hppfcl.Capsule):
51 r, l = geom.radius, 2 * geom.halfLength
52 self.
viewer.append_capsule(root, obj.name, r, l)
53 elif isinstance(geom, hppfcl.Cylinder):
54 r, l = geom.radius, 2 * geom.halfLength
55 self.
viewer.append_cylinder(root, obj.name, r, l)
56 elif isinstance(geom, hppfcl.Box):
58 self.
viewer.append_box(root, obj.name, size)
59 elif isinstance(geom, hppfcl.Sphere):
60 self.
viewer.append_sphere(root, obj.name, geom.radius)
62 msg =
"Unsupported geometry type for %s (%s)" % (
66 warnings.warn(msg, category=UserWarning, stacklevel=2)
71 self.
viewer.append_mesh(root, obj.name, obj.meshPath, scale)
73 if obj.overrideMaterial:
75 path = obj.meshTexturePath
76 self.
viewer.set_material(root, obj.name, rgba, path)
77 elif color
is not None:
78 self.
viewer.set_material(root, obj.name, color)
92 """Return the name of the geometry object inside the viewer."""
93 if geometry_type
is pin.GeometryType.VISUAL:
95 elif geometry_type
is pin.GeometryType.COLLISION:
99 """Display the robot at configuration q in the viewer by placing all the bodies."""
101 pin.forwardKinematics(self.
model, self.
data, q)
103 def move(group, model, data):
104 pin.updateGeometryPlacements(self.
model, self.
data, model, data)
106 for obj
in model.geometryObjects:
107 oMg = data.oMg[model.getGeometryId(obj.name)]
108 x, y, z, qx, qy, qz, qw = pin.SE3ToXYZQUATtuple(oMg)
109 name_pose_dict[obj.name] = (x, y, z), (qw, qx, qy, qz)
110 self.
viewer.move_nodes(group, name_pose_dict)
119 """Set whether to display collision objects or not."""
124 """Set whether to display visual objects or not."""
129 raise NotImplementedError()
132 raise NotImplementedError()
135 raise NotImplementedError()
138 raise NotImplementedError()
141 raise NotImplementedError()
144 raise NotImplementedError()
147 raise NotImplementedError()
150 raise NotImplementedError()
153 raise NotImplementedError()
156 __all__ = [
"Panda3dVisualizer"]