Merge branch 'devel' into master

Author: lvjonok

.github/workflows/nix.yml

@@ -11,7 +11,7 @@ jobs:
         os: [ubuntu, macos]
     steps:
       - uses: actions/checkout@v4
-      - uses: cachix/install-nix-action@v27
+      - uses: cachix/install-nix-action@V28
       - uses: cachix/cachix-action@v15
         with:
           name: gepetto

CHANGELOG.md

@@ -6,9 +6,14 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Added
+
+- Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419))
+
 ### Fixed
 - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400))
 - Fix mjcf parser appending of inertias at root joint ([#2403](https://github.com/stack-of-tasks/pinocchio/pull/2403))
+- Fix unit tests with GCC 13.3 ([#2406](https://github.com/stack-of-tasks/pinocchio/pull/2416)
 
 ## [3.2.0] - 2024-08-27
 

bindings/python/CMakeLists.txt

@@ -224,8 +224,14 @@ if(BUILD_PYTHON_INTERFACE)
 
   # --- INSTALL VISUALIZATION SCRIPTS
   install_python_files(
-    MODULE visualize FILES __init__.py base_visualizer.py gepetto_visualizer.py
-                           meshcat_visualizer.py panda3d_visualizer.py rviz_visualizer.py)
+    MODULE visualize
+    FILES __init__.py
+          base_visualizer.py
+          gepetto_visualizer.py
+          meshcat_visualizer.py
+          panda3d_visualizer.py
+          rviz_visualizer.py
+          visualizers.py)
 
   # --- STUBS --- #
   if(GENERATE_PYTHON_STUBS)

bindings/python/pinocchio/robot_wrapper.py

@@ -413,14 +413,14 @@ def initViewer(self, share_data=True, *args, **kwargs):
         """Init the viewer"""
         # Set viewer to use to MeshCat.
         if self.viz is None:
-            from .visualize import MeshcatVisualizer
+            from .visualize import Visualizer
 
             data, collision_data, visual_data = None, None, None
             if share_data:
                 data = self.data
                 collision_data = self.collision_data
                 visual_data = self.visual_data
-            self.viz = MeshcatVisualizer(
+            self.viz = Visualizer.default()(
                 self.model,
                 self.collision_model,
                 self.visual_model,

bindings/python/pinocchio/visualize/__init__.py

@@ -4,3 +4,4 @@
 from .meshcat_visualizer import MeshcatVisualizer
 from .panda3d_visualizer import Panda3dVisualizer
 from .rviz_visualizer import RVizVisualizer
+from .visualizers import Visualizer

bindings/python/pinocchio/visualize/visualizers.py

@@ -0,0 +1,55 @@
+from enum import Enum
+from importlib.util import find_spec
+from os import environ
+
+from .base_visualizer import BaseVisualizer
+from .gepetto_visualizer import GepettoVisualizer
+from .meshcat_visualizer import MeshcatVisualizer
+from .panda3d_visualizer import Panda3dVisualizer
+from .rviz_visualizer import RVizVisualizer
+
+
+class Visualizer(Enum):
+    BASE = BaseVisualizer
+    GEPETTO = GepettoVisualizer
+    MESHCAT = MeshcatVisualizer
+    PANDA3D = Panda3dVisualizer
+    RVIZ = RVizVisualizer
+
+    @classmethod
+    def default(cls):
+        """
+        Allow user to choose their prefered viewer with eg.
+        export PINOCCHIO_VIEWER=RVIZ.
+
+        Otherwise, try to find one which is installed.
+        """
+        # Allow user to define which viewer they want
+        if "PINOCCHIO_VIEWER" in environ:
+            selected = environ["PINOCCHIO_VIEWER"].upper()
+            if hasattr(cls, selected):
+                return getattr(cls, selected).value
+            err = (
+                f"The visualizer '{selected}' is not available.\n"
+                "Please set PINOCCHIO_VIEWER to something installed among:\n"
+                "- meshcat\n"
+                "- gepetto-viewer\n"
+                "- panda3d\n"
+                "- rviz\n"
+            )
+            raise ImportError(err)
+
+        # Otherwise, use the first available
+        for v in ["meshcat", "gepetto", "panda3d_viewer", "rviz"]:
+            if find_spec(v) is not None:
+                return getattr(cls, v.replace("_viewer", "").upper()).value
+
+        err = (
+            "No visualizer could be found.\n"
+            "Please install one of those:\n"
+            "- meshcat\n"
+            "- gepetto-viewer\n"
+            "- panda3d\n"
+            "- rviz\n"
+        )
+        raise ImportError(err)

doc/d-practical-exercises/3-invkine.md

@@ -152,7 +152,29 @@ the null space of task 1 is:
 
 \f$P_1 = I_9 - J_1^+ J_1\f$
 
-\f$vq_2 = vq_1 + P_1 (J_2 P_1)^+ ( v_2^* - J_2 vq_1)\f$
+\f$vq_2 = vq_1 + P_1 (J_2 P_1)^+ (v_2^* - J_2 vq_1)\f$
+
+With the mathematical property
+
+\f$(J_2 P_1)^+ = P_1 (J_2 P_1)^+\f$
+
+we can save a matrix multiplication:
+
+\f$vq_2 = vq_1 + (J_2 P_1)^+ (v_2^* - J_2 vq_1)\f$
+
+It is important to note that the null space of `J1` is defined up to a numeric
+threshold on its eigen values. A low threshold will lead to a trivial null space
+of `J1`, and task 2 could numerically interfere with task 1. It translates into
+setting an appropriate `rcond` or `rtol` when computing the pseudo-inverse of
+`J2 P1`:
+
+```py
+pinv_J2_P1 = pinv(J2 @ P1, rcond=1e-3)
+```
+
+Tuning `rcond` is a tradeoff: if `rcond` is too low (its default is `1e-15`),
+task 2 could interfere with task 1. If it is too high, there won't be enough
+DoFs to complete task 2.
 
 #### Question 2
 

flake.lock

@@ -3,9 +3,7 @@
 
   inputs = {
     flake-parts.url = "github:hercules-ci/flake-parts";
-    #nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable";
-    # use gepetto fork until https://github.com/NixOS/nixpkgs/pull/337942
-    nixpkgs.url = "github:gepetto/nixpkgs";
+    nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable";
   };
 
   outputs =
@@ -22,7 +20,11 @@
           devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; };
           packages = {
             default = self'.packages.pinocchio;
-            pinocchio = pkgs.python3Packages.pinocchio.overrideAttrs (_: {
+            pinocchio = pkgs.python3Packages.pinocchio.overrideAttrs (super: {
+              # avoid SIGTRAP on macos github runners
+              cmakeFlags = super.cmakeFlags ++ pkgs.lib.optionals pkgs.stdenv.isDarwin [
+                "-DCMAKE_CTEST_ARGUMENTS=--exclude-regex;pinocchio-example-py-casadi-quadrotor-ocp"
+              ];
               src = pkgs.lib.fileset.toSource {
                 root = ./.;
                 fileset = pkgs.lib.fileset.unions [

flake.nix

@@ -742,9 +742,10 @@ namespace pinocchio
         if (current1_id > current2_id)
         {
           const JointModel & joint1 = model.joints[current1_id];
+          const int j1nv = joint1.nv();
           joint1_span_indexes.push_back((Eigen::DenseIndex)current1_id);
           Eigen::DenseIndex current1_col_id = joint1.idx_v();
-          for (int k = 0; k < joint1.nv(); ++k, ++current1_col_id)
+          for (int k = 0; k < j1nv; ++k, ++current1_col_id)
           {
             colwise_joint1_sparsity[current1_col_id] = true;
           }
@@ -753,9 +754,10 @@ namespace pinocchio
         else
         {
           const JointModel & joint2 = model.joints[current2_id];
+          const int j2nv = joint2.nv();
           joint2_span_indexes.push_back((Eigen::DenseIndex)current2_id);
           Eigen::DenseIndex current2_col_id = joint2.idx_v();
-          for (int k = 0; k < joint2.nv(); ++k, ++current2_col_id)
+          for (int k = 0; k < j2nv; ++k, ++current2_col_id)
           {
             colwise_joint2_sparsity[current2_col_id] = true;
           }
@@ -770,10 +772,11 @@ namespace pinocchio
         while (current_id > 0)
         {
           const JointModel & joint = model.joints[current_id];
+          const int jnv = joint.nv();
           joint1_span_indexes.push_back((Eigen::DenseIndex)current_id);
           joint2_span_indexes.push_back((Eigen::DenseIndex)current_id);
           Eigen::DenseIndex current_row_id = joint.idx_v();
-          for (int k = 0; k < joint.nv(); ++k, ++current_row_id)
+          for (int k = 0; k < jnv; ++k, ++current_row_id)
           {
             colwise_joint1_sparsity[current_row_id] = true;
             colwise_joint2_sparsity[current_row_id] = true;