Merge branch 'passivity_based_rnea_dev' of github.com:Cfather/pinocch…

…io into passivity_based_rnea_dev

.github/workflows/scripts/get_label.js

@@ -34,7 +34,7 @@ module.exports = async ({github, context, core}) => {
             ' -DBUILD_PYTHON_BINDINGS_WITH_BOOST_MPFR_SUPPORT=ON',
             ' -DINSTALL_DOCUMENTATION=ON',
             ' -DGENERATE_PYTHON_STUBS=ON',
-            ' -DBUILD_WITH_ACCELERATE_SUPPORT=ON'
+            ' -DBUILD_WITH_ACCELERATE_SUPPORT=OFF'
         ],
         build_collision: ' -DBUILD_WITH_COLLISION_SUPPORT=ON',
         build_casadi: ' -DBUILD_WITH_CASADI_SUPPORT=ON',

CHANGELOG.md

@@ -9,11 +9,17 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ### Added
 
 - Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419))
+- Add more Python and C++ examples related to inverse kinematics with 3d tasks ([#2428](https://github.com/stack-of-tasks/pinocchio/pull/2428))
 
 ### 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)
+- Fix class abtract error for Rviz viewer ([#2425](https://github.com/stack-of-tasks/pinocchio/pull/2425))
+
+### Changed
+
+- Modernize python code base with ruff ([#2418](https://github.com/stack-of-tasks/pinocchio/pull/2418))
 
 ## [3.2.0] - 2024-08-27
 

bindings/python/pinocchio/deprecated.py

@@ -2,6 +2,5 @@
 # Copyright (c) 2018-2023 CNRS INRIA
 #
 
-## In this file, are reported some deprecated functions that are still maintained until the next important future releases ##
-
-from __future__ import print_function
+## In this file, are reported some deprecated functions
+# that are still maintained until the next important future releases

bindings/python/pinocchio/deprecation.py

@@ -20,9 +20,7 @@ def decorator(func):
 
         @functools.wraps(func)
         def wrapper(*args, **kwargs):
-            message = "Call to deprecated function {}. {}".format(
-                func.__name__, instructions
-            )
+            message = f"Call to deprecated function {func.__name__}. {instructions}"
 
             warnings.warn(message, category=DeprecatedWarning, stacklevel=2)
 

bindings/python/pinocchio/derivative/dcrba.py

@@ -2,19 +2,14 @@
 # Copyright (c) 2016 CNRS
 #
 
-from __future__ import print_function
 
+import lambdas
 import numpy as np
 import pinocchio as pin
-from numpy.linalg import norm
-from pinocchio.robot_wrapper import RobotWrapper
-from pinocchio.utils import rand
-
-import lambdas
 from lambdas import (
     FCross,
-    Mcross,
     MCross,
+    Mcross,
     adj,
     adjdual,
     ancestors,
@@ -23,6 +18,9 @@
     quad,
     td,
 )
+from numpy.linalg import norm
+from pinocchio.robot_wrapper import RobotWrapper
+from pinocchio.utils import rand
 
 
 def hessian(robot, q, crossterms=False):
@@ -111,9 +109,12 @@ def __call__(self):
                         T_jSd = np.array(H[:, d, j0:j1])  # this is 0 is d<=j
 
                         """
-                        assert( norm(T_iSd)<1e-6 or not joint_diff<i )  # d<i => TiSd=0
-                        assert( norm(T_jSd)<1e-6 or not joint_diff<j )  # d<j => TjSd=0
-                        assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 )  # TiSd=0 => TjSd=0
+                        # d<i => TiSd=0
+                        assert( norm(T_iSd)<1e-6 or not joint_diff<i )
+                        # d<j => TjSd=0
+                        assert( norm(T_jSd)<1e-6 or not joint_diff<j )
+                        # TiSd=0 => TjSd=0
+                        assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 )
                         assert( norm(T_iSd)>1e-6 )
                         """
 
@@ -123,7 +124,8 @@ def __call__(self):
                         if j < joint_diff:
                             dM[i0:i1, j0:j1, d] += Si.T * Yd * T_jSd
 
-                        # Make dM triangular by copying strict-upper triangle to lower one.
+                        # Make dM triangular by copying strict-upper triangle to lower
+                        # one.
                         if i != j:
                             dM[j0:j1, i0:i1, d] = dM[i0:i1, j0:j1, d].T
                         else:
@@ -203,7 +205,8 @@ def __call__(self, q):
                         H[:, k0:k1, j0:j1], YJ[:, i0:i1], [0, 0]
                     )
 
-            # Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk<k
+            # Fill the border elements of levels below k
+            # Q[kk,k,:] and Q[kk,:,k] with kk<k
             for kk in ancestors(k)[:-1]:
                 kk0, kk1 = iv(kk)[0], iv(kk)[-1] + 1
                 _Skk = J[:, kk0:kk1]
@@ -377,7 +380,7 @@ def adjf(f):
                 Tkf = Yci * (-MCross(Sk, a[lk]) + MCross(MCross(Sk, v[lk]), v[lk]))
 
                 # Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
-                #           =      ""           + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
+                #           =      ""           + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)  # noqa E501
                 Tkf += Yvx[i] * MCross(Sk, v[lk])
 
                 R[i0:i1, k0:k1] = Si.T * Tkf

bindings/python/pinocchio/derivative/lambdas.py

@@ -4,7 +4,7 @@
 # ruff: noqa: E731
 
 import numpy as np
-from pinocchio import Motion, Force, skew
+from pinocchio import Force, Motion, skew
 from pinocchio.utils import zero
 
 
@@ -25,16 +25,13 @@ def jFromIdx(idxv, robot):
         robot.model.joints[i].idx_v + robot.model.joints[i].nv,
     )
 )
-ancestors = (
-    lambda j, robot, res=[]: res
+ancestors = lambda j, robot, res=[]: (
+    res
     if j == 0
     else ancestors(
         robot.model.parents[j],
         robot,
-        [
-            j,
-        ]
-        + res,
+        [j, *res],
     )
 )
 
@@ -53,12 +50,13 @@ def __contains__(self, anc):
                 dec = self.robot.model.parents[dec]
 
 
-# descendants = lambda root,robot: filter( lambda i: root in ancestorOf(i,robot), range(root,robot.model.njoints) )
+# descendants = lambda root,robot: filter(
+# lambda i: root in ancestorOf(i,robot), range(root,robot.model.njoints) )
 descendants = lambda root, robot: robot.model.subtrees[root]
 
 
 def setRobotArgs(robot):
-    ancestors.__defaults__ = (robot,) + ancestors.__defaults__
+    ancestors.__defaults__ = (robot, *ancestors.__defaults__)
     descendants.__defaults__ = (robot,)
     # ancestorsOf.__init__.__defaults__ = (robot,)
     iv.__defaults__ = (robot,)
@@ -91,7 +89,7 @@ def setRobotArgs(robot):
 
 
 def np_prettyprint(sarg="{: 0.5f}", eps=5e-7):
-    mformat = (
-        lambda x, sarg=sarg, eps=eps: sarg.format(x) if abs(x) > eps else " 0.     "
+    mformat = lambda x, sarg=sarg, eps=eps: (
+        sarg.format(x) if abs(x) > eps else " 0.     "
     )
     np.set_printoptions(formatter={"float": mformat})

bindings/python/pinocchio/derivative/xm.py

@@ -4,12 +4,11 @@
 
 import numpy as np
 import pinocchio as pin
+from dcrba import DCRBA, DRNEA, Coriolis
 from numpy.linalg import norm
 from pinocchio.robot_wrapper import RobotWrapper
 from pinocchio.utils import rand
 
-from dcrba import DCRBA, DRNEA, Coriolis
-
 np.random.seed(0)
 
 robot = RobotWrapper(

bindings/python/pinocchio/robot_wrapper.py

@@ -12,7 +12,7 @@
 )
 
 
-class RobotWrapper(object):
+class RobotWrapper:
     @staticmethod
     def BuildFromURDF(
         filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None
@@ -160,13 +160,16 @@ def centroidalMomentum(self, q, v):
 
     def centroidalMap(self, q):
         """
-        Computes the centroidal momentum matrix which maps from the joint velocity vector to the centroidal momentum expressed around the center of mass.
+        Computes the centroidal momentum matrix which maps from the joint velocity
+        vector to the centroidal momentum expressed around the center of mass.
         """
         return pin.computeCentroidalMap(self.model, self.data, q)
 
     def centroidal(self, q, v):
         """
-        Computes all the quantities related to the centroidal dynamics (hg, Ag and Ig), corresponding to the centroidal momentum, the centroidal map and the centroidal rigid inertia.
+        Computes all the quantities related to the centroidal dynamics (hg, Ag and Ig),
+        corresponding to the centroidal momentum, the centroidal map and the centroidal
+        rigid inertia.
         """
         pin.ccrba(self.model, self.data, q, v)
         return (self.data.hg, self.data.Ag, self.data.Ig)
@@ -350,15 +353,16 @@ def buildReducedRobot(self, list_of_joints_to_lock, reference_configuration=None
 
     def getFrameJacobian(self, frame_id, rf_frame=pin.ReferenceFrame.LOCAL):
         """
-        It computes the Jacobian of frame given by its id (frame_id) either expressed in the
-        local coordinate frame or in the world coordinate frame.
+        It computes the Jacobian of frame given by its id (frame_id) either expressed in
+        the local coordinate frame or in the world coordinate frame.
         """
         return pin.getFrameJacobian(self.model, self.data, frame_id, rf_frame)
 
     def computeFrameJacobian(self, q, frame_id):
         """
         Similar to getFrameJacobian but does not need pin.computeJointJacobians and
-        pin.updateFramePlacements to update internal value of self.data related to frames.
+        pin.updateFramePlacements to update internal value of self.data related to
+        frames.
         """
         return pin.computeFrameJacobian(self.model, self.data, q, frame_id)
 
@@ -396,8 +400,10 @@ def viewer(self):
         return self.viz.viewer
 
     def setVisualizer(self, visualizer, init=True, copy_models=False):
-        """Set the visualizer. If init is True, the visualizer is initialized with this wrapper's models.
-        If copy_models is also True, the models are copied. Otherwise, they are simply kept as a reference.
+        """
+        Set the visualizer. If init is True, the visualizer is initialized with this
+        wrapper's models.  If copy_models is also True, the models are copied.
+        Otherwise, they are simply kept as a reference.
         """
         if init:
             visualizer.__init__(
@@ -406,7 +412,10 @@ def setVisualizer(self, visualizer, init=True, copy_models=False):
         self.viz = visualizer
 
     def getViewerNodeName(self, geometry_object, geometry_type):
-        """For each geometry object, returns the corresponding name of the node in the display."""
+        """
+        For each geometry object, returns the corresponding name of the node in the
+        display.
+        """
         return self.viz.getViewerNodeName(geometry_object, geometry_type)
 
     def initViewer(self, share_data=True, *args, **kwargs):
@@ -437,7 +446,9 @@ def loadViewerModel(self, *args, **kwargs):
         self.viz.loadViewerModel(*args, **kwargs)
 
     def display(self, q):
-        """Display the robot at configuration q in the viewer by placing all the bodies."""
+        """
+        Display the robot at configuration q in the viewer by placing all the bodies.
+        """
         self.viz.display(q)
 
     def displayCollisions(self, visibility):

bindings/python/pinocchio/romeo_wrapper.py

@@ -68,7 +68,9 @@ def __init__(self, filename, package_dirs=None, verbose=False):
 
         for op, name in self.opCorrespondances.items():
             self.__dict__[op] = self.index(name)
-            # self.__dict__['_M'+op] = types.MethodType(lambda s, q: s.position(q,idx),self)
+            # self.__dict__["_M" + op] = types.MethodType(
+            # lambda s, q: s.position(q, idx), self
+            # )
 
     # --- SHORTCUTS ---
     def Mrh(self, q):

bindings/python/pinocchio/shortcuts.py

@@ -1,13 +1,15 @@
+# ruff: noqa: E501
 #
 # Copyright (c) 2018-2020 CNRS INRIA
 #
 
 ## In this file, some shortcuts are provided ##
 
-from . import pinocchio_pywrap_default as pin
-from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS
 from typing import Tuple
 
+from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS
+from . import pinocchio_pywrap_default as pin
+
 nle = pin.nonLinearEffects
 
 
@@ -77,8 +79,10 @@ def buildModelsFromUrdf(
 
 
 def createDatas(*models):
-    """Call createData() on each Model or GeometryModel in input and return the results in a tuple.
-    If one of the models is None, the corresponding data object in the result is also None.
+    """
+    Call createData() on each Model or GeometryModel in input and return the results in
+    a tuple. If one of the models is None, the corresponding data object in the result
+    is also None.
     """
     return tuple([None if model is None else model.createData() for model in models])
 
@@ -120,7 +124,8 @@ def buildModelsFromSdf(
         )
     if verbose and not WITH_HPP_FCL_BINDINGS and meshLoader is not None:
         print(
-            "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed."
+            "Info: MeshLoader is ignored. "
+            "The HPP-FCL Python bindings have not been installed."
         )
     if package_dirs is None:
         package_dirs = []
@@ -173,7 +178,8 @@ def buildModelsFromMJCF(
         )
     if verbose and not WITH_HPP_FCL_BINDINGS and meshLoader is not None:
         print(
-            "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed."
+            "Info: MeshLoader is ignored. "
+            "The HPP-FCL Python bindings have not been installed."
         )
 
     lst = [model]

bindings/python/pinocchio/utils.py

@@ -76,7 +76,7 @@ def mprint(M, name="ans", eps=1e-15):
         cmin = i * 6
         cmax = (i + 1) * 6
         cmax = ncol if ncol < cmax else cmax
-        print("Columns %s through %s" % (cmin, cmax - 1))
+        print(f"Columns {cmin} through {cmax - 1}")
         print()
         for r in range(M.shape[0]):
             sys.stdout.write("  ")