placo 0.3.6__tar.gz → 0.3.7__tar.gz

{placo-0.3.6 → placo-0.3.7}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: placo
-Version: 0.3.6
+Version: 0.3.7
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.8
 License-Expression: MIT
@@ -14,6 +14,7 @@ Requires-Dist: pin >= 2.6.18, < 3
 Requires-Dist: rhoban-cmeel-jsoncpp
 Requires-Dist: meshcat
 Requires-Dist: numpy
+Requires-Dist: ischedule
 Provides-Extra: build
 Requires-Dist: pin[build] >= 2.6.18, < 3 ; extra == "build"
 Requires-Dist: cmeel-urdfdom[build] ; extra == "build"

{placo-0.3.6 → placo-0.3.7}/pyproject.toml

@@ -17,12 +17,13 @@ dependencies = [
   "rhoban-cmeel-jsoncpp",
   "meshcat",
   "numpy",
+  "ischedule"
 ]
 description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.8"
-version = "0.3.6"
+version = "0.3.7"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.3.6 → placo-0.3.7}/python/placo_utils/visualization.py

@@ -18,12 +18,15 @@ def get_viewer() -> meshcat.Visualizer:
 
     if viewer is None:
         viewer = meshcat.Visualizer()
+
         print(f"Viewer URL: {viewer.url()}")
 
     return viewer
 
 
-def robot_viz(robot: placo.RobotWrapper, name: str = "robot") -> pin.visualize.MeshcatVisualizer:
+def robot_viz(
+    robot: placo.RobotWrapper, name: str = "robot"
+) -> pin.visualize.MeshcatVisualizer:
     """
     Builds an instance of pinocchio MeshcatVisualizer, which allows to push the model to the meshcat
     visualizer passed as parameter
@@ -31,14 +34,13 @@ def robot_viz(robot: placo.RobotWrapper, name: str = "robot") -> pin.visualize.M
     The robot can further be displayed using:
 
     > viz.display(q)
-
-    :param robot: robot instance
-    :param name: name of the robot
     """
     global robot_names
 
     robot_names[robot] = name
-    viz = pin.visualize.MeshcatVisualizer(robot.model, robot.collision_model, robot.visual_model)
+    viz = pin.visualize.MeshcatVisualizer(
+        robot.model, robot.collision_model, robot.visual_model
+    )
     viz.initViewer(viewer=get_viewer())
     viz.loadViewerModel(name)
 
@@ -51,10 +53,6 @@ cylinders: dict = {}
 def frame_viz(name: str, T: np.ndarray, opacity: float = 1.0) -> None:
     """
     Visualizes a given frame
-
-    :param name: name of the frame
-    :param T: transformation of the frame to the world
-    :param opacity: opacity of the frame
     """
     global cylinders
     vis = get_viewer()
@@ -76,30 +74,60 @@ def frame_viz(name: str, T: np.ndarray, opacity: float = 1.0) -> None:
             )
         obj = cylinders[node_name]
 
-        obj.set_transform(T @ tf.rotation_matrix(*rotate) @ tf.translation_matrix([0, 0.05, 0]))
+        obj.set_transform(
+            T @ tf.rotation_matrix(*rotate) @ tf.translation_matrix([0, 0.05, 0])
+        )
 
 
-def point_viz(name: str, point: np.ndarray, radius: float = 0.01, color: float = 0xFF0000, opacity: float = 1.0) -> None:
+def point_viz(
+    name: str,
+    point: np.ndarray,
+    radius: float = 0.01,
+    color: float = 0xFF0000,
+    opacity: float = 1.0,
+) -> None:
     """
     Prints a point (sphere)
+    """
+    vis = get_viewer()
+    vis["point"][name].set_object(
+        g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity)
+    )
+    vis["point"][name].set_transform(tf.translation_matrix(point))
+
 
-    :param name: name of the point
-    :param point: point to print
-    :param radius: radius of the point
-    :param color: color of the point
-    :param opacity: opacity of the point
+points_sizes = {}
+
+
+def points_viz(
+    name: str,
+    points: np.ndarray,
+    radius: float = 0.01,
+    color: float = 0xFF0000,
+    opacity: float = 1.0,
+) -> None:
+    """
+    Prints a point (sphere)
     """
+    global points_sizes
     vis = get_viewer()
-    vis["points"][name].set_object(g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity))
-    vis["points"][name].set_transform(tf.translation_matrix(point))
+    k = 0
+    for point in points:
+        entry = f"{name}_{k}"
+        k += 1
+        vis["points"][name][entry].set_object(
+            g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity)
+        )
+        vis["points"][name][entry].set_transform(tf.translation_matrix(point))
+    while k < points_sizes.get(name, 0):
+        vis["points"][name][f"{name}_{k}"].delete()
+        k += 1
+    points_sizes[name] = len(points)
 
 
 def robot_frame_viz(robot: placo.RobotWrapper, frame: str) -> None:
     """
-    Draw a frame attached to the robot
-
-    :param robot: robot instance
-    :param frame: frame name
+    Draw a frame from the robot
     """
     node_name = f"{robot_names[robot]}_{frame}"
     frame_viz(node_name, robot.get_T_world_frame(frame))
@@ -109,12 +137,6 @@ steps: int = 0
 
 
 def footsteps_viz(footsteps: placo.Footsteps, T: np.ndarray = np.eye(4)) -> None:
-    """
-    Visualizes humanoid footsteps
-
-    :param footsteps: footsteps to visualize
-    :param T: left transformation to apply to the footsteps
-    """    
     global steps
     vis = get_viewer()
 
@@ -132,23 +154,25 @@ def footsteps_viz(footsteps: placo.Footsteps, T: np.ndarray = np.eye(4)) -> None
             if len(footstep.footsteps) >= 2:
                 color = 0x1111AA
             else:
-                color = 0xFF3333 if str(footstep.footsteps[0].side) == "left" else 0x33FF33
+                color = (
+                    0xFF3333 if str(footstep.footsteps[0].side) == "left" else 0x33FF33
+                )
         else:
             color = 0xFF3333 if str(footstep.side) == "left" else 0x33FF33
 
-        vis["footsteps"][str(k)].set_object(g.LineLoop(g.PointsGeometry(polygon.T), g.MeshBasicMaterial(color=color)))
+        vis["footsteps"][str(k)].set_object(
+            g.LineLoop(g.PointsGeometry(polygon.T), g.MeshBasicMaterial(color=color))
+        )
 
 
 def line_viz(name: str, points: np.ndarray, color: float = 0xFF0000) -> None:
     """
     Prints a line
-
-    :param name: name of the line
-    :param points: points of the line
-    :param color: color of the line
     """
     vis = get_viewer()
-    vis["lines"][name].set_object(g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color)))
+    vis["lines"][name].set_object(
+        g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color))
+    )
 
 
 def arrow_viz(
@@ -160,14 +184,8 @@ def arrow_viz(
 ) -> None:
     """
     Prints an arrow
-
-    :param name: name of the arrow
-    :param point_from: origin of the arrow
-    :param point_to: end of the arrow
-    :param color: color of the arrow
-    :param radius: radius of the arrow
     """
-    head_length = radius*3
+    head_length = radius * 3
     vis = get_viewer()
     length = np.linalg.norm(point_to - point_from)
     length = max(1e-3, length - head_length)
@@ -189,43 +207,58 @@ def arrow_viz(
     T_cylinder = T @ tf.translation_matrix(np.array([0, length / 2.0, 0.0]))
     T_head = T @ tf.translation_matrix(np.array([0, length + head_length / 2.0, 0.0]))
 
-    vis["arrows"][name]["cylinder"].set_object(cylinder, g.MeshBasicMaterial(color=color))
+    vis["arrows"][name]["cylinder"].set_object(
+        cylinder, g.MeshBasicMaterial(color=color)
+    )
     vis["arrows"][name]["cylinder"].set_transform(T_cylinder)
     vis["arrows"][name]["head"].set_object(head, g.MeshBasicMaterial(color=color))
     vis["arrows"][name]["head"].set_transform(T_head)
 
+
 previous_contacts: int = 0
 
-def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
-    """
-    Draws the contacts produced by a dynamics solver
 
-    :param solver: solver instance
-    :param ratio: ratio of the wrench to display
-    :param radius: radius of the arrows
-    """    
+def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
     global previous_contacts
     robot = solver.robot
     frames = robot.frame_names()
-    
+
     for k in range(solver.count_contacts()):
         contact = solver.get_contact(k)
 
         if isinstance(contact, placo.PointContact):
             frame_name = frames[contact.position_task().frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
-            arrow_viz(f"contact_{k}", T_world_frame[:3, 3], T_world_frame[:3, 3] + contact.wrench * ratio, color=0x00FF00, radius=radius)
+            arrow_viz(
+                f"contact_{k}",
+                T_world_frame[:3, 3],
+                T_world_frame[:3, 3] + contact.wrench * ratio,
+                color=0x00FF00,
+                radius=radius,
+            )
         elif isinstance(contact, placo.Contact6D):
             frame_name = frames[contact.position_task().frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
             wrench = T_world_frame[:3, :3] @ contact.wrench[:3]
             origin = T_world_frame[:3, 3] + T_world_frame[:3, :3] @ contact.zmp()
-            arrow_viz(f"contact_{k}", origin, origin + wrench * ratio, color=0x00FFAA, radius=radius)
+            arrow_viz(
+                f"contact_{k}",
+                origin,
+                origin + wrench * ratio,
+                color=0x00FFAA,
+                radius=radius,
+            )
         elif isinstance(contact, placo.ExternalWrenchContact):
             frame_name = frames[contact.frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
-            arrow_viz(f"contact_{k}", T_world_frame[:3, 3], T_world_frame[:3, 3] + contact.w_ext[:3] * ratio, color=0xFF2222, radius=radius)
-            
+            arrow_viz(
+                f"contact_{k}",
+                T_world_frame[:3, 3],
+                T_world_frame[:3, 3] + contact.w_ext[:3] * ratio,
+                color=0xFF2222,
+                radius=radius,
+            )
+
     vis = get_viewer()
     while k < previous_contacts:
         k += 1