[misc] Jiminy release 1.6.18 (#345)

* [core] Provide robust initial guess to 'impulse' contact solver.
* [python] Various minor improvements.
* [python/viewer] Fix clicking on node for panda3d Qt widget.
* [python/viewer] Fix seldom lock issues when displaying many robots simultaneously.
* [gym/common] 'sample' now supports providing both 'shape' and vector 'low'/'high'/'mean'/'scale' if broadcastable.
* [gym/examples/rllib] More robust obs flattening.
* [gym/examples/rllib] Do not replay if recording is disable during training.
* [gym/examples/rllib] Add option to use custom logger. Interrupt learning gracefully in case of ray task exception.
* [gym/examples/rllib] Add option to save checkpoint every given number of training iterations.
* [gym/examples/rllib] Start ray backend in local mode in debug.
* [misc] Fix easy-install script.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>

CMakeLists.txt

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

build_tools/easy_install_deps_ubuntu.sh

@@ -57,9 +57,9 @@ if ! [ -d "/opt/openrobots/lib/${PYTHON_BIN}/site-packages/" ] ; then
 
     # apt-get must be used instead of apt to support wildcard in package name on Ubuntu 20
     apt-get install -y --allow-downgrades --allow-unauthenticated \
-        robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.7.1 robotpkg-pinocchio=2.5.6 \
-        robotpkg-py3*-qt5-gepetto-viewer=4.10.1r2 robotpkg-py3*-qt5-gepetto-viewer-corba=5.5.1 robotpkg-py3*-omniorbpy=4.2.4 \
-        robotpkg-py3*-eigenpy=2.6.2 robotpkg-py3*-hpp-fcl=1.7.1 robotpkg-py3*-pinocchio=2.5.6
+        robotpkg-octomap=1.9.0 robotpkg-urdfdom-headers=1.0.4 robotpkg-hpp-fcl=1.7.1 robotpkg-pinocchio=2.5.6 \
+        robotpkg-qt5-osgqt=3.5.7r2 robotpkg-py3*-qt5-gepetto-viewer=4.12.0r2 robotpkg-py3*-qt5-gepetto-viewer-corba=5.6.0 \
+        robotpkg-py3*-omniorbpy=4.2.4 robotpkg-py3*-eigenpy=2.6.2 robotpkg-py3*-hpp-fcl=1.7.1 robotpkg-py3*-pinocchio=2.5.6
 
     sudo -H -u $(id -nu "$SUDO_UID") bash -c " \
     echo 'export LD_LIBRARY_PATH=\"/opt/openrobots/lib:\${LD_LIBRARY_PATH}\"' >> \$HOME/.bashrc && \

core/CMakeLists.txt

@@ -50,17 +50,17 @@ set(SRC
     "${CMAKE_CURRENT_SOURCE_DIR}/src/telemetry/TelemetryData.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/telemetry/TelemetrySender.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/telemetry/TelemetryRecorder.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/AbstractConstraint.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/JointConstraint.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/FixedFrameConstraint.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/DistanceConstraint.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/SphereConstraint.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/constraints/WheelConstraint.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/Model.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/AbstractConstraint.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/AbstractMotor.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/BasicMotors.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/AbstractSensor.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/BasicSensors.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/JointConstraint.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/FixedFrameConstraint.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/DistanceConstraint.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/SphereConstraint.cc"
-    "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/WheelConstraint.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/robot/Robot.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/control/AbstractController.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/solver/LCPSolvers.cc"

core/include/jiminy/core/robot/AbstractConstraint.h → core/include/jiminy/core/constraints/AbstractConstraint.h

@@ -103,6 +103,9 @@ namespace jiminy
         ///////////////////////////////////////////////////////////////////////////////////////////////
         void detach(void);
 
+    public:
+        vectorN_t lambda_;                  ///< Lambda multipliers.
+
     protected:
         std::weak_ptr<Model const> model_;  ///< Model on which the constraint operates.
         bool_t isAttached_;                 ///< Flag to indicate if the constraint has been attached to a model.

core/include/jiminy/core/robot/DistanceConstraint.h → core/include/jiminy/core/constraints/DistanceConstraint.h

@@ -4,7 +4,7 @@
 #include <memory>
 
 #include "jiminy/core/Types.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy

core/include/jiminy/core/robot/FixedFrameConstraint.h → core/include/jiminy/core/constraints/FixedFrameConstraint.h

@@ -12,7 +12,7 @@
 #include <memory>
 
 #include "jiminy/core/Types.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy

core/include/jiminy/core/robot/JointConstraint.h → core/include/jiminy/core/constraints/JointConstraint.h

@@ -4,7 +4,7 @@
 #include <memory>
 
 #include "jiminy/core/Types.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy

core/include/jiminy/core/robot/SphereConstraint.h → core/include/jiminy/core/constraints/SphereConstraint.h

@@ -13,7 +13,7 @@
 #include <memory>
 
 #include "jiminy/core/Types.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy

core/include/jiminy/core/robot/WheelConstraint.h → core/include/jiminy/core/constraints/WheelConstraint.h

@@ -13,7 +13,7 @@
 #include <memory>
 
 #include "jiminy/core/Types.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy

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

@@ -316,6 +316,9 @@ namespace jiminy
         /// \brief Get drift of the constraints.
         constVectorBlock_t getConstraintsDrift(void) const;
 
+        /// \brief Get lambda multipliers of the constraints.
+        constVectorBlock_t getConstraintsLambda(void) const;
+
         /// \brief Returns true if at least one constraint is active on the robot.
         bool_t hasConstraint(void) const;
 
@@ -426,6 +429,7 @@ namespace jiminy
         uint64_t constraintsMask_;                              ///< Mask used to filter out disable constraints from full jacobian and drift
         matrixN_t constraintsJacobian_;                         ///< Matrix holding the jacobian of the constraints
         vectorN_t constraintsDrift_;                            ///< Vector holding the drift of the constraints
+        vectorN_t constraintsLambda_;                           ///< Vector holding the lambda multipliers 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

core/src/Constants.cc

@@ -20,6 +20,6 @@ namespace jiminy
     float64_t const SIMULATION_MAX_TIMESTEP = 0.02;
 
     uint32_t const PGS_MAX_ITERATIONS = 100U;
-    uint32_t const PGS_RANDOM_PERMUTATION_PERIOD = 7U;  // 0 to disable
+    uint32_t const PGS_RANDOM_PERMUTATION_PERIOD = 0U;  // 0 to disable
     float64_t const PGS_MIN_REGULARIZER = 1.0e-12;
 }

core/src/robot/AbstractConstraint.cc → core/src/constraints/AbstractConstraint.cc

@@ -1,12 +1,13 @@
 #include "jiminy/core/robot/Robot.h"
 #include "jiminy/core/Macros.h"
 
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 
 
 namespace jiminy
 {
     AbstractConstraintBase::AbstractConstraintBase(void) :
+    lambda_(),
     model_(),
     isAttached_(false),
     isEnabled_(true),
@@ -63,6 +64,7 @@ namespace jiminy
 
     void AbstractConstraintBase::disable(void)
     {
+        lambda_.setZero();
         isEnabled_ = false;
     }
 

core/src/robot/DistanceConstraint.cc → core/src/constraints/DistanceConstraint.cc

@@ -3,7 +3,7 @@
 #include "jiminy/core/robot/Model.h"
 #include "jiminy/core/utilities/Pinocchio.h"
 
-#include "jiminy/core/robot/DistanceConstraint.h"
+#include "jiminy/core/constraints/DistanceConstraint.h"
 
 
 namespace jiminy
@@ -75,9 +75,10 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            // Set jacobian / drift to right dimension
+            // Set jacobian, drift and multipliers to right dimension
             jacobian_ = matrixN_t::Zero(1, model->pncModel_.nv);
             drift_ = vectorN_t::Zero(1);
+            lambda_ = vectorN_t::Zero(1);
 
             // Initialize frames jacobians buffers
             firstFrameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv);

core/src/robot/FixedFrameConstraint.cc → core/src/constraints/FixedFrameConstraint.cc

@@ -3,7 +3,7 @@
 #include "jiminy/core/robot/Model.h"
 #include "jiminy/core/utilities/Pinocchio.h"
 
-#include "jiminy/core/robot/FixedFrameConstraint.h"
+#include "jiminy/core/constraints/FixedFrameConstraint.h"
 
 
 namespace jiminy
@@ -81,11 +81,12 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            // Set jacobian / drift to right dimension
+            // Initialize jacobian, drift and multipliers
             frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv);
             uint32_t dim = 3U * (uint32_t(isTranslationFixed_) + uint32_t(isRotationFixed_));
             jacobian_ = matrixN_t::Zero(dim, model->pncModel_.nv);
             drift_ = vectorN_t::Zero(dim);
+            lambda_ = vectorN_t::Zero(dim);
 
             // Get the current frame position and use it as reference
             transformRef_ = model->pncData_.oMf[frameIdx_];

core/src/robot/JointConstraint.cc → core/src/constraints/JointConstraint.cc

@@ -2,7 +2,7 @@
 #include "jiminy/core/Macros.h"
 #include "jiminy/core/Types.h"
 
-#include "jiminy/core/robot/JointConstraint.h"
+#include "jiminy/core/constraints/JointConstraint.h"
 
 
 namespace jiminy
@@ -68,15 +68,16 @@ namespace jiminy
             // Get the joint model
             pinocchio::JointModel const & jointModel = model->pncModel_.joints[jointIdx_];
 
-            // Compute the jacobian. It is simply the velocity selector mask.
+            // Initialize the jacobian. It is simply the velocity selector mask.
             jacobian_ = matrixN_t::Zero(jointModel.nv(), model->pncModel_.nv);
             for (Eigen::Index i=0; i < jointModel.nv(); ++i)
             {
                 jacobian_(i, jointModel.idx_v() + i) = 1.0;
             }
 
-            // Compute the drift.
+            // Initialize drift and multipliers
             drift_ = vectorN_t::Zero(jointModel.nv());
+            lambda_ = vectorN_t::Zero(jointModel.nv());
 
             // Get the current joint position and use it as reference
             configurationRef_ = jointModel.jointConfigSelector(q);

core/src/robot/SphereConstraint.cc → core/src/constraints/SphereConstraint.cc

@@ -3,7 +3,7 @@
 #include "jiminy/core/robot/Model.h"
 #include "jiminy/core/utilities/Pinocchio.h"
 
-#include "jiminy/core/robot/SphereConstraint.h"
+#include "jiminy/core/constraints/SphereConstraint.h"
 
 
 namespace jiminy
@@ -72,10 +72,11 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            // Set jacobian / drift to right dimension
+            // Initialize jacobian, drift and multipliers
             frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv);
             jacobian_ = matrixN_t::Zero(3, model->pncModel_.nv);
             drift_ = vectorN_t::Zero(3);
+            lambda_ = vectorN_t::Zero(3);
 
             // Get the current frame position and use it as reference
             transformRef_ = model->pncData_.oMf[frameIdx_];

core/src/robot/WheelConstraint.cc → core/src/constraints/WheelConstraint.cc

@@ -3,7 +3,7 @@
 #include "jiminy/core/robot/Model.h"
 #include "jiminy/core/utilities/Pinocchio.h"
 
-#include "jiminy/core/robot/WheelConstraint.h"
+#include "jiminy/core/constraints/WheelConstraint.h"
 
 
 namespace jiminy
@@ -76,10 +76,11 @@ namespace jiminy
 
         if (returnCode == hresult_t::SUCCESS)
         {
-            // Set jacobian / drift to right dimension
+            // Initialize jacobian, drift and multipliers
             frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv);
             jacobian_ = matrixN_t::Zero(3, model->pncModel_.nv);
             drift_ = vectorN_t::Zero(3);
+            lambda_ = vectorN_t::Zero(3);
 
             // Get the current frame position and use it as reference
             transformRef_ = model->pncData_.oMf[frameIdx_];

core/src/engine/EngineMultiRobot.cc

@@ -34,9 +34,9 @@
 #include "jiminy/core/robot/PinocchioOverloadAlgorithms.h"
 #include "jiminy/core/robot/AbstractMotor.h"
 #include "jiminy/core/robot/AbstractSensor.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
-#include "jiminy/core/robot/JointConstraint.h"
-#include "jiminy/core/robot/FixedFrameConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
+#include "jiminy/core/constraints/JointConstraint.h"
+#include "jiminy/core/constraints/FixedFrameConstraint.h"
 #include "jiminy/core/robot/Robot.h"
 #include "jiminy/core/control/AbstractController.h"
 #include "jiminy/core/control/ControllerFunctor.h"
@@ -3696,8 +3696,11 @@ namespace jiminy
 
             // Compute kinematic constraints
             system.robot->computeConstraints(q, v);
+
+            // Extract updated jacobian, drift and multipliers
             auto constraintsJacobian = system.robot->getConstraintsJacobian();
             auto constraintsDrift = system.robot->getConstraintsDrift();
+            data.lambda_c = system.robot->getConstraintsLambda();
 
             // Compute constraints bounds
             lo = vectorN_t::Constant(constraintsDrift.size(), -INF);
@@ -3788,20 +3791,23 @@ namespace jiminy
             systemData.constraintsHolder.foreach(
                 constraintsHolderType_t::BOUNDS_JOINTS,
                 [&constraintIdx,
-                    &lambda_c = const_cast<vectorN_t &>(data.lambda_c),  // std::as_const is not supported by gcc<7.3
-                    &u = systemData.state.u,
-                    &uInternal = systemData.state.uInternal,
-                    &joints = const_cast<pinocchio::Model::JointModelVector &>(model.joints)](
-                    std::shared_ptr<AbstractConstraintBase> const & constraint,
+                 &lambda_c = const_cast<vectorN_t &>(data.lambda_c),  // std::as_const is not supported by gcc<7.3
+                 &u = systemData.state.u,
+                 &uInternal = systemData.state.uInternal,
+                 &joints = const_cast<pinocchio::Model::JointModelVector &>(model.joints)](
+                    std::shared_ptr<AbstractConstraintBase> & constraint,
                     constraintsHolderType_t const & /* holderType */)
                 {
                     if (!constraint->getIsEnabled())
                     {
                         return;
                     }
+
+                    vectorN_t & uJoint = constraint->lambda_;
+                    uJoint = lambda_c.segment(constraintIdx, constraint->getDim());
+
                     auto const & jointConstraint = static_cast<JointConstraint const &>(*constraint.get());
                     auto const & jointModel = joints[jointConstraint.getJointIdx()];
-                    auto uJoint = lambda_c.segment(constraintIdx, constraint->getDim());  // auto to avoid memory allocation
                     jointModel.jointVelocitySelector(uInternal) += uJoint;
                     jointModel.jointVelocitySelector(u) += uJoint;
                     constraintIdx += constraint->getDim();
@@ -3812,16 +3818,17 @@ namespace jiminy
             for ( ; constraintIt != systemData.constraintsHolder.contactFrames.end() ;
                 ++constraintIt, ++forceIt)
             {
-                auto const & constraint = *constraintIt->second.get();
+                auto & constraint = *constraintIt->second.get();
                 if (!constraint.getIsEnabled())
                 {
                     continue;
                 }
                 auto const & frameConstraint = static_cast<FixedFrameConstraint const &>(constraint);
                 // if (frameConstraint.getIsTranslationFixed())
                 // {
-                    // Extract force from lagrangian multipliers
-                    auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
+                    // Extract part of the lagrangian multipliers associated with the constraint
+                    vectorN_t & fextWorld = constraint.lambda_;
+                    fextWorld = data.lambda_c.segment<3>(constraintIdx);
 
                     // Convert the force from local world aligned to local frame
                     frameIndex_t const & frameIdx = frameConstraint.getFrameIdx();
@@ -3839,7 +3846,7 @@ namespace jiminy
             systemData.constraintsHolder.foreach(
                 constraintsHolderType_t::COLLISION_BODIES,
                 [&constraintIdx, &fext, &model, &data](
-                    std::shared_ptr<AbstractConstraintBase> const & constraint,
+                    std::shared_ptr<AbstractConstraintBase> & constraint,
                     constraintsHolderType_t const & /* holderType */)
                 {
                     if (!constraint || !constraint->getIsEnabled())
@@ -3850,8 +3857,9 @@ namespace jiminy
                     // auto const & collisionConstraint = static_cast<SphereConstraint const &>(*constraint.get());
                     auto const & collisionConstraint = static_cast<FixedFrameConstraint const &>(*constraint.get());
 
-                    // Extract force from lagrangian multipliers
-                    auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
+                    // Extract part of the lagrangian multipliers associated with the constraint
+                    vectorN_t & fextWorld = constraint->lambda_;
+                    fextWorld = data.lambda_c.segment<3>(constraintIdx);
 
                     // Convert the force from local world aligned to local parent joint
                     frameIndex_t const & frameIdx = collisionConstraint.getFrameIdx();

core/src/engine/System.cc

@@ -2,7 +2,7 @@
 #include "pinocchio/algorithm/joint-configuration.hpp"  // `pinocchio::neutral`
 
 #include "jiminy/core/robot/Robot.h"
-#include "jiminy/core/robot/AbstractConstraint.h"
+#include "jiminy/core/constraints/AbstractConstraint.h"
 #include "jiminy/core/control/AbstractController.h"
 #include "jiminy/core/utilities/Helpers.h"
 #include "jiminy/core/engine/System.h"