1 from ..
import pinocchio_pywrap_default
as pin
2 from ..shortcuts
import buildModelsFromUrdf, createDatas
3 from ..utils
import npToTuple
6 from numpy.linalg
import norm
8 from .
import BaseVisualizer
15 WITH_HPP_FCL_BINDINGS =
True
17 WITH_HPP_FCL_BINDINGS =
False
21 """A Pinocchio display using Gepetto Viewer"""
24 """Return the name of the geometry object inside the viewer"""
25 if geometry_type
is pin.GeometryType.VISUAL:
27 elif geometry_type
is pin.GeometryType.COLLISION:
33 windowName="python-pinocchio",
37 """Init GepettoViewer by loading the gui and creating a window."""
40 import gepetto.corbaserver
45 "Error while importing the viewer client.\n"
46 "Check whether gepetto-gui is properly installed"
48 warnings.warn(msg, category=UserWarning, stacklevel=2)
51 self.
viewer = gepetto.corbaserver.Client()
if viewer
is None else viewer
55 window_l = gui.getWindowList()
56 if not windowName
in window_l:
62 scene_l = gui.getSceneList()
63 if sceneName
not in scene_l:
64 gui.createScene(sceneName)
66 gui.addSceneToWindow(sceneName, self.
windowID)
74 "Error while starting the viewer client.\n"
75 "Check whether gepetto-viewer is properly started"
77 warnings.warn(msg, category=UserWarning, stacklevel=2)
82 meshColor = geometry_object.meshColor
84 geom = geometry_object.geometry
85 if isinstance(geom, hppfcl.Capsule):
86 return gui.addCapsule(
87 meshName, geom.radius, 2.0 * geom.halfLength,
npToTuple(meshColor)
89 elif isinstance(geom, hppfcl.Cylinder):
90 return gui.addCylinder(
91 meshName, geom.radius, 2.0 * geom.halfLength,
npToTuple(meshColor)
93 elif isinstance(geom, hppfcl.Box):
95 return gui.addBox(meshName, w, h, d,
npToTuple(meshColor))
96 elif isinstance(geom, hppfcl.Sphere):
97 return gui.addSphere(meshName, geom.radius,
npToTuple(meshColor))
98 elif isinstance(geom, hppfcl.Cone):
100 meshName, geom.radius, 2.0 * geom.halfLength,
npToTuple(meshColor)
102 elif isinstance(geom, hppfcl.Plane)
or isinstance(geom, hppfcl.Halfspace):
103 res = gui.createGroup(meshName)
106 planeName = meshName +
"/plane"
107 res = gui.addFloor(planeName)
111 rot = pin.Quaternion.FromTwoVectors(normal, pin.ZAxis)
112 alpha = geom.d / norm(normal, 2) ** 2
113 trans = alpha * normal
114 plane_offset = pin.SE3(rot, trans)
115 gui.applyConfiguration(planeName, pin.SE3ToXYZQUATtuple(plane_offset))
116 elif isinstance(geom, hppfcl.Convex):
118 npToTuple(geom.points(geom.polygons(f)[i]))
119 for f
in range(geom.num_polygons)
122 gui.addCurve(meshName, pts,
npToTuple(meshColor))
123 gui.setCurveMode(meshName,
"TRIANGLES")
124 gui.setLightingMode(meshName,
"ON")
125 gui.setBoolProperty(meshName,
"BackfaceDrawing",
True)
127 elif isinstance(geom, hppfcl.ConvexBase):
128 pts = [
npToTuple(geom.points(i))
for i
in range(geom.num_points)]
129 gui.addCurve(meshName, pts,
npToTuple(meshColor))
130 gui.setCurveMode(meshName,
"POINTS")
131 gui.setLightingMode(meshName,
"OFF")
134 msg =
"Unsupported geometry type for %s (%s)" % (
135 geometry_object.name,
138 warnings.warn(msg, category=UserWarning, stacklevel=2)
142 """Load a single geometry object"""
147 meshPath = geometry_object.meshPath
148 meshTexturePath = geometry_object.meshTexturePath
149 meshScale = geometry_object.meshScale
150 meshColor = geometry_object.meshColor
153 if WITH_HPP_FCL_BINDINGS
and isinstance(
154 geometry_object.geometry, hppfcl.ShapeBase
159 msg =
"Display of geometric primitives is supported only if pinocchio is build with HPP-FCL bindings."
160 warnings.warn(msg, category=UserWarning, stacklevel=2)
162 success = gui.addMesh(meshName, meshPath)
165 except Exception
as e:
166 msg =
"Error while loading geometry object: %s\nError message:\n%s" % (
167 geometry_object.name,
170 warnings.warn(msg, category=UserWarning, stacklevel=2)
173 gui.setScale(meshName,
npToTuple(meshScale))
174 if geometry_object.overrideMaterial:
175 gui.setColor(meshName,
npToTuple(meshColor))
176 if meshTexturePath !=
"":
177 gui.setTexture(meshName, meshTexturePath)
180 """Create the scene displaying the robot meshes in gepetto-viewer"""
198 if self.collision_model
is not None:
199 for collision
in self.collision_model.geometryObjects:
203 self.collision_model
is not None and self.visual_model
is None
206 if self.visual_model
is not None:
207 for visual
in self.visual_model.geometryObjects:
215 """Display the robot at configuration q in the viewer by placing all the bodies."""
216 if "viewer" not in self.__dict__:
222 pin.forwardKinematics(self.model, self.data, q)
225 pin.updateGeometryPlacements(
226 self.model, self.data, self.collision_model, self.collision_data
228 gui.applyConfigurations(
231 for collision
in self.collision_model.geometryObjects
234 pin.SE3ToXYZQUATtuple(
235 self.collision_data.oMg[
236 self.collision_model.getGeometryId(collision.name)
239 for collision
in self.collision_model.geometryObjects
244 pin.updateGeometryPlacements(
245 self.model, self.data, self.visual_model, self.visual_data
247 gui.applyConfigurations(
250 for visual
in self.visual_model.geometryObjects
253 pin.SE3ToXYZQUATtuple(
254 self.visual_data.oMg[
255 self.visual_model.getGeometryId(visual.name)
258 for visual
in self.visual_model.geometryObjects
265 """Set whether to display collision objects or not"""
268 if self.collision_model
is None:
272 visibility_mode =
"ON"
274 visibility_mode =
"OFF"
276 for collision
in self.collision_model.geometryObjects:
278 gui.setVisibility(nodeName, visibility_mode)
281 """Set whether to display visual objects or not"""
284 if self.visual_model
is None:
288 visibility_mode =
"ON"
290 visibility_mode =
"OFF"
292 for visual
in self.visual_model.geometryObjects:
294 gui.setVisibility(nodeName, visibility_mode)
297 raise NotImplementedError()
300 raise NotImplementedError()
303 raise NotImplementedError()
306 raise NotImplementedError()
309 raise NotImplementedError()
312 raise NotImplementedError()
315 raise NotImplementedError()
318 raise NotImplementedError()
321 raise NotImplementedError()
324 __all__ = [
"GepettoVisualizer"]