[core] Fix internal efforts computation.

.github/workflows/macos.yml

@@ -12,7 +12,7 @@ jobs:
     name: >-
       (${{ matrix.PYTHON_VERSION }}) (${{ matrix.BUILD_TYPE }})
       Build and run the unit tests. Then generate and publish the wheels on PyPi.
-    runs-on: macos-11
+    runs-on: macos-13
 
     strategy:
       matrix:
@@ -30,8 +30,8 @@ jobs:
       TORCH_VERSION: "2.0.1"
       CMAKE_C_COMPILER: "/usr/bin/clang"
       CMAKE_CXX_COMPILER: "/usr/bin/clang++"
-      CMAKE_CXX_FLAGS: "-DEIGEN_MPL2_ONLY -Wno-deprecated-declarations"
-      OSX_DEPLOYMENT_TARGET: "10.15"
+      CMAKE_CXX_FLAGS: "-DEIGEN_MPL2_ONLY -D_LIBCPP_ENABLE_CXX17_REMOVED_UNARY_BINARY_FUNCTION -Wno-deprecated-declarations"
+      OSX_DEPLOYMENT_TARGET: "11.0"
       OSX_ARCHITECTURES: "x86_64;arm64"
       WHEEL_ARCH: "universal2"
 
@@ -108,8 +108,8 @@ jobs:
 
     - name: Generate and install Python Pip wheels
       run: |
-        export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib"
-        export DYLD_FALLBACK_LIBRARY_PATH="$LD_LIBRARY_PATH"
+        export LD_LIBRARY_PATH="${InstallDir}/lib:${DYLD_LIBRARY_PATH}"
+        export DYLD_FALLBACK_LIBRARY_PATH="${LD_LIBRARY_PATH}"
 
         # Generate stubs
         stubgen -p jiminy_py -o ${RootDir}/build/pypi/jiminy_py/src
@@ -139,7 +139,8 @@ jobs:
 
     - name: Build extension module
       run: |
-        export LD_LIBRARY_PATH="${InstallDir}/lib/:/usr/local/lib"
+        export DYLD_FALLBACK_LIBRARY_PATH="${InstallDir}/lib"
+
         "${InstallDir}/bin/jiminy_double_pendulum"
 
         mkdir -p "${RootDir}/core/examples/external_project/build"
@@ -156,6 +157,8 @@ jobs:
 
     - name: Run unit tests for jiminy
       run: |
+        export DYLD_FALLBACK_LIBRARY_PATH="${InstallDir}/lib"
+
         cd "${RootDir}/build/core/unit"
         ctest --output-on-failure
 

core/include/jiminy/core/robot/pinocchio_overload_algorithms.h

@@ -478,52 +478,6 @@ namespace jiminy::pinocchio_overload
         return data.ddq;
     }
 
-    template<typename TangentVectorType>
-    struct ForwardKinematicsAccelerationStep :
-    public pinocchio::fusion::JointUnaryVisitorBase<
-        ForwardKinematicsAccelerationStep<TangentVectorType>>
-    {
-        typedef boost::fusion::vector<const pinocchio::Model &,
-                                      pinocchio::Data &,
-                                      const Eigen::MatrixBase<TangentVectorType> &>
-            ArgsType;
-
-        template<typename JointModel>
-        static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
-                         pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
-                         const pinocchio::Model & model,
-                         pinocchio::Data & data,
-                         const Eigen::MatrixBase<TangentVectorType> & a)
-        {
-            pinocchio::JointIndex i = jmodel.id();
-            pinocchio::JointIndex parent = model.parents[i];
-            data.a[i] =
-                jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
-            if (parent > 0)
-            {
-                data.a[i] += data.liMi[i].actInv(data.a[parent]);
-            }
-        }
-    };
-
-    /// \brief Compute only joints spatial accelerations, assuming positions and velocities
-    ///        are already up-to-date.
-    ///
-    /// \warning This method does not update the internal buffer `data.ddq`. This buffer is only
-    ///          updated by `aba` and `forwardDynamics` algorithms.
-    template<typename TangentVectorType>
-    inline void forwardKinematicsAcceleration(const pinocchio::Model & model,
-                                              pinocchio::Data & data,
-                                              const Eigen::MatrixBase<TangentVectorType> & a)
-    {
-        typedef ForwardKinematicsAccelerationStep<TangentVectorType> Pass;
-        data.a[0].setZero();
-        for (int32_t i = 1; i < model.njoints; ++i)
-        {
-            Pass::run(model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, a));
-        }
-    }
-
     template<typename JacobianType>
     hresult_t computeJMinvJt(const pinocchio::Model & model,
                              pinocchio::Data & data,

core/src/engine/engine_multi_robot.cc

@@ -920,14 +920,32 @@ namespace jiminy
         isTelemetryConfigured_ = false;
     }
 
+    struct ForwardKinematicsAccelerationStep :
+    public pinocchio::fusion::JointUnaryVisitorBase<ForwardKinematicsAccelerationStep>
+    {
+        typedef boost::fusion::vector<pinocchio::Data &, const Eigen::VectorXd &> ArgsType;
+
+        template<typename JointModel>
+        static void algo(const pinocchio::JointModelBase<JointModel> & jmodel,
+                         pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
+                         pinocchio::Data & data,
+                         const Eigen::VectorXd & a)
+        {
+            pinocchio::JointIndex i = jmodel.id();
+            data.a[i] = jdata.c() + data.v[i].cross(jdata.v());
+            data.a[i] += jdata.S() * jmodel.jointVelocitySelector(a);
+        }
+    };
+
     /// \details This method is optimized to avoid redundant computations.
     ///
     /// \see See `pinocchio::computeAllTerms` for reference:
     ///      https://github.com/stack-of-tasks/pinocchio/blob/a1df23c2f183d84febdc2099e5fbfdbd1fc8018b/src/algorithm/compute-all-terms.hxx
     ///
     /// Copyright (c) 2014-2020, CNRS
     /// Copyright (c) 2018-2020, INRIA
-    void computeExtraTerms(systemHolder_t & system, const systemDataHolder_t & systemData)
+    void computeExtraTerms(
+        systemHolder_t & system, const systemDataHolder_t & systemData, ForceVector & fExt)
     {
         const pinocchio::Model & model = system.robot->pncModel_;
         pinocchio::Data & data = system.robot->pncData_;
@@ -964,25 +982,52 @@ namespace jiminy
            accelerations, joint forces and body forces, so it must be done separately:
            - 1st step: computing the accelerations based on ForwardKinematic algorithm
            - 2nd step: computing the forces based on RNEA algorithm */
-        pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);
 
-        // Compute the spatial momenta and the sum of external forces acting on each body
+        /* Compute the true joint acceleration and the one due to the lone gravity field, then
+           the spatial momenta and the total internal and external forces acting on each body. */
         data.h[0].setZero();
+        fExt[0].setZero();
         data.f[0].setZero();
+        data.a[0].setZero();
+        data.a_gf[0] = -model.gravity;
         for (int i = 1; i < model.njoints; ++i)
         {
-            data.h[i] = model.inertias[i] * data.v[i];
-            data.f[i] = model.inertias[i] * data.a[i] + data.v[i].cross(data.h[i]);
+            const auto & jmodel = model.joints[i];
+            const pinocchio::JointIndex jointModelIdx = jmodel.id();
+            const pinocchio::JointIndex parentJointModelIdx = model.parents[jointModelIdx];
+
+            ForwardKinematicsAccelerationStep::run(
+                jmodel,
+                data.joints[i],
+                typename ForwardKinematicsAccelerationStep::ArgsType(data, systemData.state.a));
+            data.a_gf[jointModelIdx] = data.a[jointModelIdx];
+            data.a[jointModelIdx] += data.liMi[jointModelIdx].actInv(data.a[parentJointModelIdx]);
+            data.a_gf[jointModelIdx] +=
+                data.liMi[jointModelIdx].actInv(data.a_gf[parentJointModelIdx]);
+
+            model.inertias[jointModelIdx].__mult__(data.v[jointModelIdx], data.h[jointModelIdx]);
+
+            model.inertias[jointModelIdx].__mult__(data.a[jointModelIdx], fExt[jointModelIdx]);
+            data.f[jointModelIdx] = data.v[jointModelIdx].cross(data.h[jointModelIdx]);
+            fExt[jointModelIdx] += data.f[jointModelIdx];
+            data.f[jointModelIdx] += model.inertias[jointModelIdx] * data.a_gf[jointModelIdx];
+            data.f[jointModelIdx] -= systemData.state.fExternal[jointModelIdx];
         }
         for (int i = model.njoints - 1; i > 0; --i)
         {
-            const pinocchio::JointIndex parentJointModelIdx = model.parents[i];
-            data.h[parentJointModelIdx] += data.liMi[i].act(data.h[i]);
-            data.f[parentJointModelIdx] += data.liMi[i].act(data.f[i]);
+            const auto & jmodel = model.joints[i];
+            const pinocchio::JointIndex jointModelIdx = jmodel.id();
+            const pinocchio::JointIndex parentJointModelIdx = model.parents[jointModelIdx];
+
+            fExt[parentJointModelIdx] += data.liMi[jointModelIdx].act(fExt[jointModelIdx]);
+            data.h[parentJointModelIdx] += data.liMi[jointModelIdx].act(data.h[jointModelIdx]);
+            if (parentJointModelIdx > 0)
+            {
+                data.f[parentJointModelIdx] += data.liMi[jointModelIdx].act(data.f[jointModelIdx]);
+            }
         }
 
-        /* Now that `data.Ycrb` and `data.h` are available, one can get directly the position and
-           velocity of the center of mass of each subtrees. */
+        // Compute the position and velocity of the center of mass of each subtree
         for (int i = 0; i < model.njoints; ++i)
         {
             data.com[i] = data.Ycrb[i].lever();
@@ -994,18 +1039,20 @@ namespace jiminy
         // Compute centroidal dynamics and its derivative
         data.hg = data.h[0];
         data.hg.angular() += data.hg.linear().cross(data.com[0]);
-        data.dhg = data.f[0];
+        data.dhg = fExt[0];
         data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
     }
 
     void computeAllExtraTerms(std::vector<systemHolder_t> & systems,
-                              const vector_aligned_t<systemDataHolder_t> & systemsDataHolder)
+                              const vector_aligned_t<systemDataHolder_t> & systemsDataHolder,
+                              vector_aligned_t<ForceVector> & f)
     {
         auto systemIt = systems.begin();
         auto systemDataIt = systemsDataHolder.begin();
-        for (; systemIt != systems.end(); ++systemIt, ++systemDataIt)
+        auto fIt = f.begin();
+        for (; systemIt != systems.end(); ++systemIt, ++systemDataIt, ++fIt)
         {
-            computeExtraTerms(*systemIt, *systemDataIt);
+            computeExtraTerms(*systemIt, *systemDataIt, *fIt);
         }
     }
 
@@ -1584,8 +1631,10 @@ namespace jiminy
                 systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
             }
 
-            // Compute joints accelerations and forces
-            computeAllExtraTerms(systems_, systemsDataHolder_);
+            // Compute all external terms including joints accelerations and forces if requested
+            computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);
+
+            // Backend the updated joint accelerations and forces
             syncAllAccelerationsAndForces(systems_, contactForcesPrev_, fPrev_, aPrev_);
 
             // Synchronize the global stepper state with the individual system states
@@ -2214,15 +2263,17 @@ namespace jiminy
                         // Reset successive iteration failure counter
                         successiveIterFailed = 0;
 
-                        /* Compute the actual joint acceleration and forces.
-                           Note that it is possible to call this method because pinocchio::Data is
-                           guaranteed to be up-to-date at this point. */
-                        computeAllExtraTerms(systems_, systemsDataHolder_);
+                        // Synchronize the position, velocity and acceleration of all systems
+                        syncSystemsStateWithStepper();
+
+                        /* Compute all external terms including joints accelerations and forces.
+                           Note that it is possible to call this method because `pinocchio::Data`
+                           is guaranteed to be up-to-date at this point. */
+                        computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);
 
-                        // Synchronize the individual system states
+                        // Backend the updated joint accelerations and forces
                         syncAllAccelerationsAndForces(
                             systems_, contactForcesPrev_, fPrev_, aPrev_);
-                        syncSystemsStateWithStepper();
 
                         // Increment the iteration counter only for successful steps
                         ++stepperState_.iter;
@@ -2338,13 +2389,15 @@ namespace jiminy
                         // Reset successive iteration failure counter
                         successiveIterFailed = 0;
 
-                        // Compute the actual joint acceleration and forces
-                        computeAllExtraTerms(systems_, systemsDataHolder_);
+                        // Synchronize the position, velocity and acceleration of all systems
+                        syncSystemsStateWithStepper();
 
-                        // Synchronize the individual system states
+                        // Compute all external terms including joints accelerations and forces
+                        computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);
+
+                        // Backend the updated joint accelerations and forces
                         syncAllAccelerationsAndForces(
                             systems_, contactForcesPrev_, fPrev_, aPrev_);
-                        syncSystemsStateWithStepper();
 
                         // Increment the iteration counter
                         ++stepperState_.iter;
@@ -3093,16 +3146,16 @@ namespace jiminy
         pinocchio::forwardKinematics(model, data, q, v, a);
 
         // Update frame placements (avoiding redundant computations)
-        for (int jointModelIdx = 1; jointModelIdx < model.nframes; ++jointModelIdx)
+        for (int frameIdx = 1; frameIdx < model.nframes; ++frameIdx)
         {
-            const pinocchio::Frame & frame = model.frames[jointModelIdx];
+            const pinocchio::Frame & frame = model.frames[frameIdx];
             pinocchio::JointIndex parentJointModelIdx = frame.parent;
             switch (frame.type)
             {
             case pinocchio::FrameType::JOINT:
                 /* If the frame is associated with an actual joint, no need to compute anything
                    new, since the frame transform is supposed to be identity. */
-                data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx];
+                data.oMf[frameIdx] = data.oMi[parentJointModelIdx];
                 break;
             case pinocchio::FrameType::BODY:
                 if (model.frames[frame.previousFrame].type == pinocchio::FrameType::FIXED_JOINT)
@@ -3111,21 +3164,21 @@ namespace jiminy
                        itself, so no need to compute them twice. Here we are doing the assumption
                        that the previous frame transform has already been updated since it is
                        closer to root in kinematic tree. */
-                    data.oMf[jointModelIdx] = data.oMf[frame.previousFrame];
+                    data.oMf[frameIdx] = data.oMf[frame.previousFrame];
                 }
                 else
                 {
                     /* BODYs connected via JOINT(s) have the identity transform, so copying parent
                        joint transform should be fine. */
-                    data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx];
+                    data.oMf[frameIdx] = data.oMi[parentJointModelIdx];
                 }
                 break;
             case pinocchio::FrameType::FIXED_JOINT:
             case pinocchio::FrameType::SENSOR:
             case pinocchio::FrameType::OP_FRAME:
             default:
                 // Nothing special, doing the actual computation
-                data.oMf[jointModelIdx] = data.oMi[parentJointModelIdx] * frame.placement;
+                data.oMf[frameIdx] = data.oMi[parentJointModelIdx] * frame.placement;
             }
         }
 

core/src/robot/model.cc

@@ -1210,8 +1210,19 @@ namespace jiminy
            Note that it will alter the actual joints spatial accelerations, so it is necessary to
            do a backup first to restore it later on. */
         jointsAcceleration_.swap(pncData_.a);
-        pinocchio_overload::forwardKinematicsAcceleration(
-            pncModel_, pncData_, Eigen::VectorXd::Zero(pncModel_.nv));
+        pncData_.a[0].setZero();
+        for (int i = 1; i < pncModel_.njoints; ++i)
+        {
+            const auto & jmodel = pncModel_.joints[i];
+            const auto & jdata = pncData_.joints[i];
+            const pinocchio::JointIndex jointModelIdx = jmodel.id();
+            const pinocchio::JointIndex parentJointModelIdx = pncModel_.parents[jointModelIdx];
+            pncData_.a[jointModelIdx] = jdata.c() + pncData_.v[jointModelIdx].cross(jdata.v());
+            if (parentJointModelIdx > 0)
+            {
+                pncData_.a[i] += pncData_.liMi[i].actInv(pncData_.a[parentJointModelIdx]);
+            }
+        }
 
         // Compute sequentially the jacobian and drift of each enabled constraint
         constraintsHolder_.foreach(

python/jiminy_py/src/jiminy_py/simulator.py

@@ -490,6 +490,11 @@ def render(self,
         # Instantiate the robot and viewer client if necessary.
         # A new dedicated scene and window will be created.
         if not self.is_viewer_available:
+            # Make sure that the current viewer is properly closed if any
+            if self.viewer is not None:
+                self.viewer.close()
+                self.viewer = None
+
             # Create new viewer instance
             self.viewer = Viewer(
                 self.robot,

python/jiminy_py/src/jiminy_py/viewer/viewer.py

@@ -519,6 +519,11 @@ def __init__(self,  # pylint: disable=unused-argument
             Optional: Root joint for robot with freeflyer by default.
         :param kwargs: Unused extra keyword arguments to enable forwarding.
         """
+        # Define some attribute to avoid raising exception at exit
+        self.robot_name = "_".join((
+            "undefined", next(tempfile._get_candidate_names())))
+        self.__is_open = False
+
         # Make sure the robot is properly initialized
         assert robot.is_initialized, (
             "Robot not initialized. Impossible to instantiate a viewer.")
@@ -758,7 +763,7 @@ def get_com_scale() -> Tuple3FType:
                             remove_if_exists=True,
                             auto_refresh=False,
                             radius=0.004,
-                            length=1.0,
+                            length=-1.0,
                             anchor_bottom=True)
 
             self.display_center_of_mass(self._display_com)
@@ -1119,8 +1124,8 @@ def close(self: Optional[Union["Viewer", Type["Viewer"]]] = None) -> None:
             # Check if the backend process has changed or the viewer instance
             # has already been closed, which may happened if it has been closed
             # manually in the meantime. If so, there is nothing left to do.
-            if (Viewer._backend_proc is not self._backend_proc or
-                    not self.__is_open):
+            if (not self.__is_open or
+                    Viewer._backend_proc is not self._backend_proc):
                 return
 
             # Assert(s) for type checker