[core] Add relaxation to PGS solver to mitigate convergence instabilities.

core/include/jiminy/core/engine/engine_multi_robot.h

@@ -149,7 +149,7 @@ namespace jiminy
             config["tolRel"] = 1.0e-4;
             config["dtMax"] = SIMULATION_MAX_TIMESTEP;
             config["dtRestoreThresholdRel"] = 0.2;
-            config["successiveIterFailedMax"] = 5000U;
+            config["successiveIterFailedMax"] = 1000U;
             config["iterMax"] = 0U;   // <= 0: disable
             config["timeout"] = 0.0;  // <= 0.0: disable
             config["sensorsUpdatePeriod"] = 0.0;

core/include/jiminy/core/hardware/abstract_sensor.hxx

@@ -289,7 +289,11 @@ namespace jiminy
     template<typename T>
     void AbstractSensorTpl<T>::interpolateData()
     {
-        assert(sharedStorage_->times_.size() > 0 && "No data to interpolatePositions.");
+        // Make sure that data is available
+        if (sharedStorage_->times_.empty())
+        {
+            throw std::logic_error("No data to interpolate.");
+        }
 
         // Sample the delay uniformly
         const double delay =

core/include/jiminy/core/hardware/fwd.h

@@ -67,8 +67,11 @@ namespace jiminy
         {
             if (sharedMeasurementsPtr_)
             {
-                assert((size() == static_cast<std::size_t>(sharedMeasurementsPtr_->cols())) &&
-                       "Number of sensors inconsistent with shared measurements.");
+                if (size() != static_cast<std::size_t>(sharedMeasurementsPtr_->cols()))
+                {
+                    throw std::logic_error(
+                        "Number of sensors inconsistent with shared measurements.");
+                }
                 return *sharedMeasurementsPtr_;
             }
             else
@@ -86,8 +89,11 @@ namespace jiminy
                 // Set internal buffer by copying sensor data sequentially
                 for (const auto & sensor : *this)
                 {
-                    assert(sensor.value.size() == dataSize &&
-                           "Cannot get all data at once for heterogeneous sensors.");
+                    if (sensor.value.size() != dataSize)
+                    {
+                        throw std::logic_error(
+                            "Cannot get all data at once for heterogeneous sensors.");
+                    }
                     data_.row(sensor.index) = sensor.value;
                 }
 

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

@@ -494,10 +494,10 @@ namespace jiminy::pinocchio_overload
     }
 
     template<typename JacobianType>
-    void computeJMinvJt(const pinocchio::Model & model,
-                        pinocchio::Data & data,
-                        const Eigen::MatrixBase<JacobianType> & J,
-                        bool updateDecomposition = true)
+    const Eigen::MatrixXd & computeJMinvJt(const pinocchio::Model & model,
+                                           pinocchio::Data & data,
+                                           const Eigen::MatrixBase<JacobianType> & J,
+                                           bool updateDecomposition = true)
     {
         // Compute the Cholesky decomposition of mass matrix M if requested
         if (updateDecomposition)
@@ -536,6 +536,8 @@ namespace jiminy::pinocchio_overload
         data.JMinvJt.resize(J.rows(), J.rows());
         data.JMinvJt.triangularView<Eigen::Lower>().setZero();
         data.JMinvJt.selfadjointView<Eigen::Lower>().rankUpdate(sDUiJt.transpose());
+
+        return data.JMinvJt;
     }
 
     template<typename RhsType>

core/include/jiminy/core/solver/constraint_solvers.h

@@ -34,7 +34,7 @@ namespace jiminy
         virtual ~AbstractConstraintSolver() = default;
 
         /// \brief Compute the solution of the Nonlinear Complementary Problem:
-        ///        A x + b = w,
+        ///        A x - b = w,
         ///        s.t. (w[i] > 0 and x[i] = 0) or (w[i] = 0 and x[i] > 0
         ///
         ///        for non-linear boxed bounds lo(x) < x < hi(x):
@@ -68,6 +68,7 @@ namespace jiminy
     private:
         void ProjectedGaussSeidelIter(const Eigen::MatrixXd & A,
                                       const Eigen::VectorXd::SegmentReturnType & b,
+                                      const double w,
                                       Eigen::VectorXd::SegmentReturnType & x);
         bool ProjectedGaussSeidelSolver(const Eigen::MatrixXd & A,
                                         const Eigen::VectorXd::SegmentReturnType & b,

core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h

@@ -38,7 +38,7 @@ namespace jiminy
         /// \details The larger the more conservative. More precisely, a small safety factor means
         ///          that the step size will be increased less aggressively when the error is small
         ///          and decreased more aggressively when the error is large.
-        inline constexpr double SAFETY = 0.7;
+        inline constexpr double SAFETY = 0.8;
         /// \brief Maximum acceptable error threshold below which step size is increased.
         inline constexpr double ERROR_THRESHOLD = 0.5;
         /// \brief Mininum allowed relative step decrease.

core/src/solver/constraint_solvers.cc

@@ -12,7 +12,13 @@
 
 namespace jiminy
 {
-    inline constexpr double PGS_MIN_REGULARIZER{1.0e-11};
+    inline constexpr double MIN_REGULARIZER{1.0e-11};
+
+    inline constexpr double RELAX_MIN{0.01};
+    inline constexpr double RELAX_MAX{1.0};
+    inline constexpr uint32_t RELAX_MIN_ITER_NUM{20};
+    inline constexpr uint32_t RELAX_MAX_ITER_NUM{30};
+    inline constexpr double RELAX_SLOPE_ORDER{2.0};
 
     PGSSolver::PGSSolver(const pinocchio::Model * model,
                          pinocchio::Data * data,
@@ -99,6 +105,7 @@ namespace jiminy
 
     void PGSSolver::ProjectedGaussSeidelIter(const Eigen::MatrixXd & A,
                                              const Eigen::VectorXd::SegmentReturnType & b,
+                                             const double w,
                                              Eigen::VectorXd::SegmentReturnType & x)
     {
         // First, loop over all unbounded constraints
@@ -168,11 +175,11 @@ namespace jiminy
                         A_max = A_kk;
                     }
                 }
-                e += y_[i0] / A_max;
+                e += w * y_[i0] / A_max;
                 for (std::uint_fast8_t j = 1; j < fSize - 1; ++j)
                 {
                     const Eigen::Index k = o + fIndex[j];
-                    x[k] += y_[k] / A_max;
+                    x[k] += w * y_[k] / A_max;
                 }
 
                 // Project the coefficient between lower and upper bounds
@@ -221,7 +228,10 @@ namespace jiminy
            tolerance, even if unconstrained. It seems to be related to compounding of errors, maybe
            due to the recursive computations. */
 
-        assert(b.size() > 0 && "The number of inequality constraints must be larger than 0.");
+        if (b.size() == 0)
+        {
+            throw std::logic_error("The number of inequality constraints must be larger than 0.");
+        }
 
         // Reset the residuals
         y_.setZero();
@@ -232,14 +242,35 @@ namespace jiminy
             // Backup previous residuals
             yPrev_ = y_;
 
-            // Do a single iteration
-            ProjectedGaussSeidelIter(A, b, x);
+            // Update the under-relaxation factor
+            const double ratio = (static_cast<double>(iterMax_ - RELAX_MIN_ITER_NUM) - iter) /
+                                 (iterMax_ - RELAX_MIN_ITER_NUM - RELAX_MAX_ITER_NUM);
+            double w = RELAX_MAX;
+            if (ratio < 1.0)
+            {
+                w = RELAX_MIN;
+                if (ratio > 0.0)
+                {
+                    w += (RELAX_MAX - RELAX_MIN) * std::pow(ratio, RELAX_SLOPE_ORDER);
+                }
+            }
 
-            /* Stopping the algorithm has stalled.
-               It is impossible to define a criteria on the residuals directly because they only
-               cancel for constraints whose bounds are inactive. Otherwise they can grow arbitrary
-               large. */
-            auto tol = tolAbs_ + tolRel_ * y_.array().abs();
+            // Do one iteration
+            ProjectedGaussSeidelIter(A, b, w, x);
+
+            /* Abort if stopping criteria is met.
+               It is not possible to define a stopping criteria on the residuals directly because
+               they can grow arbitrary large for constraints whose bounds are active. It follows
+               that stagnation of residuals is the only viable criteria.
+               The PGS algorithm has been modified for solving second-order cone LCP, which means
+               that only the L2-norm of the tangential forces can be expected to converge. Because
+               of this, it is too restrictive to check the element-wise variation of the residuals
+               over iterations. It makes more sense to look at the Linf-norm instead, but this
+               criteria is very lax. A good compromise may be to look at the constraint-block-wise
+               L2-norm, which is similar to what Drake simulator is doing. For reference, see:
+               https://github.com/RobotLocomotion/drake/blob/master/multibody/contact_solvers/pgs_solver.cc
+            */
+            const double tol = tolAbs_ + tolRel_ * y_.lpNorm<Eigen::Infinity>() + EPS;
             if (((y_ - yPrev_).array().abs() < tol).all())
             {
                 return true;
@@ -300,7 +331,7 @@ namespace jiminy
 
                See: - http://mujoco.org/book/modeling.html#CSolver
                     - http://mujoco.org/book/computation.html#soParameters  */
-            A.diagonal() += (A.diagonal() * dampingInv).cwiseMax(PGS_MIN_REGULARIZER);
+            A.diagonal() += (A.diagonal() * dampingInv).cwiseMax(MIN_REGULARIZER);
 
             /* The LCP is not fully up-to-date since the upper triangular part is still missing.
                This will only be done if necessary. */

core/src/stepper/runge_kutta_dopri_stepper.cc

@@ -31,7 +31,7 @@ namespace jiminy
         scale_.absInPlace();
         scale_ *= tolRel_;
         scale_ += tolAbs_;
-        
+
         // Compute alternative solution
         stateIncrement_.setZero();
         for (std::size_t i = 0; i < ki_.size(); ++i)
@@ -70,8 +70,7 @@ namespace jiminy
             {
                 /* Prevent numeric rounding error when close to zero.
                    Multiply step size by 'DOPRI::SAFETY / (error ** (1 / DOPRI::STEPPER_ORDER))',
-                   up to 'DOPRI::MAX_FACTOR'.
-                */
+                   up to 'DOPRI::MAX_FACTOR'. */
                 const double clippedError = std::max(
                     error, std::pow(DOPRI::MAX_FACTOR / DOPRI::SAFETY, -DOPRI::STEPPER_ORDER));
                 dt *= DOPRI::SAFETY * std::pow(clippedError, -1.0 / DOPRI::STEPPER_ORDER);

data/bipedal_robots/atlas/atlas_v4_options.toml

@@ -8,7 +8,7 @@ tolRel = 1e-4
 sensorsUpdatePeriod = 0.005
 controllerUpdatePeriod = 0.005
 logInternalStepperSteps = false
-successiveIterFailedMax = 5000
+successiveIterFailedMax = 1000
 randomSeedSeq = [0,]
 
 # ============== Contact dynamics ===============
@@ -22,6 +22,7 @@ model = "constraint"
 stabilizationFreq = 25.0
 transitionEps = 5.0e-3
 friction = 0.8
+torsion = 0.0
 
 # ======== Joints bounds configuration ========
 

python/jiminy_pywrap/src/helpers.cc

@@ -306,33 +306,33 @@ namespace jiminy::python
 
         // Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors
         bp::def("interpolate_positions", &interpolatePositions,
-                                         (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out"),
-                                         bp::return_value_policy<result_converter<true>>());
+                                         bp::return_value_policy<result_converter<true>>(),
+                                         (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out"));
 
         bp::def("aba",
                 &pinocchio_overload::aba<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>,
+                bp::return_value_policy<result_converter<false>>(),
                 (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "u", "fext"),
-                "Compute ABA with external forces, store the result in Data::ddq and return it.",
-                bp::return_value_policy<result_converter<false>>());
+                "Compute ABA with external forces, store the result in Data::ddq and return it.");
         bp::def("rnea",
                 &pinocchio_overload::rnea<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd>,
+                bp::return_value_policy<result_converter<false>>(),
                 (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"),
-                "Compute the RNEA without external forces, store the result in Data and return it.",
-                bp::return_value_policy<result_converter<false>>());
+                "Compute the RNEA without external forces, store the result in Data and return it.");
         bp::def("rnea",
                 &pinocchio_overload::rnea<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>,
+                bp::return_value_policy<result_converter<false>>(),
                 (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a", "fext"),
-                "Compute the RNEA with external forces, store the result in Data and return it.",
-                bp::return_value_policy<result_converter<false>>());
+                "Compute the RNEA with external forces, store the result in Data and return it.");
         bp::def("crba",
                 &pinocchio_overload::crba<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>,
+                bp::return_value_policy<result_converter<false>>(),
                 (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false),
-                "Computes CRBA, store the result in Data and return it.",
-                bp::return_value_policy<result_converter<false>>());
+                "Computes CRBA, store the result in Data and return it.");
         bp::def("computeKineticEnergy",
                 &pinocchio_overload::computeKineticEnergy<
                     double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd>,
@@ -343,6 +343,7 @@ namespace jiminy::python
 
         bp::def("computeJMinvJt",
                 &pinocchio_overload::computeJMinvJt<Eigen::MatrixXd>,
+                bp::return_value_policy<result_converter<false>>(),
                 (bp::arg("pinocchio_model"), "pinocchio_data", "J", bp::arg("update_decomposition") = true));
         bp::def("solveJMinvJtv", &solveJMinvJtv,
                 (bp::arg("pinocchio_data"), "v", bp::arg("update_decomposition") = true));