[python/viewer] Add options to display Capture Point and external forces.

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.6.15)
+set(BUILD_VERSION 1.6.16)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

core/include/jiminy/core/control/ControllerFunctor.h

@@ -73,10 +73,10 @@ namespace jiminy
         /// \return     Return code to determine whether the execution of the method was successful.
         ///
         ///////////////////////////////////////////////////////////////////////////////////////////////
-        hresult_t computeCommand(float64_t const & t,
-                                 vectorN_t const & q,
-                                 vectorN_t const & v,
-                                 vectorN_t       & command) override;
+        virtual hresult_t computeCommand(float64_t const & t,
+                                         vectorN_t const & q,
+                                         vectorN_t const & v,
+                                         vectorN_t       & command) override;
 
         ///////////////////////////////////////////////////////////////////////////////////////////////
         ///
@@ -91,10 +91,10 @@ namespace jiminy
         /// \return     Return code to determine whether the execution of the method was successful.
         ///
         ///////////////////////////////////////////////////////////////////////////////////////////////
-        hresult_t internalDynamics(float64_t const & t,
-                                   vectorN_t const & q,
-                                   vectorN_t const & v,
-                                   vectorN_t       & uCustom) override;
+        virtual hresult_t internalDynamics(float64_t const & t,
+                                           vectorN_t const & q,
+                                           vectorN_t const & v,
+                                           vectorN_t       & uCustom) override;
 
     private:
         // std::conditional_t enables to use both functors and lambdas

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

@@ -172,6 +172,7 @@ namespace jiminy
             config["enableConfiguration"] = true;
             config["enableVelocity"] = true;
             config["enableAcceleration"] = true;
+            config["enableForceExternal"] = false;
             config["enableCommand"] = true;
             config["enableMotorEffort"] = true;
             config["enableEnergy"] = true;
@@ -284,6 +285,7 @@ namespace jiminy
             bool_t const enableConfiguration;
             bool_t const enableVelocity;
             bool_t const enableAcceleration;
+            bool_t const enableForceExternal;
             bool_t const enableCommand;
             bool_t const enableMotorEffort;
             bool_t const enableEnergy;
@@ -293,6 +295,7 @@ namespace jiminy
             enableConfiguration(boost::get<bool_t>(options.at("enableConfiguration"))),
             enableVelocity(boost::get<bool_t>(options.at("enableVelocity"))),
             enableAcceleration(boost::get<bool_t>(options.at("enableAcceleration"))),
+            enableForceExternal(boost::get<bool_t>(options.at("enableForceExternal"))),
             enableCommand(boost::get<bool_t>(options.at("enableCommand"))),
             enableMotorEffort(boost::get<bool_t>(options.at("enableMotorEffort"))),
             enableEnergy(boost::get<bool_t>(options.at("enableEnergy"))),

core/include/jiminy/core/engine/System.h

@@ -161,6 +161,7 @@ namespace jiminy
         std::vector<std::string> positionFieldnames;
         std::vector<std::string> velocityFieldnames;
         std::vector<std::string> accelerationFieldnames;
+        std::vector<std::string> forceExternalFieldnames;
         std::vector<std::string> commandFieldnames;
         std::vector<std::string> motorEffortFieldnames;
         std::string energyFieldname;

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

@@ -353,6 +353,7 @@ namespace jiminy
         std::vector<std::string> const & getPositionFieldnames(void) const;
         std::vector<std::string> const & getVelocityFieldnames(void) const;
         std::vector<std::string> const & getAccelerationFieldnames(void) const;
+        std::vector<std::string> const & getForceExternalFieldnames(void) const;
 
         hresult_t getFlexibleConfigurationFromRigid(vectorN_t const & qRigid,
                                                     vectorN_t       & qFlex) const;
@@ -427,16 +428,17 @@ namespace jiminy
         vectorN_t constraintsDrift_;                            ///< Vector holding the drift of the constraints
 
         vectorN_t positionLimitMin_;                            ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
-        vectorN_t positionLimitMax_;                            ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
-        vectorN_t velocityLimit_;                               ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any)
+        vectorN_t positionLimitMax_;                            ///< Lower position limit of the whole configuration vector
+        vectorN_t velocityLimit_;                               ///< Maximum absolute velocity of the whole velocity vector
 
-        std::vector<std::string> positionFieldnames_;           ///< Fieldnames of the elements in the configuration vector of the rigid robot
-        std::vector<std::string> velocityFieldnames_;           ///< Fieldnames of the elements in the velocity vector of the rigid robot
-        std::vector<std::string> accelerationFieldnames_;       ///< Fieldnames of the elements in the acceleration vector of the rigid robot
+        std::vector<std::string> positionFieldnames_;       ///< Fieldnames of the elements in the configuration vector of the model
+        std::vector<std::string> velocityFieldnames_;       ///< Fieldnames of the elements in the velocity vector of the model
+        std::vector<std::string> accelerationFieldnames_;   ///< Fieldnames of the elements in the acceleration vector of the model
+        std::vector<std::string> forceExternalFieldnames_;  ///< Concatenated fieldnames of the external force applied at each joint of the model, 'universe' excluded
 
     private:
         pinocchio::Model pncModelFlexibleOrig_;
-        motionVector_t jointsAcceleration_;                     ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints.
+        motionVector_t jointsAcceleration_;      ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints.
 
         int32_t nq_;
         int32_t nv_;

core/src/engine/EngineMultiRobot.cc

@@ -665,6 +665,9 @@ namespace jiminy
                 systemDataIt->accelerationFieldnames =
                     addCircumfix(systemIt->robot->getAccelerationFieldnames(),
                                  systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
+                systemDataIt->forceExternalFieldnames =
+                    addCircumfix(systemIt->robot->getForceExternalFieldnames(),
+                                 systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
                 systemDataIt->commandFieldnames =
                     addCircumfix(systemIt->robot->getCommandFieldnames(),
                                  systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER);
@@ -703,6 +706,19 @@ namespace jiminy
                             systemDataIt->state.a);
                     }
                 }
+                if (engineOptions_->telemetry.enableForceExternal)
+                {
+                    for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i)
+                    {
+                        auto const & fext = systemDataIt->state.fExternal[i].toVector();
+                        for (uint8_t j = 0; j < 6U; ++j)
+                        {
+                            returnCode = telemetrySender_.registerVariable(
+                                systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j],
+                                fext[j]);
+                        }
+                    }
+                }
                 if (returnCode == hresult_t::SUCCESS)
                 {
                     if (engineOptions_->telemetry.enableCommand)
@@ -767,7 +783,7 @@ namespace jiminy
                 systemIt->robot->pncModel_,
                 systemIt->robot->pncData_);
 
-            // Update the telemetry internal state
+            // Update telemetry values
             if (engineOptions_->telemetry.enableConfiguration)
             {
                 telemetrySender_.updateValue(systemDataIt->positionFieldnames,
@@ -783,6 +799,19 @@ namespace jiminy
                 telemetrySender_.updateValue(systemDataIt->accelerationFieldnames,
                                              systemDataIt->state.a);
             }
+            if (engineOptions_->telemetry.enableForceExternal)
+            {
+                for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i)
+                {
+                    auto const & fext = systemDataIt->state.fExternal[i].toVector();
+                    for (uint8_t j = 0; j < 6U; ++j)
+                    {
+                        telemetrySender_.updateValue(
+                            systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j],
+                            fext[j]);
+                    }
+                }
+            }
             if (engineOptions_->telemetry.enableCommand)
             {
                 telemetrySender_.updateValue(systemDataIt->commandFieldnames,
@@ -857,6 +886,14 @@ namespace jiminy
     void computeExtraTerms(systemHolder_t           & system,
                            systemDataHolder_t const & systemData)
     {
+        /// This method is optimized to avoid redundant computations.
+        /// See `pinocchio::computeAllTerms` for reference:
+        ///
+        /// Based on 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
+
         pinocchio::Model const & model = system.robot->pncModel_;
         pinocchio::Data & data = system.robot->pncData_;
 
@@ -887,16 +924,6 @@ namespace jiminy
             }
         }
 
-        // Now that Ycrb is available, it is possible to infer directly the subtree center of masses
-        pinocchio::getComFromCrba(model, data);
-        data.Ig.mass() = data.oYcrb[0].mass();
-        data.Ig.lever().setZero();
-        data.Ig.inertia() = data.oYcrb[0].inertia();
-        for (int32_t i = 1; i < model.njoints; ++i)
-        {
-            data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
-        }
-
         /* Neither 'aba' nor 'forwardDynamics' are computed the actual joints
            acceleration and forces, so it must be done separately:
            - 1st step: computing the forces based on rnea algorithm
@@ -928,7 +955,19 @@ namespace jiminy
             }
         }
 
-         pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);
+        /* Now that `data.oYcrb` and `data.h` are available, one can get directly
+           the position and velocity of the center of mass of each subtrees. */
+        data.Ig.mass() = data.oYcrb[0].mass();
+        data.Ig.lever().setZero();
+        data.Ig.inertia() = data.oYcrb[0].inertia();
+        data.com[0] = data.oYcrb[0].lever();
+        for (int32_t i = 1; i < model.njoints; ++i)
+        {
+            data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever());
+            data.vcom[i] = data.h[i].linear() / data.mass[i];
+        }
+
+        pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq);
     }
 
     void computeAllExtraTerms(std::vector<systemHolder_t>           & systems,
@@ -2216,6 +2255,11 @@ namespace jiminy
     template<typename ...Args>
     std::tuple<bool_t, float64_t> isGcdIncluded(std::vector<systemDataHolder_t> const & systemsDataHolder, Args... values)
     {
+        if (systemsDataHolder.empty())
+        {
+            return isGcdIncluded(std::forward<Args>(values)...);
+        }
+
         float64_t minValue = INF;
         auto lambda = [&minValue, &values...](systemDataHolder_t const & systemData)
         {

core/src/engine/System.cc

@@ -182,6 +182,7 @@ namespace jiminy
     positionFieldnames(),
     velocityFieldnames(),
     accelerationFieldnames(),
+    forceExternalFieldnames(),
     commandFieldnames(),
     motorEffortFieldnames(),
     energyFieldname(),