diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 7f0f6eae7..53a11fcf6 100644 --- a/.github/workflows/macos.yml +++ b/.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 diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 005570845..77a3e0d37 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -478,52 +478,6 @@ namespace jiminy::pinocchio_overload return data.ddq; } - template - struct ForwardKinematicsAccelerationStep : - public pinocchio::fusion::JointUnaryVisitorBase< - ForwardKinematicsAccelerationStep> - { - typedef boost::fusion::vector &> - ArgsType; - - template - static void algo(const pinocchio::JointModelBase & jmodel, - pinocchio::JointDataBase & jdata, - const pinocchio::Model & model, - pinocchio::Data & data, - const Eigen::MatrixBase & 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 - inline void forwardKinematicsAcceleration(const pinocchio::Model & model, - pinocchio::Data & data, - const Eigen::MatrixBase & a) - { - typedef ForwardKinematicsAccelerationStep 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 hresult_t computeJMinvJt(const pinocchio::Model & model, pinocchio::Data & data, diff --git a/core/src/engine/engine_multi_robot.cc b/core/src/engine/engine_multi_robot.cc index 17a1ec136..74666fe93 100644 --- a/core/src/engine/engine_multi_robot.cc +++ b/core/src/engine/engine_multi_robot.cc @@ -920,6 +920,23 @@ namespace jiminy isTelemetryConfigured_ = false; } + struct ForwardKinematicsAccelerationStep : + public pinocchio::fusion::JointUnaryVisitorBase + { + typedef boost::fusion::vector ArgsType; + + template + static void algo(const pinocchio::JointModelBase & jmodel, + pinocchio::JointDataBase & 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: @@ -927,7 +944,8 @@ namespace jiminy /// /// 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 & systems, - const vector_aligned_t & systemsDataHolder) + const vector_aligned_t & systemsDataHolder, + vector_aligned_t & 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,13 +3164,13 @@ 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: @@ -3125,7 +3178,7 @@ namespace jiminy 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; } } diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index 08c902a34..9c36dd14f 100644 --- a/core/src/robot/model.cc +++ b/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( diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 647db841e..4735e4d7a 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/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, diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 06e2333d7..b6d52336b 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/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