Jiminy release 1.5.0 (#266)

* [core] Log the robot input args, robot model, and all options as constants.
* [core] Double space before inline comments.
* [core/python] Simplify C++ to Python log converter.
* [python/log] Fix print logged variables.
* [core] Add constant for telemetry constant delimiter.
* [core] Store constants in hdf5 datasets instead of attributes to get around 64K size limit.
* [core] TMLC telemetry format now supports both attributes and dataset field.
* [python/viewer] Split viewer and replay tools in separated submodules to reduce file complexity.
* [core] Clarity telemetry namespace.
* [gym] Make viewer instance directly accessible in pipeline environment. (#260)
* [python/simulator] Simulator's Viewer instance is no longer private attribute.
* [gym] Get access to simulator's attributes directly.
* [core] More generic flexibilities and fix external forces computation (#263)
* [core] Enable to add flexible joints at any FIXED_JOINT frame.
* [core] Fix body forces wrongly computed by 'aba'.
* [core] Add composite centroidal rigid-body inertia computation.
* [core/python] Fix bindings of some system_state object fields.
* [python/robot] Fix support of multiple collision boxes
* [python/robot] Fixes some edge cases when automatically generating HDF file.
* [python/viewer] Add partial support of Panda3d visualizer. (#262)
* [core] Enable to add coupling forces between frames of the same robot.
* [python/viewer] Fix viewer exception when closing backend during replay.
* [python/viewer] Rename 'replay_speed' in 'speed_ratio' for clarity.
* [python/viewer] Add partial support of Panda3d visualizer.
* [misc] Fix compilation issues on Windows, related to 'pinocchio::Symmetric3' implicit constructor.
* [core] Do not enforce position/velocity limits at init if disable.
* [core] Fallback to not loading/using collision model if meshes are missing.
* [core/model] Fix 'getRigidConfigurationFromFlexible' method.
* [python/robot] Enable explicit robot initialization without hardware description file.
* [python/robot] Skip collision bounding box handling if meshes are not available.
* [python/simulator] Fix callback not working since scalar references are now ndarray in Python.
* [python/simulator] Fix 'state' property for 'use_theoretical_state'=True.
* [gym/common/env] Fix 'replay' not working.
* [core] Fix computeSystemDynamics edge case and Python binding signature.
* [core|core/python] Expose 'computeForwardKinematics'. 
* [core|core/python] Rename 'computeSystemDynamics' in 'computeSystemsDynamics' for consistency.
* [core/python] Enable to get system from 'Engine'.
* [gym/common] Add new random sampling utility.
* [gym/common/env] Make sure robot shared memory is reset after calling '_setup'.
* [gym/common/env] Do not log error is observation is out-of-bounds, and only warning in debug.
* [gym/common/env] Make sure the low-level engine memory address does not change.
* [core/python] Avoid memory allocation by 'compute_systems_dynamics' for efficiency.
* [gym/common] Add missing documentation for helper methods.
* [gym/common/env] Fix 'gym.logger.warning' method not available.
* [gym] Use tuple instead of list to store constant sequences to preserve value and type check. (#268)

.github/workflows/manylinux.yml

@@ -10,7 +10,7 @@ on:
 jobs:
   build-test-and-publish-pypi-manylinux:
     name: (manylinux2010) Build and run the unit tests. Then generate and publish the wheels on PyPi.
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     container: quay.io/pypa/manylinux2010_x86_64
 
     defaults:

.github/workflows/win.yml

@@ -134,7 +134,7 @@ jobs:
   publish-pypi-win10:
     name: (Windows) Publish on PyPi the wheel for Windows of Jiminy_py
     needs: build-and-test-win10
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     if: github.repository == 'Wandercraft/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master'
 
     strategy:

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.4.25)
+set(BUILD_VERSION 1.5.0)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

core/CMakeLists.txt

@@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 3.10)
 project(${LIBRARY_NAME}_core VERSION ${BUILD_VERSION})
 
 # Find dependencies
-find_package(Boost REQUIRED COMPONENTS system filesystem date_time thread)
+find_package(Boost REQUIRED COMPONENTS system filesystem serialization date_time thread)
 
 if(NOT LEGACY_MODE)
     find_package(urdfdom REQUIRED NO_MODULE NO_CMAKE_SYSTEM_PATH) # It is impossible to specify the version because it is not exported in cmake config files...

core/include/jiminy/core/Constants.h

@@ -23,12 +23,13 @@ namespace jiminy
     extern std::string const FREE_FLYER_PREFIX_BASE_NAME;
     extern std::string const FLEXIBLE_JOINT_SUFFIX;
 
-    extern std::string const TELEMETRY_DELIMITER;
+    extern std::string const TELEMETRY_FIELDNAME_DELIMITER;
+    extern std::string const TELEMETRY_CONSTANT_DELIMITER;
     extern int64_t const TELEMETRY_MIN_BUFFER_SIZE;
     extern float64_t const TELEMETRY_DEFAULT_TIME_UNIT;
 
-    extern uint8_t const DELAY_MIN_BUFFER_RESERVE; ///< Minimum memory allocation is memory is full and the older data stored is dated less than the desired delay
-    extern uint8_t const DELAY_MAX_BUFFER_EXCEED;  ///< Maximum number of data stored allowed to be dated more than the desired delay
+    extern uint8_t const DELAY_MIN_BUFFER_RESERVE;  ///< Minimum memory allocation is memory is full and the older data stored is dated less than the desired delay
+    extern uint8_t const DELAY_MAX_BUFFER_EXCEED;   ///< Maximum number of data stored allowed to be dated more than the desired delay
 
     extern float64_t const SIMULATION_MIN_TIMESTEP;
     extern float64_t const SIMULATION_MAX_TIMESTEP;

core/include/jiminy/core/Types.h

@@ -97,22 +97,22 @@ namespace jiminy
     // Flexible joints
     struct flexibleJointData_t
     {
-        std::string jointName;
+        std::string frameName;
         vectorN_t stiffness;
         vectorN_t damping;
 
         flexibleJointData_t(void) :
-        jointName(),
+        frameName(),
         stiffness(),
         damping()
         {
             // Empty.
         };
 
-        flexibleJointData_t(std::string const & jointNameIn,
+        flexibleJointData_t(std::string const & frameNameIn,
                             vectorN_t   const & stiffnessIn,
                             vectorN_t   const & dampingIn) :
-        jointName(jointNameIn),
+        frameName(frameNameIn),
         stiffness(stiffnessIn),
         damping(dampingIn)
         {
@@ -121,7 +121,7 @@ namespace jiminy
 
         inline bool_t operator==(flexibleJointData_t const & other) const
         {
-            return (this->jointName == other.jointName
+            return (this->frameName == other.frameName
                  && this->stiffness == other.stiffness
                  && this->damping == other.damping);
         };

core/include/jiminy/core/Utilities.h

@@ -218,9 +218,14 @@ namespace jiminy
                               vectorN_t        const & position,
                               bool_t                 & isValid);
 
-    hresult_t insertFlexibilityInModel(pinocchio::Model       & modelInOut,
-                                       std::string      const & childJointNameIn,
-                                       std::string      const & newJointNameIn);
+    hresult_t insertFlexibilityBeforeJointInModel(pinocchio::Model       & modelInOut,
+                                                  std::string      const & childJointNameIn,
+                                                  std::string      const & newJointNameIn);
+
+    hresult_t insertFlexibilityAtFixedFrameInModel(pinocchio::Model         & modelInOut,
+                                                   std::string        const & frameNameIn,
+                                                   pinocchio::Inertia const & childBodyInertiaIn,
+                                                   std::string        const & newJointNameIn);
 
     /// \brief Convert a force expressed in the global frame of a specific frame to its parent joint frame.
     ///

core/include/jiminy/core/control/AbstractController.h

@@ -8,7 +8,7 @@
 
 namespace jiminy
 {
-    std::string const CONTROLLER_OBJECT_NAME("HighLevelController"); ///< Name of the telemetry object
+    std::string const CONTROLLER_TELEMETRY_NAMESPACE("HighLevelController");  ///< Namespace of the telemetry object
 
     class TelemetryData;
     class Robot;
@@ -90,7 +90,7 @@ namespace jiminy
         ///
         ///////////////////////////////////////////////////////////////////////////////////////////////
         hresult_t registerVariable(std::vector<std::string> const & fieldnames,
-                                   Eigen::Ref<vectorN_t>            values); // Make a "copy" to support both vectorN_t and reference
+                                   Eigen::Ref<vectorN_t>            values);  // Make a "copy" to support both vectorN_t and reference
 
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///

core/include/jiminy/core/control/AbstractController.tpp

@@ -52,17 +52,24 @@ namespace jiminy
 
         // Check in local cache before.
         auto constantIt = std::find_if(registeredConstants_.begin(),
-                                        registeredConstants_.end(),
-                                        [&fieldName](auto const & element)
-                                        {
-                                            return element.first == fieldName;
-                                        });
+                                       registeredConstants_.end(),
+                                       [&fieldName](auto const & element)
+                                       {
+                                           return element.first == fieldName;
+                                       });
         if (constantIt != registeredConstants_.end())
         {
             PRINT_ERROR("Constant already registered.");
             return hresult_t::ERROR_BAD_INPUT;
         }
-        registeredConstants_.emplace_back(fieldName, to_string(value));
+        if constexpr (std::is_same_v<T, std::string>)
+        {
+            registeredConstants_.emplace_back(fieldName, value);
+        }
+        else
+        {
+            registeredConstants_.emplace_back(fieldName, to_string(value));
+        }
 
         return hresult_t::SUCCESS;
     }

core/include/jiminy/core/control/ControllerFunctor.tpp

@@ -54,7 +54,7 @@ namespace jiminy
             return hresult_t::ERROR_INIT_FAILED;
         }
 
-        internalDynamicsFct_(t, q, v, sensorsData_, u); // The sensor data are already up-to-date
+        internalDynamicsFct_(t, q, v, sensorsData_, u);  // The sensor data are already up-to-date
 
         return hresult_t::SUCCESS;
     }

core/include/jiminy/core/engine/Engine.h

@@ -63,8 +63,12 @@ namespace jiminy
                                        pinocchio::Force const & F);
         hresult_t registerForceProfile(std::string           const & frameName,
                                        forceProfileFunctor_t         forceFct);
+        hresult_t addCouplingForce(std::string const & frameName1,
+                                   std::string const & frameName2,
+                                   forceProfileFunctor_t forceFct);
 
         bool_t const & getIsInitialized(void) const;
+        hresult_t getSystem(systemHolder_t * & system);
         hresult_t getRobot(std::shared_ptr<Robot> & robot);
         hresult_t getController(std::shared_ptr<AbstractController> & controller);
         hresult_t getSystemState(systemState_t const * & systemState) const;
@@ -75,7 +79,6 @@ namespace jiminy
         using EngineMultiRobot::removeSystem;
         using EngineMultiRobot::setController;
         using EngineMultiRobot::addCouplingForce;
-        using EngineMultiRobot::removeCouplingForces;
         using EngineMultiRobot::start;
         using EngineMultiRobot::simulate;
         using EngineMultiRobot::registerForceImpulse;

core/include/jiminy/core/engine/EngineMultiRobot.h

@@ -13,16 +13,15 @@
 
 namespace jiminy
 {
-    std::string const ENGINE_OBJECT_NAME("HighLevelController");
+    std::string const ENGINE_TELEMETRY_NAMESPACE("HighLevelController");
 
     std::set<std::string> const STEPPERS {
         "runge_kutta_4",
         "runge_kutta_dopri5",
         "explicit_euler"
     };
 
-    float64_t const CONSTRAINT_INVERSION_DAMPING = 1.0e-12; ///< Damping factor used to perform matrix pseudo-inverse
-                                                            /// when computing forward dynamics with constraints.
+    float64_t const CONSTRAINT_INVERSION_DAMPING = 1.0e-12;  ///< Damping factor used to perform matrix pseudo-inverse when computing forward dynamics with constraints.
 
     class Robot;
     class AbstractController;
@@ -75,7 +74,7 @@ namespace jiminy
         uint32_t iterFailed;
         float64_t t;
         float64_t tPrev;
-        float64_t tError; ///< Internal buffer used for Kahan algorithm storing the residual sum of errors
+        float64_t tError;  ///< Internal buffer used for Kahan algorithm storing the residual sum of errors
         float64_t dt;
         float64_t dtLargest;
         float64_t dtLargestPrev;
@@ -96,7 +95,7 @@ namespace jiminy
             config["frictionStictionRatio"] = 0.5;
             config["stiffness"] = 1.0e6;
             config["damping"] = 2.0e3;
-            config["transitionEps"] = 1.0e-3; // [m]
+            config["transitionEps"] = 1.0e-3;  // [m]
 
             return config;
         };
@@ -128,14 +127,14 @@ namespace jiminy
             configHolder_t config;
             config["verbose"] = false;
             config["randomSeed"] = 0U;
-            config["odeSolver"] = std::string("runge_kutta_dopri5"); // ["runge_kutta_dopri5", "runge_kutta_4", "explicit_euler"]
+            config["odeSolver"] = std::string("runge_kutta_dopri5");  // ["runge_kutta_dopri5", "runge_kutta_4", "explicit_euler"]
             config["tolAbs"] = 1.0e-5;
             config["tolRel"] = 1.0e-4;
             config["dtMax"] = SIMULATION_MAX_TIMESTEP;
             config["dtRestoreThresholdRel"] = 0.2;
             config["successiveIterFailedMax"] = 1000U;
-            config["iterMax"] = -1; // <= 0: disable
-            config["timeout"] = 0.0; // <= 0.0: disable
+            config["iterMax"] = -1;  // <= 0: disable
+            config["timeout"] = 0.0;  // <= 0.0: disable
             config["sensorsUpdatePeriod"] = 0.0;
             config["controllerUpdatePeriod"] = 0.0;
             config["logInternalStepperSteps"] = false;
@@ -423,10 +422,14 @@ namespace jiminy
         bool_t const & getIsSimulationRunning(void) const;
         float64_t getMaxSimulationDuration(void) const;
 
-        hresult_t computeSystemDynamics(float64_t              const & t,
-                                        std::vector<vectorN_t> const & qSplit,
-                                        std::vector<vectorN_t> const & vSplit,
-                                        std::vector<vectorN_t>       & aSplit);
+        static void computeForwardKinematics(systemHolder_t  & system,
+                                             vectorN_t const & q,
+                                             vectorN_t const & v,
+                                             vectorN_t const & a);
+        hresult_t computeSystemsDynamics(float64_t              const & t,
+                                         std::vector<vectorN_t> const & qSplit,
+                                         std::vector<vectorN_t> const & vSplit,
+                                         std::vector<vectorN_t>       & aSplit);
 
     protected:
         hresult_t configureTelemetry(void);
@@ -438,11 +441,6 @@ namespace jiminy
         void reset(bool_t const & resetRandomNumbers,
                    bool_t const & resetDynamicForceRegister);
 
-        static void computeForwardKinematics(systemHolder_t  & system,
-                                             vectorN_t const & q,
-                                             vectorN_t const & v,
-                                             vectorN_t const & a);
-
         /// \brief Compute the force resulting from ground contact on a given body.
         ///
         /// \param[in] system              System for which to perform computation.

core/include/jiminy/core/engine/PinocchioOverloadAlgorithms.h

@@ -2,6 +2,8 @@
 /// adding support for rotor inertia for 1DoF (prismatic, revolute) joints.
 ///
 /// Based on https://github.com/stack-of-tasks/pinocchio/blob/820d0f85fbabddce20924a6e0f781fb2be5029e9/src/algorithm/aba.hxx
+///          https://github.com/stack-of-tasks/pinocchio/blob/820d0f85fbabddce20924a6e0f781fb2be5029e9/src/algorithm/rnea.hxx
+///          https://github.com/stack-of-tasks/pinocchio/blob/820d0f85fbabddce20924a6e0f781fb2be5029e9/src/algorithm/crba.hxx
 /// Copyright (c) 2014-2020, CNRS
 /// Copyright (c) 2018-2020, INRIA
 
@@ -18,6 +20,7 @@
 #include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/aba.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/energy.hpp"
 
 #include "jiminy/core/Macros.h"
@@ -30,8 +33,8 @@ namespace pinocchio_overload
     template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl,
              typename ConfigVectorType, typename TangentVectorType>
     inline Scalar
-    kineticEnergy(pinocchio::ModelTpl<Scalar,Options,JointCollectionTpl> const & model,
-                  pinocchio::DataTpl<Scalar,Options,JointCollectionTpl>        & data,
+    kineticEnergy(pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl> const & model,
+                  pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>        & data,
                   Eigen::MatrixBase<ConfigVectorType>                    const & q,
                   Eigen::MatrixBase<TangentVectorType>                   const & v,
                   bool_t                                                 const & update_kinematics)
@@ -57,6 +60,32 @@ namespace pinocchio_overload
         return data.tau;
     }
 
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl,
+             typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
+    inline const typename pinocchio::DataTpl<Scalar,Options,JointCollectionTpl>::TangentVectorType &
+    rnea(pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl> const & model,
+         pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>        & data,
+         Eigen::MatrixBase<ConfigVectorType>                    const & q,
+         Eigen::MatrixBase<TangentVectorType1>                  const & v,
+         Eigen::MatrixBase<TangentVectorType2>                  const & a)
+    {
+        pinocchio::rnea(model, data, q, v, a);
+        data.tau += model.rotorInertia.asDiagonal() * a;
+        return data.tau;
+    }
+
+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl,
+             typename ConfigVectorType>
+    inline const typename pinocchio::DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs &
+    crba(pinocchio::ModelTpl<Scalar, Options, JointCollectionTpl> const & model,
+         pinocchio::DataTpl<Scalar, Options, JointCollectionTpl> & data,
+         Eigen::MatrixBase<ConfigVectorType> const & q)
+    {
+        pinocchio::crba(model, data, q);
+        data.M += model.rotorInertia.asDiagonal();
+        return data.M;
+    }
+
     template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
     struct AbaBackwardStep
     : public pinocchio::fusion::JointUnaryVisitorBase<AbaBackwardStep<Scalar,Options,JointCollectionTpl> >

core/include/jiminy/core/robot/AbstractMotor.h

@@ -288,7 +288,7 @@ namespace jiminy
         hresult_t detach(void);
 
     public:
-        std::unique_ptr<abstractMotorOptions_t const> baseMotorOptions_; ///< Structure with the parameters of the motor
+        std::unique_ptr<abstractMotorOptions_t const> baseMotorOptions_;  ///< Structure with the parameters of the motor
 
     protected:
         configHolder_t motorOptionsHolder_;         ///< Dictionary with the parameters of the motor

core/include/jiminy/core/robot/AbstractSensor.h

@@ -96,7 +96,7 @@ namespace jiminy
             vectorN_t const noiseStd;   ///< Standard deviation of the noise of the sensor
             vectorN_t const bias;       ///< Bias of the sensor
             float64_t const delay;      ///< Delay of the sensor
-            uint32_t  const delayInterpolationOrder; ///< Order of the interpolation used to compute delayed sensor data. [0: Zero-order holder, 1: Linear interpolation]
+            uint32_t  const delayInterpolationOrder;  ///< Order of the interpolation used to compute delayed sensor data. [0: Zero-order holder, 1: Linear interpolation]
 
             abstractSensorOptions_t(configHolder_t const & options) :
             noiseStd(boost::get<vectorN_t>(options.at("noiseStd"))),

core/include/jiminy/core/robot/AbstractSensor.tpp

@@ -133,7 +133,7 @@ namespace jiminy
         sharedHolder_->data_.resize(2);
         for (matrixN_t & data : sharedHolder_->data_)
         {
-            data = matrixN_t::Zero(getSize(), sharedHolder_->num_); // Do NOT use setZero since the size may be ill-defined
+            data = matrixN_t::Zero(getSize(), sharedHolder_->num_);  // Do NOT use setZero since the size may be ill-defined
         }
         sharedHolder_->dataMeasured_.setZero();
 
@@ -207,7 +207,7 @@ namespace jiminy
     {
         if (areFieldnamesGrouped_)
         {
-            return getType() + TELEMETRY_DELIMITER + name_;
+            return getType() + TELEMETRY_FIELDNAME_DELIMITER + name_;
         }
         else
         {

core/include/jiminy/core/robot/BasicSensors.h

@@ -37,7 +37,7 @@ namespace jiminy
     private:
         std::string frameName_;
         int32_t frameIdx_;
-        quaternion_t sensorRotationBias_; ///< Sensor rotation bias.
+        quaternion_t sensorRotationBias_;  ///< Sensor rotation bias.
     };
 
     class ContactSensor : public AbstractSensorTpl<ContactSensor>

core/include/jiminy/core/robot/Model.h

@@ -158,7 +158,7 @@ namespace jiminy
         hresult_t addContactPoints(std::vector<std::string> const & frameNames);
         hresult_t removeContactPoints(std::vector<std::string> const & frameNames = {});
 
-        hresult_t setOptions(configHolder_t modelOptions); // Make a copy
+        hresult_t setOptions(configHolder_t modelOptions);  // Make a copy
         configHolder_t getOptions(void) const;
 
         /// This method are not intended to be called manually. The Engine is taking care of it.