3 from typing
import ClassVar, List
7 from ..
import pinocchio_pywrap_default
as pin
8 from ..deprecation
import DeprecatedWarning
9 from ..utils
import npToTuple
10 from .
import BaseVisualizer
14 import meshcat.geometry
as mg
16 import_meshcat_succeed =
False
18 import_meshcat_succeed =
True
23 import xml.etree.ElementTree
as Et
24 from typing
import Any, Dict, Optional, Set, Union
26 MsgType = Dict[str, Union[str, bytes, bool, float,
"MsgType"]]
31 WITH_HPP_FCL_BINDINGS =
True
33 WITH_HPP_FCL_BINDINGS =
False
35 DEFAULT_COLOR_PROFILES = {
36 "gray": ([0.98, 0.98, 0.98], [0.8, 0.8, 0.8]),
37 "white": ([1.0, 1.0, 1.0], [1.0, 1.0, 1.0]),
39 COLOR_PRESETS = DEFAULT_COLOR_PROFILES.copy()
41 FRAME_AXIS_POSITIONS = (
42 np.array([[0, 0, 0], [1, 0, 0], [0, 0, 0], [0, 1, 0], [0, 0, 0], [0, 0, 1]])
47 np.array([[1, 0, 0], [1, 0.6, 0], [0, 1, 0], [0.6, 1, 0], [0, 0, 1], [0, 0.6, 1]])
54 assert color
is not None
55 color = np.asarray(color)
56 assert color.shape == (3,)
57 return color.clip(0.0, 1.0)
61 """Check whether the geometry object contains a Mesh supported by MeshCat"""
62 if geometry_object.meshPath ==
"":
65 _, file_extension = os.path.splitext(geometry_object.meshPath)
66 if file_extension.lower()
in [
".dae",
".obj",
".stl"]:
72 if import_meshcat_succeed:
75 """A cone of the given height and radius. By Three.js convention, the axis
76 of rotational symmetry is aligned with the y-axis.
83 radialSegments: float = 32,
84 openEnded: bool =
False,
92 def lower(self, object_data: Any) -> MsgType:
95 "type":
"ConeGeometry",
103 def __init__(self, dae_path: str, cache: Optional[Set[str]] =
None) ->
None:
104 """Load Collada files with texture images.
106 https://gist.github.com/danzimmerman/a392f8eadcf1166eb5bd80e3922dbdc5
117 dae_dir = os.path.dirname(dae_path)
118 with open(dae_path)
as text_file:
122 img_resource_paths = []
123 img_lib_element = Et.parse(dae_path).find(
124 "{http://www.collada.org/2005/11/COLLADASchema}library_images"
127 img_resource_paths = [
128 e.text
for e
in img_lib_element.iter()
if e.tag.count(
"init_from")
133 for img_path
in img_resource_paths:
135 if cache
is not None:
136 if img_path
in cache:
142 img_path_abs = img_path
143 if not os.path.isabs(img_path):
144 img_path_abs = os.path.normpath(os.path.join(dae_dir, img_path_abs))
145 if not os.path.isfile(img_path_abs):
146 raise UserWarning(f
"Texture '{img_path}' not found.")
147 with open(img_path_abs,
"rb")
as img_file:
148 img_data = base64.b64encode(img_file.read())
149 img_uri = f
"data:image/png;base64,{img_data.decode('utf-8')}"
153 """Pack data into a dictionary of the format that must be passed to
154 `Visualizer.window.send`.
157 "type":
"set_object",
160 "metadata": {
"version": 4.5,
"type":
"Object"},
165 "type":
"_meshfile_object",
183 """A plane of the given width and height."""
189 widthSegments: float = 1,
190 heightSegments: float = 1,
198 def lower(self, object_data: Any) -> MsgType:
201 "type":
"PlaneGeometry",
210 WITH_HPP_FCL_BINDINGS
211 and tuple(map(int, hppfcl.__version__.split(
"."))) >= (3, 0, 0)
212 and hppfcl.WITH_OCTOMAP
216 boxes = octree.toBoxes()
220 bs = boxes[0][3] / 2.0
221 num_boxes = len(boxes)
223 box_corners = np.array(
236 all_points = np.empty((8 * num_boxes, 3))
237 all_faces = np.empty((12 * num_boxes, 3), dtype=int)
239 for box_id, box_properties
in enumerate(boxes):
240 box_center = box_properties[:3]
242 corners = box_corners + box_center
243 point_range = range(box_id * 8, (box_id + 1) * 8)
244 all_points[point_range, :] = corners
255 all_faces[face_id] = np.array([C, D, B])
256 all_faces[face_id + 1] = np.array([B, A, C])
257 all_faces[face_id + 2] = np.array([A, B, F])
258 all_faces[face_id + 3] = np.array([F, E, A])
259 all_faces[face_id + 4] = np.array([E, F, H])
260 all_faces[face_id + 5] = np.array([H, G, E])
261 all_faces[face_id + 6] = np.array([G, H, D])
262 all_faces[face_id + 7] = np.array([D, C, G])
264 all_faces[face_id + 8] = np.array([A, E, G])
265 all_faces[face_id + 9] = np.array([G, C, A])
267 all_faces[face_id + 10] = np.array([B, H, F])
268 all_faces[face_id + 11] = np.array([H, B, D])
272 colors = np.empty((all_points.shape[0], 3))
273 colors[:] = np.ones(3)
274 mesh = mg.TriangularMeshGeometry(all_points, all_faces, colors)
280 raise NotImplementedError(
"loadOctree need hppfcl with octomap support")
283 if WITH_HPP_FCL_BINDINGS:
286 if isinstance(mesh, (hppfcl.HeightFieldOBBRSS, hppfcl.HeightFieldAABB)):
287 heights = mesh.getHeights()
288 x_grid = mesh.getXGrid()
289 y_grid = mesh.getYGrid()
290 min_height = mesh.getMinHeight()
292 X, Y = np.meshgrid(x_grid, y_grid)
297 num_cells = (nx) * (ny) * 2 + (nx + ny) * 4 + 2
299 num_vertices = X.size
302 faces = np.empty((num_tris, 3), dtype=int)
303 vertices = np.vstack(
307 X.reshape(num_vertices),
308 Y.reshape(num_vertices),
309 heights.reshape(num_vertices),
315 X.reshape(num_vertices),
316 Y.reshape(num_vertices),
317 np.full(num_vertices, min_height),
325 for y_id
in range(ny):
326 for x_id
in range(nx):
327 p0 = x_id + y_id * (nx + 1)
332 faces[face_id] = np.array([p0, p3, p1])
334 faces[face_id] = np.array([p3, p2, p1])
338 p0_low = p0 + num_vertices
339 p1_low = p1 + num_vertices
341 faces[face_id] = np.array([p0, p1_low, p0_low])
343 faces[face_id] = np.array([p0, p1, p1_low])
347 p2_low = p2 + num_vertices
348 p3_low = p3 + num_vertices
350 faces[face_id] = np.array([p3, p3_low, p2_low])
352 faces[face_id] = np.array([p3, p2_low, p2])
356 p0_low = p0 + num_vertices
357 p3_low = p3 + num_vertices
359 faces[face_id] = np.array([p0, p3_low, p3])
361 faces[face_id] = np.array([p0, p0_low, p3_low])
365 p1_low = p1 + num_vertices
366 p2_low = p2 + num_vertices
368 faces[face_id] = np.array([p1, p2_low, p2])
370 faces[face_id] = np.array([p1, p1_low, p2_low])
376 p2 = 2 * num_vertices - 1
379 faces[face_id] = np.array([p0, p1, p2])
381 faces[face_id] = np.array([p0, p2, p3])
384 elif isinstance(mesh, (hppfcl.Convex, hppfcl.BVHModelBase)):
385 if isinstance(mesh, hppfcl.BVHModelBase):
386 num_vertices = mesh.num_vertices
387 num_tris = mesh.num_tris
389 call_triangles = mesh.tri_indices
390 call_vertices = mesh.vertices
392 elif isinstance(mesh, hppfcl.Convex):
393 num_vertices = mesh.num_points
394 num_tris = mesh.num_polygons
396 call_triangles = mesh.polygons
397 call_vertices = mesh.points
399 faces = np.empty((num_tris, 3), dtype=int)
400 for k
in range(num_tris):
401 tri = call_triangles(k)
402 faces[k] = [tri[i]
for i
in range(3)]
404 vertices = call_vertices()
405 vertices = vertices.astype(np.float32)
408 mesh = mg.TriangularMeshGeometry(vertices, faces)
412 vertices.T, color=np.repeat(np.ones((3, 1)), num_vertices, axis=1)
414 mg.PointsMaterial(size=0.002),
422 raise NotImplementedError(
"loadMesh need hppfcl")
426 import meshcat.geometry
as mg
431 [1.0, 0.0, 0.0, 0.0],
432 [0.0, 0.0, -1.0, 0.0],
433 [0.0, 1.0, 0.0, 0.0],
434 [0.0, 0.0, 0.0, 1.0],
437 RotatedCylinder =
type(
438 "RotatedCylinder", (mg.Cylinder,), {
"intrinsic_transform":
lambda self: R}
441 geom = geometry_object.geometry
443 if WITH_HPP_FCL_BINDINGS
and isinstance(geom, hppfcl.ShapeBase):
444 if isinstance(geom, hppfcl.Capsule):
445 if hasattr(mg,
"TriangularMeshGeometry"):
448 obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)
449 elif isinstance(geom, hppfcl.Cylinder):
450 obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)
451 elif isinstance(geom, hppfcl.Cone):
452 obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0)
453 elif isinstance(geom, hppfcl.Box):
454 obj = mg.Box(
npToTuple(2.0 * geom.halfSide))
455 elif isinstance(geom, hppfcl.Sphere):
456 obj = mg.Sphere(geom.radius)
457 elif isinstance(geom, hppfcl.ConvexBase):
461 msg = f
"Unsupported geometry type for {geometry_object.name} ({type(geom)})"
462 warnings.warn(msg, category=UserWarning, stacklevel=2)
468 nbv = np.array([
max(radial_resolution, 4),
max(cap_resolution, 4)])
472 vertices = np.zeros((nbv[0] * (2 * nbv[1]) + 2, 3))
473 for j
in range(nbv[0]):
474 phi = (2 * np.pi * j) / nbv[0]
475 for i
in range(nbv[1]):
476 theta = (np.pi / 2 * i) / nbv[1]
477 vertices[position + i, :] = np.array(
479 np.cos(theta) * np.cos(phi) * r,
480 np.cos(theta) * np.sin(phi) * r,
481 -h / 2 - np.sin(theta) * r,
484 vertices[position + i + nbv[1], :] = np.array(
486 np.cos(theta) * np.cos(phi) * r,
487 np.cos(theta) * np.sin(phi) * r,
488 h / 2 + np.sin(theta) * r,
491 position += nbv[1] * 2
492 vertices[-2, :] = np.array([0, 0, -h / 2 - r])
493 vertices[-1, :] = np.array([0, 0, h / 2 + r])
494 indexes = np.zeros((nbv[0] * (4 * (nbv[1] - 1) + 4), 3))
497 last = nbv[0] * (2 * nbv[1]) + 1
498 for j
in range(nbv[0]):
499 j_next = (j + 1) % nbv[0]
500 indexes[index + 0] = np.array(
501 [j_next * stride + nbv[1], j_next * stride, j * stride]
503 indexes[index + 1] = np.array(
504 [j * stride + nbv[1], j_next * stride + nbv[1], j * stride]
506 indexes[index + 2] = np.array(
507 [j * stride + nbv[1] - 1, j_next * stride + nbv[1] - 1, last - 1]
509 indexes[index + 3] = np.array(
510 [j_next * stride + 2 * nbv[1] - 1, j * stride + 2 * nbv[1] - 1, last]
512 for i
in range(nbv[1] - 1):
513 indexes[index + 4 + i * 4 + 0] = np.array(
514 [j_next * stride + i, j_next * stride + i + 1, j * stride + i]
516 indexes[index + 4 + i * 4 + 1] = np.array(
517 [j_next * stride + i + 1, j * stride + i + 1, j * stride + i]
519 indexes[index + 4 + i * 4 + 2] = np.array(
521 j_next * stride + nbv[1] + i + 1,
522 j_next * stride + nbv[1] + i,
523 j * stride + nbv[1] + i,
526 indexes[index + 4 + i * 4 + 3] = np.array(
528 j_next * stride + nbv[1] + i + 1,
529 j * stride + nbv[1] + i,
530 j * stride + nbv[1] + i + 1,
533 index += 4 * (nbv[1] - 1) + 4
534 return mg.TriangularMeshGeometry(vertices, indexes)
538 """A Pinocchio display using Meshcat"""
541 FRAME_VEL_COLOR = 0x00FF00
542 CAMERA_PRESETS: ClassVar = {
547 "preset1": [np.zeros(3), [1.0, 1.0, 1.0]],
548 "preset2": [[0.0, 0.0, 0.6], [0.8, 1.0, 1.2]],
549 "acrobot": [[0.0, 0.1, 0.0], [0.5, 0.0, 0.2]],
550 "cam_ur": [[0.4, 0.6, -0.2], [1.0, 0.4, 1.2]],
551 "cam_ur2": [[0.4, 0.3, 0.0], [0.5, 0.1, 1.4]],
552 "cam_ur3": [[0.4, 0.3, 0.0], [0.6, 1.3, 0.3]],
553 "cam_ur4": [[-1.0, 0.3, 0.0], [1.3, 0.1, 1.2]],
554 "cam_ur5": [[-1.0, 0.3, 0.0], [-0.05, 1.5, 1.2]],
555 "talos": [[0.0, 1.2, 0.0], [1.5, 0.3, 1.5]],
556 "talos2": [[0.0, 1.1, 0.0], [1.2, 0.6, 1.5]],
562 collision_model=
None,
569 if not import_meshcat_succeed:
571 "Error while importing the viewer client.\n"
572 "Check whether meshcat is properly installed "
573 "(pip install --user meshcat)."
575 raise ImportError(msg)
589 """Return the name of the geometry object inside the viewer."""
590 if geometry_type
is pin.GeometryType.VISUAL:
592 elif geometry_type
is pin.GeometryType.COLLISION:
595 def initViewer(self, viewer=None, open=False, loadModel=False, zmq_url=None):
596 """Start a new MeshCat server and client.
597 Note: the server can also be started separately using the "meshcat-server"
598 command in a terminal:
599 this enables the server to remain active after the current script ends.
602 self.
viewer = meshcat.Visualizer(zmq_url)
if viewer
is None else viewer
629 """Set the background."""
630 if col_top
is not None:
634 assert preset_name
in COLOR_PRESETS.keys()
635 col_top, col_bot = COLOR_PRESETS[preset_name]
640 self.
viewer.set_cam_target(target)
643 self.
viewer.set_cam_pos(position)
646 """Set the camera angle and position using a given preset."""
647 assert preset_key
in self.CAMERA_PRESETS
648 cam_val = self.CAMERA_PRESETS[preset_key]
654 elt.set_property(
"zoom", zoom)
666 import meshcat.geometry
as mg
669 basic_three_js_transform = np.array(
671 [1.0, 0.0, 0.0, 0.0],
672 [0.0, 0.0, -1.0, 0.0],
673 [0.0, 1.0, 0.0, 0.0],
674 [0.0, 0.0, 0.0, 1.0],
677 RotatedCylinder =
type(
680 {
"intrinsic_transform":
lambda self: basic_three_js_transform},
685 geom = geometry_object.geometry
687 if WITH_HPP_FCL_BINDINGS
and isinstance(geom, hppfcl.ShapeBase):
688 if isinstance(geom, hppfcl.Capsule):
689 if hasattr(mg,
"TriangularMeshGeometry"):
692 obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)
693 elif isinstance(geom, hppfcl.Cylinder):
694 obj = RotatedCylinder(2.0 * geom.halfLength, geom.radius)
695 elif isinstance(geom, hppfcl.Cone):
696 obj = RotatedCylinder(2.0 * geom.halfLength, 0, geom.radius, 0)
697 elif isinstance(geom, hppfcl.Box):
698 obj = mg.Box(
npToTuple(2.0 * geom.halfSide))
699 elif isinstance(geom, hppfcl.Sphere):
700 obj = mg.Sphere(geom.radius)
701 elif isinstance(geom, hppfcl.Plane):
703 To[:3, 3] = geom.d * geom.n
704 TranslatedPlane =
type(
707 {
"intrinsic_transform":
lambda self: To},
709 sx = geometry_object.meshScale[0] * 10
710 sy = geometry_object.meshScale[1] * 10
711 obj = TranslatedPlane(sx, sy)
712 elif isinstance(geom, hppfcl.Ellipsoid):
713 obj = mg.Ellipsoid(geom.radii)
714 elif isinstance(geom, (hppfcl.Plane, hppfcl.Halfspace)):
715 plane_transform: pin.SE3 = pin.SE3.Identity()
717 plane_transform.rotation = pin.Quaternion.FromTwoVectors(
720 TransformedPlane =
type(
723 {
"intrinsic_transform":
lambda self: plane_transform.homogeneous},
725 obj = TransformedPlane(1000, 1000)
726 elif isinstance(geom, hppfcl.ConvexBase):
730 msg = f
"Unsupported geometry type for {geometry_object.name} ({type(geom)})"
731 warnings.warn(msg, category=UserWarning, stacklevel=2)
738 if geometry_object.meshPath ==
"":
740 "Display of geometric primitives is supported only if "
741 "pinocchio is build with HPP-FCL bindings."
743 warnings.warn(msg, category=UserWarning, stacklevel=2)
747 _, file_extension = os.path.splitext(geometry_object.meshPath)
748 if file_extension.lower() ==
".dae":
750 elif file_extension.lower() ==
".obj":
751 obj = mg.ObjMeshGeometry.from_file(geometry_object.meshPath)
752 elif file_extension.lower() ==
".stl":
753 obj = mg.StlMeshGeometry.from_file(geometry_object.meshPath)
755 msg = f
"Unknown mesh file format: {geometry_object.meshPath}."
756 warnings.warn(msg, category=UserWarning, stacklevel=2)
762 """Load a single geometry object"""
764 meshcat_node = self.
viewer[node_name]
769 if WITH_HPP_FCL_BINDINGS:
770 if isinstance(geometry_object.geometry, hppfcl.ShapeBase):
773 tuple(map(int, hppfcl.__version__.split(
"."))) >= (3, 0, 0)
774 and hppfcl.WITH_OCTOMAP
775 and isinstance(geometry_object.geometry, hppfcl.OcTree)
782 geometry_object.geometry,
785 hppfcl.HeightFieldOBBRSS,
786 hppfcl.HeightFieldAABB,
789 obj =
loadMesh(geometry_object.geometry)
795 "The geometry object named "
796 + geometry_object.name
797 +
" is not supported by Pinocchio/MeshCat for vizualization."
799 warnings.warn(msg, category=UserWarning, stacklevel=2)
801 except Exception
as e:
803 "Error while loading geometry object: "
804 f
"{geometry_object.name}\nError message:\n{e}"
806 warnings.warn(msg, category=UserWarning, stacklevel=2)
809 if isinstance(obj, mg.Object):
810 meshcat_node.set_object(obj)
811 elif isinstance(obj, (mg.Geometry, mg.ReferenceSceneElement)):
812 material = mg.MeshPhongMaterial()
816 def to_material_color(rgba) -> int:
817 """Convert rgba color as list into rgba color as int"""
819 int(rgba[0] * 255) * 256**2
820 + int(rgba[1] * 255) * 256
825 meshColor = geometry_object.meshColor
829 material.color = to_material_color(meshColor)
831 if float(meshColor[3]) != 1.0:
832 material.transparent =
True
833 material.opacity = float(meshColor[3])
835 geom_material = geometry_object.meshMaterial
836 if geometry_object.overrideMaterial
and isinstance(
837 geom_material, pin.GeometryPhongMaterial
839 material.emissive = to_material_color(geom_material.meshEmissionColor)
840 material.specular = to_material_color(geom_material.meshSpecularColor)
841 material.shininess = geom_material.meshShininess * 100.0
843 if isinstance(obj, DaeMeshGeometry):
844 obj.path = meshcat_node.path
845 scale = list(np.asarray(geometry_object.meshScale).flatten())
847 if geometry_object.overrideMaterial:
848 obj.material = material
849 meshcat_node.window.send(obj)
851 meshcat_node.set_object(obj, material)
854 if is_mesh
and not isinstance(obj, DaeMeshGeometry):
855 scale = list(np.asarray(geometry_object.meshScale).flatten())
856 meshcat_node.set_property(
"scale", scale)
860 rootNodeName="pinocchio",
862 collision_color=None,
865 """Load the robot in a MeshCat viewer.
867 rootNodeName: name to give to the robot in the viewer
868 color: deprecated and optional, color to give to both the collision
869 and visual models of the robot. This setting overwrites any color
870 specified in the robot description. Format is a list of four
871 RGBA floating-point numbers (between 0 and 1)
872 collision_color: optional, color to give to the collision model of
873 the robot. Format is a list of four RGBA floating-point numbers
875 visual_color: optional, color to give to the visual model of
876 the robot. Format is a list of four RGBA floating-point numbers
879 if color
is not None:
881 "The 'color' argument is deprecated and will be removed in a "
882 "future version of Pinocchio. Consider using "
883 "'collision_color' and 'visual_color' instead.",
884 category=DeprecatedWarning,
886 collision_color = color
895 if self.collision_model
is not None:
896 for collision
in self.collision_model.geometryObjects:
898 collision, pin.GeometryType.COLLISION, collision_color
904 if self.visual_model
is not None:
905 for visual
in self.visual_model.geometryObjects:
907 visual, pin.GeometryType.VISUAL, visual_color
915 def reload(self, new_geometry_object, geometry_type=None):
916 """Reload a geometry_object given by its name and its type"""
917 if geometry_type == pin.GeometryType.VISUAL:
918 geom_model = self.visual_model
920 geom_model = self.collision_model
921 geometry_type = pin.GeometryType.COLLISION
923 geom_id = geom_model.getGeometryId(new_geometry_object.name)
924 geom_model.geometryObjects[geom_id] = new_geometry_object
926 self.
delete(new_geometry_object, geometry_type)
927 visual = geom_model.geometryObjects[geom_id]
933 def delete(self, geometry_object, geometry_type):
939 Display the robot at configuration q in the viewer by placing all the bodies
942 pin.forwardKinematics(self.model, self.data, q)
954 if geometry_type == pin.GeometryType.VISUAL:
955 geom_model = self.visual_model
956 geom_data = self.visual_data
958 geom_model = self.collision_model
959 geom_data = self.collision_data
961 pin.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
962 for visual
in geom_model.geometryObjects:
965 M = geom_data.oMg[geom_model.getGeometryId(visual.name)]
968 geom = visual.geometry
969 if WITH_HPP_FCL_BINDINGS
and isinstance(
970 geom, (hppfcl.Plane, hppfcl.Halfspace)
973 T.translation += M.rotation @ (geom.d * geom.n)
979 self.
viewer[visual_name].set_transform(T)
983 M: pin.SE3 = visual.placement
985 self.
viewer[visual_name].set_transform(T)
988 """Add a visual GeometryObject to the viewer, with an optional color."""
993 if not hasattr(self.
viewer,
"get_image"):
995 "meshcat.Visualizer does not have the get_image() method."
996 " You need meshcat >= 0.2.0 to get this feature."
1000 """Capture an image from the Meshcat viewer and return an RGB array."""
1001 if w
is not None or h
is not None:
1003 img = self.
viewer.get_image(w, h)
1005 img = self.
viewer.get_image()
1006 img_arr = np.asarray(img)
1010 """Set whether to display collision objects or not."""
1011 if self.collision_model
is None:
1021 """Set whether to display visual objects or not."""
1022 if self.visual_model
is None:
1031 def displayFrames(self, visibility, frame_ids=None, axis_length=0.2, axis_width=2):
1032 """Set whether to display frames or not."""
1039 """Initializes the frame objects for display."""
1040 import meshcat.geometry
as mg
1045 for fid, frame
in enumerate(self.model.frames):
1046 if frame_ids
is None or fid
in frame_ids:
1047 frame_viz_name = f
"{self.viewerFramesGroupName}/{frame.name}"
1048 self.
viewer[frame_viz_name].set_object(
1051 position=axis_length * FRAME_AXIS_POSITIONS,
1052 color=FRAME_AXIS_COLORS,
1054 mg.LineBasicMaterial(
1055 linewidth=axis_width,
1064 Updates the frame visualizations with the latest transforms from model data.
1066 pin.updateFramePlacements(self.model, self.data)
1068 frame_name = self.model.frames[fid].name
1069 frame_viz_name = f
"{self.viewerFramesGroupName}/{frame_name}"
1070 self.
viewer[frame_viz_name].set_transform(self.data.oMf[fid].homogeneous)
1073 pin.updateFramePlacement(self.model, self.data, frame_id)
1074 vFr = pin.getFrameVelocity(
1075 self.model, self.data, frame_id, pin.LOCAL_WORLD_ALIGNED
1077 line_group_name = f
"ee_vel/{frame_id}"
1079 [v_scale * vFr.linear], [frame_id], [line_group_name], [color]
1084 vecs: List[np.ndarray],
1085 frame_ids: List[int],
1086 vec_names: List[str],
1089 """Draw vectors extending from given frames."""
1090 import meshcat.geometry
as mg
1092 if len(vecs) != len(frame_ids)
or len(vecs) != len(vec_names):
1094 "Number of vectors and frames IDs or names is inconsistent."
1096 for i, (fid, v)
in enumerate(zip(frame_ids, vecs)):
1097 frame_pos = self.data.oMf[fid].translation
1098 vertices = np.array([frame_pos, frame_pos + v]).astype(np.float32).T
1100 geometry = mg.PointsGeometry(position=vertices)
1101 geom_object = mg.LineSegments(
1102 geometry, mg.LineBasicMaterial(color=colors[i])
1105 self.
viewer[prefix].set_object(geom_object)
1108 __all__ = [
"MeshcatVisualizer"]