1 from ..
import pinocchio_pywrap_default
as pin
2 from ..utils
import npToTuple
4 from .
import BaseVisualizer
11 WITH_HPP_FCL_BINDINGS =
True
13 WITH_HPP_FCL_BINDINGS =
False
17 """Make capsule using two sphere and one cylinder"""
18 from copy
import deepcopy
19 from visualization_msgs.msg
import Marker
20 from geometry_msgs.msg
import Point
22 displacment = pin.SE3.Identity()
24 displacment.translation[2] = l / 2.0
25 oMsphere_1 = oMg * displacment
26 displacment.translation[2] = -l / 2.0
27 oMsphere_2 = oMg * displacment
29 marker_cylinder = marker_ref
30 marker_cylinder.type = Marker.CYLINDER
31 marker_cylinder.scale = Point(d, d, l)
34 marker_sphere_1 = deepcopy(marker_ref)
35 marker_sphere_1.id += 10000
36 marker_sphere_1.type = Marker.SPHERE
37 marker_sphere_1.scale = Point(d, d, d)
40 marker_sphere_2 = deepcopy(marker_ref)
41 marker_sphere_2.id += 20000
42 marker_sphere_2.type = Marker.SPHERE
43 marker_sphere_2.scale = Point(d, d, d)
46 return [marker_cylinder, marker_sphere_1, marker_sphere_2]
50 """Converts SE3 matrix to ROS geometry_msgs/Pose format"""
51 from geometry_msgs.msg
import Pose, Point, Quaternion
53 xyz_quat = pin.SE3ToXYZQUATtuple(oMg)
54 return Pose(position=Point(*xyz_quat[:3]), orientation=
Quaternion(*xyz_quat[3:]))
58 """A Pinocchio display using RViz"""
68 windowName="python-pinocchio",
72 """Init RVizViewer by starting a ros node (or not) and creating an RViz window."""
73 from rospy
import init_node, WARN
74 from rosgraph
import is_master_online
75 from rviz
import bindings
as rviz
76 from python_qt_binding.QtWidgets
import QApplication
78 if not is_master_online():
81 "Error while importing the viz client.\n"
82 "Check whether ROS master (roscore) is properly started",
89 init_node(
"pinocchio_viewer", anonymous=
True, log_level=WARN)
93 self.
viewer.app = QApplication([])
94 self.
viewer.viz = rviz.VisualizationFrame()
95 self.
viewer.viz.setSplashPath(
"")
96 self.
viewer.viz.initialize()
97 self.
viewer.viz.setWindowTitle(windowName +
"[*]")
109 """Create the displays in RViz and create publishers for the MarkerArray"""
110 from rospy
import Publisher
111 from visualization_msgs.msg
import MarkerArray
115 rootNodeName +
"_visuals", MarkerArray, queue_size=1, latch=
True
118 "rviz/MarkerArray", rootNodeName +
"_visuals",
True
120 self.
visual_Display.subProp(
"Marker Topic").setValue(rootNodeName +
"_visuals")
125 rootNodeName +
"_collisions", MarkerArray, queue_size=1, latch=
True
128 "rviz/MarkerArray", rootNodeName +
"_collisions",
True
131 rootNodeName +
"_collisions"
136 root_group = self.
viewer.viz_manager.getRootDisplayGroup()
138 "rviz/Group", rootNodeName,
True
151 """Delete all the objects from the whole scene"""
152 if hasattr(self,
"collisions_publisher"):
156 if hasattr(self,
"visuals_publisher"):
161 """Display the robot at configuration q in the viz by placing all the bodies."""
164 pin.forwardKinematics(self.model, self.data, q)
166 if self.collision_model
is not None and hasattr(self,
"collisions_publisher"):
167 pin.updateGeometryPlacements(
168 self.model, self.data, self.collision_model, self.collision_data
172 self.collision_model,
177 if self.visual_model
is not None and hasattr(self,
"visuals_publisher"):
178 pin.updateGeometryPlacements(
179 self.model, self.data, self.visual_model, self.visual_data
188 def _plot(self, publisher, model, data, previous_ids=()):
189 """Create markers for each object of the model and publish it as MarkerArray (also delete unused previously created markers)"""
190 from rospy
import get_rostime
191 from std_msgs.msg
import Header, ColorRGBA
192 from geometry_msgs.msg
import Point
193 from visualization_msgs.msg
import MarkerArray, Marker
197 frame_id=
"map", seq=self.
seq, stamp=get_rostime()
200 marker_array = MarkerArray()
201 for obj
in model.geometryObjects:
202 obj_id = model.getGeometryId(obj.name)
207 marker.header = header
208 marker.action = Marker.ADD
210 marker.color = ColorRGBA(*obj.meshColor)
212 if obj.meshTexturePath !=
"":
214 "Textures are not supported in RVizVisualizer (for "
217 category=UserWarning,
223 if WITH_HPP_FCL_BINDINGS
and isinstance(geom, hppfcl.ShapeBase):
225 if isinstance(geom, hppfcl.Cylinder):
226 d, l = 2 * geom.radius, 2 * geom.halfLength
227 marker.type = Marker.CYLINDER
228 marker.scale = Point(d, d, l)
229 marker_array.markers.append(marker)
230 elif isinstance(geom, hppfcl.Box):
232 marker.type = Marker.CUBE
233 marker.scale = Point(*size)
234 marker_array.markers.append(marker)
235 elif isinstance(geom, hppfcl.Sphere):
237 marker.type = Marker.SPHERE
238 marker.scale = Point(d, d, d)
239 marker_array.markers.append(marker)
240 elif isinstance(geom, hppfcl.Capsule):
241 d, l = 2 * geom.radius, 2 * geom.halfLength
242 marker_array.markers.extend(
246 msg =
"Unsupported geometry type for %s (%s)" % (
250 warnings.warn(msg, category=UserWarning, stacklevel=2)
254 marker.type = Marker.MESH_RESOURCE
255 marker.scale = Point(*
npToTuple(obj.meshScale))
256 marker.mesh_resource =
"file://" + obj.meshPath
257 marker_array.markers.append(marker)
260 new_ids = [marker.id
for marker
in marker_array.markers]
261 for old_id
in previous_ids:
262 if not old_id
in new_ids:
263 marker_remove = Marker()
264 marker_remove.header = header
265 marker_remove.id = old_id
266 marker_remove.action = Marker.DELETE
267 marker_array.markers.append(marker_remove)
270 publisher.publish(marker_array)
276 """Delete all the markers from a topic (use one marker with action DELETEALL)"""
277 from rospy
import get_rostime
278 from std_msgs.msg
import Header
279 from visualization_msgs.msg
import MarkerArray, Marker
286 marker.header = Header(frame_id=
"map", seq=self.
seq, stamp=get_rostime())
287 marker.action = Marker.DELETEALL
290 marker_array = MarkerArray(markers=[marker])
293 publisher.publish(marker_array)
296 """Set whether to display collision objects or not"""
300 """Set whether to display visual objects or not"""
304 from python_qt_binding.QtTest
import QTest
306 QTest.qWait(1e3 * dt)
309 __all__ = [
"RVizVisualizer"]