placo 0.9.15__0-cp311-cp311-macosx_11_0_arm64.whl

cmeel.prefix/lib/python3.11/site-packages/placo_utils/__init__.py

@@ -0,0 +1,4 @@
+from . import tf
+from . import visualization
+
+__all__ = ["tf", "visualization"]

cmeel.prefix/lib/python3.11/site-packages/placo_utils/tf.py

@@ -0,0 +1 @@
+import meshcat.transformations as tf

cmeel.prefix/lib/python3.11/site-packages/placo_utils/view.py

@@ -0,0 +1,37 @@
+import argparse
+import placo
+import time
+from placo_utils.visualization import *
+
+arg_parser = argparse.ArgumentParser()
+arg_parser.add_argument("path", help="Path to the URDF")
+arg_parser.add_argument("--frames", help="Frame to display", nargs="+")
+arg_parser.add_argument("--animate", help="Animate the robot", action="store_true")
+args = arg_parser.parse_args()
+
+robot = placo.RobotWrapper(args.path, placo.Flags.ignore_collisions)
+robot.update_kinematics()
+
+print("Joint names:")
+print(list(robot.joint_names()))
+
+print("Frame names:")
+print(list(robot.frame_names()))
+
+viz = robot_viz(robot)
+t = 0
+
+while True:
+    viz.display(robot.state.q)
+
+    if args.frames:
+        for frame in args.frames:
+            robot_frame_viz(robot, frame)
+
+    if args.animate:
+        for joint in robot.joint_names():
+            robot.set_joint(joint, np.sin(t))
+            robot.update_kinematics()
+
+    t += 0.01
+    time.sleep(0.01)

cmeel.prefix/lib/python3.11/site-packages/placo_utils/visualization.py

@@ -0,0 +1,356 @@
+import meshcat
+import pinocchio as pin
+import pinocchio.visualize as pin_viz
+import numpy as np
+import meshcat.geometry as g
+import meshcat.transformations as tf
+import placo
+
+
+viewer = None
+robot_names: dict = {}
+
+
+def get_viewer() -> meshcat.Visualizer:
+    """
+    Gets the meshcat viewer, if it doesn't exist, create it
+    """
+    global viewer
+
+    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_viz.MeshcatVisualizer:
+    """
+    Builds an instance of pinocchio MeshcatVisualizer, which allows to push the model to the meshcat
+    visualizer passed as parameter
+
+    The robot can further be displayed using:
+
+    > viz.display(q)
+    """
+    global robot_names
+
+    robot_names[robot] = name
+    viz = pin_viz.MeshcatVisualizer(
+        robot.model, robot.collision_model, robot.visual_model
+    )
+    viz.initViewer(viewer=get_viewer())
+    viz.loadViewerModel(name)
+
+    return viz
+
+
+cylinders: dict = {}
+
+
+def frame_viz(
+    name: str, T: np.ndarray, opacity: float = 1.0, scale: float = 1.0
+) -> None:
+    """
+    Visualizes a given frame
+    """
+    global cylinders
+    vis = get_viewer()
+    axises = {
+        "x": (0xFF0000, (-np.pi / 2, [0, 0, 1])),
+        "y": (0x00FF00, (0, [0, 1, 0])),
+        "z": (0x0000FF, (np.pi / 2, [1, 0, 0])),
+    }
+
+    for axis_name in axises:
+        color, rotate = axises[axis_name]
+        node_name = f"frames/{name}/{axis_name}"
+
+        if node_name not in cylinders:
+            cylinders[node_name] = vis[node_name]
+            cylinders[node_name].set_object(
+                g.Cylinder(0.1, 0.005),
+                g.MeshLambertMaterial(color=color, opacity=opacity),
+            )
+        obj = cylinders[node_name]
+
+        obj.set_transform(
+            T
+            @ tf.scale_matrix(scale)
+            @ 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:
+    """
+    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))
+
+
+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()
+    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, opacity: float = 1.0, scale: float = 1.0
+) -> None:
+    """
+    Draw a frame from the robot
+    """
+    node_name = f"{robot_names[robot]}_{frame}"
+    frame_viz(node_name, robot.get_T_world_frame(frame), opacity, scale)
+
+
+steps: int = 0
+
+
+def footsteps_viz(footsteps: placo.Footsteps, T: np.ndarray = np.eye(4)) -> None:
+    global steps
+    vis = get_viewer()
+
+    if len(footsteps) < steps:
+        vis["footsteps"].delete()
+    steps = len(footsteps)
+
+    k = 0
+    for footstep in footsteps:
+        k += 1
+        polygon = [(T @ [*xy, 0, 1])[:3] for xy in footstep.support_polygon()]
+        polygon = np.array([*polygon, polygon[-1]])
+
+        if isinstance(footstep, placo.Support):
+            if len(footstep.footsteps) >= 2:
+                color = 0x1111AA
+            else:
+                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))
+        )
+
+
+def line_viz(name: str, points: np.ndarray, color: float = 0xFF0000) -> None:
+    """
+    Prints a line
+    """
+    vis = get_viewer()
+    vis["lines"][name].set_object(
+        g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color))
+    )
+
+
+def cross_viz(
+    name: str, position: np.ndarray, size: float = 0.15, color: float = 0xFF0000
+) -> None:
+    """
+    Prints a cross
+    """
+    vis = get_viewer()
+
+    points = np.array(
+        [position - np.array([size, size]), position + np.array([size, size])]
+    )
+    vis["crosses"][f"{name}_1"].set_object(
+        g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color))
+    )
+    points = np.array(
+        [position - np.array([size, -size]), position + np.array([size, -size])]
+    )
+    vis["crosses"][f"{name}_2"].set_object(
+        g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color))
+    )
+
+
+def path_viz(name: str, points: np.ndarray, color: float = 0xFF0000) -> None:
+    """
+    Prints a path
+    """
+    key = f"path_{name}"
+
+    vis = get_viewer()
+    vis[key].delete()
+
+    for k in range(len(points) - 1):
+        vis[key][str(k)].set_object(
+            g.LineSegments(
+                g.PointsGeometry(points[k : k + 2].T), g.LineBasicMaterial(color=color)
+            )
+        )
+
+
+def cylinder_viz(
+    name: str,
+    position: np.ndarray,
+    length: float,
+    radius: float,
+    color: float = 0xFF0000,
+    opacity=1.0,
+) -> None:
+    vis = get_viewer()
+
+    cylinder = g.Cylinder(length, radius)
+    vis["cylinders"][name].set_object(
+        cylinder, g.MeshBasicMaterial(color=color, opacity=opacity)
+    )
+    vis["cylinders"][name].set_transform(
+        tf.translation_matrix((*position, length / 2))
+        @ tf.rotation_matrix(np.pi / 2, (1, 0, 0))
+    )
+
+
+def arrow_viz(
+    name: str,
+    point_from: np.ndarray,
+    point_to: np.ndarray,
+    color: float = 0xFF0000,
+    radius: float = 0.003,
+) -> None:
+    """
+    Prints an arrow
+    """
+    head_length = radius * 3
+    vis = get_viewer()
+    length = np.linalg.norm(point_to - point_from)
+    length = max(1e-3, length - head_length)
+
+    cylinder = g.Cylinder(length, radius)
+    head = g.Cylinder(head_length, 2 * radius, 0.0, 2 * radius)
+
+    T = tf.translation_matrix(point_from)
+    if np.linalg.norm(point_to - point_from) > 1e-6:
+        T[:3, :3] = placo.rotation_from_axis(
+            "y", (point_to - point_from) / np.linalg.norm(point_to - point_from)
+        )
+
+    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_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):
+    global previous_contacts
+    robot = solver.robot
+    frames = robot.frame_names()
+    k = 0
+
+    for _ in range(solver.count_contacts()):
+        contact = solver.get_contact(k)
+        if not contact.active:
+            continue
+
+        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,
+            )
+        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]
+
+            if np.linalg.norm(wrench) < 1e-6:
+                origin = T_world_frame[:3, 3]
+            else:
+                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,
+            )
+        elif isinstance(contact, placo.LineContact):
+            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]
+
+            if np.linalg.norm(wrench) < 1e-6:
+                origin = T_world_frame[:3, 3]
+            else:
+                origin = T_world_frame[:3, 3] + T_world_frame[:3, :3] @ contact.zmp()
+
+            arrow_viz(
+                f"contact_{k}",
+                origin,
+                origin + wrench * ratio,
+                color=0xFF33AA,
+                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,
+            )
+
+        k += 1
+
+    vis = get_viewer()
+    k_d = k
+    while k_d < previous_contacts:
+        vis["arrows"][f"contact_{k_d}"].delete()
+        k_d += 1
+    previous_contacts = k

placo-0.9.15.dist-info/METADATA

@@ -0,0 +1,62 @@
+Metadata-Version: 2.4
+Name: placo
+Version: 0.9.15
+Summary: PlaCo: Rhoban Planning and Control
+Requires-Python: >= 3.8
+License-Expression: MIT
+License-File: LICENSE
+Author-email: Rhoban team <team@rhoban.com>
+Project-URL: Changelog, https://github.com/rhoban/placo/blob/main/CHANGELOG.md
+Home-page: https://placo.readthedocs.io/en/latest/
+Project-URL: Repository, https://github.com/rhoban/placo.git
+Requires-Dist: cmeel
+Requires-Dist: eiquadprog >= 1.2.6, < 2
+Requires-Dist: pin==3.4.0
+Requires-Dist: rhoban-cmeel-jsoncpp
+Requires-Dist: meshcat
+Requires-Dist: ischedule
+Provides-Extra: build
+Requires-Dist: pin[build]==3.4.0 ; extra == "build"
+Requires-Dist: doxystub >= 0.1.7 ; extra == "build"
+Requires-Dist: cmake<4 ; extra == "build"
+Description-Content-Type: text/markdown
+
+<img width="400" src="https://placo.readthedocs.io/en/latest/_static/placo.png" />
+
+## Planning & Control
+
+PlaCo is Rhoban's planning and control library. It is built on the top of [pinocchio](https://github.com/stack-of-tasks/pinocchio), [eiquadprog](https://github.com/stack-of-tasks/eiquadprog) QP solver, and fully written in C++ with Python bindings, allowing fast prototyping with good runtime performances. It features task-space inverse kinematics and dynamics (see below) high-level API for whole-body control tasks.
+
+### Task-Space Inverse Kinematics
+
+[![Quadruoped demo](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/kinematics/videos/quadruped_targets.mp4?raw=true)
+
+High-level API to specify tasks for constrained inverse kinematics (IK).
+
+- [See documentation](https://placo.readthedocs.io/en/latest/kinematics/getting_started.html)
+- [Examples](https://placo.readthedocs.io/en/latest/kinematics/examples_gallery.html)
+
+### Task-Space Inverse Dynamics
+
+[![Megabot demo](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.gif?raw=true)](https://github.com/Rhoban/placo-examples/blob/master/dynamics/videos/megabot.mp4?raw=true)
+
+High-level API to specify tasks for constrained inverse dynamics (ID).
+
+- [See documentation](https://placo.readthedocs.io/en/latest/dynamics/getting_started.html)
+- [Examples](https://placo.readthedocs.io/en/latest/dynamics/examples_gallery.html)
+
+
+## Installing
+
+PlaCo can be installed from ``pip``
+
+```
+pip install placo
+```
+
+Or [built from sources](https://placo.readthedocs.io/en/latest/basics/installation_source.html)
+
+## Resources
+
+* [Documentation](https://placo.readthedocs.io/en/latest/)
+* [Examples](https://github.com/rhoban/placo-examples) repository

placo-0.9.15.dist-info/RECORD

@@ -0,0 +1,12 @@
+cmeel.prefix/lib/liblibplaco.dylib,sha256=B3K2ZL1cOLh4GsNkvF_cjOcEx9JDV4movMffJa_Df0Q,1359504
+cmeel.prefix/lib/python3.11/site-packages/placo.pyi,sha256=igje8A4_ATZzaE5C8hOAdOxgCVm0qvMcsZwSQV9Jv78,162865
+cmeel.prefix/lib/python3.11/site-packages/placo.so,sha256=jJ6RzRL0NyltTwAjoRUixOOaI9bfgwb2l1f9z3ZkXdc,6682888
+cmeel.prefix/lib/python3.11/site-packages/placo_utils/__init__.py,sha256=UN-fc5KfBWQ-_qkm0Ajouh-T9tBGm5aUtuzBiH1tRtk,80
+cmeel.prefix/lib/python3.11/site-packages/placo_utils/tf.py,sha256=fFRXNbeLlXzn5VOqYl7hcSuvOOtTDTiLi_Lpd9_l6wA,36
+cmeel.prefix/lib/python3.11/site-packages/placo_utils/view.py,sha256=7KiLYGpTKaPJtFHZ6kjERdOzJiPSDUtkIKHbziHpkYk,928
+cmeel.prefix/lib/python3.11/site-packages/placo_utils/visualization.py,sha256=z0IEIr4DW6qOwJ2ofEsm_JKx_Do2FF7wZY8Wt_7aqxM,9923
+placo-0.9.15.dist-info/licenses/LICENSE,sha256=q2bBXvk4Eh7TmP11LoIOIGSUuJbR30JBI6ZZ37g52T4,1061
+placo-0.9.15.dist-info/METADATA,sha256=NdaXI4FUVDlqvJ3P7ddsy-tRdrLs5_4_pgB3tzoDHvA,2623
+placo-0.9.15.dist-info/WHEEL,sha256=t66Vmq09ySSuYNkvxj8pPRXN3x8PGEstXQgBdYltQZc,111
+placo-0.9.15.dist-info/top_level.txt,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+placo-0.9.15.dist-info/RECORD,,

placo-0.9.15.dist-info/WHEEL

@@ -0,0 +1,6 @@
+Wheel-Version: 1.0
+Generator: cmeel 0.57.3
+Root-Is-Purelib: false
+Tag: cp311-cp311-macosx_11_0_arm64
+Build: 0
+

placo-0.9.15.dist-info/licenses/LICENSE

@@ -0,0 +1,19 @@
+Copyright (c) <2023-2099> Rhoban Team
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is furnished
+to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.