[python|gym] Add contraint lambda multipliers to state and extracted trajectory.

.pylintrc

@@ -71,6 +71,7 @@ disable =
     useless-parent-delegation,
     use-dict-literal,
     unspecified-encoding,
+    undefined-loop-variable,
     cyclic-import
 
 # Enable the message, report, category or checker with the given id(s)

core/include/jiminy/core/constraints/abstract_constraint.h

@@ -55,7 +55,7 @@ namespace jiminy
         virtual const std::string & getType() const = 0;
 
         /// \brief Dimension of the constraint.
-        uint64_t getDim() const;
+        uint64_t getSize() const;
 
         /// \brief Jacobian of the constraint.
         const Eigen::MatrixXd & getJacobian() const;

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

@@ -201,6 +201,7 @@ namespace jiminy
         std::vector<std::string> logAccelerationFieldnames{};
         std::vector<std::string> logEffortFieldnames{};
         std::vector<std::string> logForceExternalFieldnames{};
+        std::vector<std::string> logConstraintFieldnames{};
         std::vector<std::string> logCommandFieldnames{};
         std::string logEnergyFieldname{};
 
@@ -329,6 +330,7 @@ namespace jiminy
             config["enableVelocity"] = true;
             config["enableAcceleration"] = true;
             config["enableForceExternal"] = false;
+            config["enableConstraint"] = false;
             config["enableCommand"] = false;
             config["enableEffort"] = false;
             config["enableEnergy"] = false;
@@ -438,6 +440,7 @@ namespace jiminy
             const bool enableVelocity;
             const bool enableAcceleration;
             const bool enableForceExternal;
+            const bool enableConstraint;
             const bool enableCommand;
             const bool enableEffort;
             const bool enableEnergy;
@@ -448,6 +451,7 @@ namespace jiminy
             enableVelocity{boost::get<bool>(options.at("enableVelocity"))},
             enableAcceleration{boost::get<bool>(options.at("enableAcceleration"))},
             enableForceExternal{boost::get<bool>(options.at("enableForceExternal"))},
+            enableConstraint{boost::get<bool>(options.at("enableConstraint"))},
             enableCommand{boost::get<bool>(options.at("enableCommand"))},
             enableEffort{boost::get<bool>(options.at("enableEffort"))},
             enableEnergy{boost::get<bool>(options.at("enableEnergy"))}

core/include/jiminy/core/robot/model.h

@@ -21,12 +21,14 @@ namespace jiminy
     class AbstractConstraintBase;
     class FrameConstraint;
     class JointConstraint;
+    class MutexLocal;
+    class LockGuardLocal;
 
     // ************************************** Constraints ************************************** //
 
     using ConstraintMap = static_map_t<std::string, std::shared_ptr<AbstractConstraintBase>>;
 
-    enum class ConstraintNodeType : uint8_t
+    enum class ConstraintRegistryType : uint8_t
     {
         CONTACT_FRAMES = 0,
         COLLISION_BODIES = 1,
@@ -37,77 +39,74 @@ namespace jiminy
     /* Note that the following ordering plays a critical role as it determines in which order
        `foreach` iterates over all the constraints. This has a directly effect on the solution
        found by 'PGS' constraint solvers. */
-    inline constexpr std::array constraintNodeTypesAll{ConstraintNodeType::BOUNDS_JOINTS,
-                                                       ConstraintNodeType::CONTACT_FRAMES,
-                                                       ConstraintNodeType::COLLISION_BODIES,
-                                                       ConstraintNodeType::USER};
+    inline constexpr std::array constraintNodeTypesAll{ConstraintRegistryType::BOUNDS_JOINTS,
+                                                       ConstraintRegistryType::CONTACT_FRAMES,
+                                                       ConstraintRegistryType::COLLISION_BODIES,
+                                                       ConstraintRegistryType::USER};
 
     struct JIMINY_DLLAPI ConstraintTree
     {
     public:
-        void clear() noexcept;
-
         std::pair<ConstraintMap *, ConstraintMap::iterator> find(const std::string & key,
-                                                                 ConstraintNodeType node);
+                                                                 ConstraintRegistryType type);
         std::pair<const ConstraintMap *, ConstraintMap::const_iterator> find(
-            const std::string & key, ConstraintNodeType node) const;
+            const std::string & key, ConstraintRegistryType type) const;
 
-        bool exist(const std::string & key) const;
-        bool exist(const std::string & key, ConstraintNodeType node) const;
+        bool exist(const std::string & key, ConstraintRegistryType type) const;
 
-        std::shared_ptr<AbstractConstraintBase> get(const std::string & key) const;
         std::shared_ptr<AbstractConstraintBase> get(const std::string & key,
-                                                    ConstraintNodeType node) const;
+                                                    ConstraintRegistryType type) const;
 
-        void insert(const ConstraintMap & constraintMap, ConstraintNodeType node);
+        void insert(const ConstraintMap & constraintMap, ConstraintRegistryType type);
 
-        ConstraintMap::iterator erase(const std::string & key, ConstraintNodeType node);
+        ConstraintMap::iterator erase(const std::string & key, ConstraintRegistryType type);
+
+        void clear() noexcept;
 
         template<typename Function>
-        void foreach(ConstraintNodeType node, Function && func) const
+        void foreach(ConstraintRegistryType type, Function && func) const
         {
-            if (node == ConstraintNodeType::COLLISION_BODIES)
+            if (type == ConstraintRegistryType::COLLISION_BODIES)
             {
                 for (auto & constraintMap : collisionBodies)
                 {
                     for (auto & constraintItem : constraintMap)
                     {
-                        std::invoke(std::forward<Function>(func), constraintItem.second, node);
+                        std::invoke(std::forward<Function>(func), constraintItem.second, type);
                     }
                 }
             }
             else
             {
                 const ConstraintMap * constraintMapPtr;
-                switch (node)
+                switch (type)
                 {
-                case ConstraintNodeType::BOUNDS_JOINTS:
+                case ConstraintRegistryType::BOUNDS_JOINTS:
                     constraintMapPtr = &boundJoints;
                     break;
-                case ConstraintNodeType::CONTACT_FRAMES:
+                case ConstraintRegistryType::CONTACT_FRAMES:
                     constraintMapPtr = &contactFrames;
                     break;
-                case ConstraintNodeType::USER:
-                    constraintMapPtr = &registry;
+                case ConstraintRegistryType::USER:
+                    constraintMapPtr = &user;
                     break;
-                case ConstraintNodeType::COLLISION_BODIES:
+                case ConstraintRegistryType::COLLISION_BODIES:
                 default:
                     constraintMapPtr = nullptr;
                 }
                 for (const auto & constraintItem : *constraintMapPtr)
                 {
-                    std::invoke(std::forward<Function>(func), constraintItem.second, node);
+                    std::invoke(std::forward<Function>(func), constraintItem.second, type);
                 }
             }
         }
 
         template<typename Function, std::size_t N>
-        void foreach(const std::array<ConstraintNodeType, N> & constraintsHolderTypes,
-                     Function && func) const
+        void foreach(const std::array<ConstraintRegistryType, N> & types, Function && func) const
         {
-            for (ConstraintNodeType node : constraintsHolderTypes)
+            for (ConstraintRegistryType type : types)
             {
-                foreach(node, std::forward<Function>(func));
+                foreach(type, std::forward<Function>(func));
             }
         }
 
@@ -125,7 +124,7 @@ namespace jiminy
         /// \brief Constraints registered by the engine to handle collision bounds.
         std::vector<ConstraintMap> collisionBodies{};
         /// \brief Constraints explicitly registered by user.
-        ConstraintMap registry{};
+        ConstraintMap user{};
     };
 
     // ***************************************** Model ***************************************** //
@@ -306,18 +305,8 @@ namespace jiminy
         /// \param[in] constraintName Unique name identifying the kinematic constraint.
         void removeConstraint(const std::string & constraintName);
 
-        /// \brief Pointer to the constraint referenced by constraintName
-        ///
-        /// \param[in] constraintName Name of the constraint to get.
-        std::shared_ptr<AbstractConstraintBase> getConstraint(const std::string & constraintName);
-
-        std::weak_ptr<const AbstractConstraintBase> getConstraint(
-            const std::string & constraintName) const;
-
         const ConstraintTree & getConstraints() const;
 
-        bool existConstraint(const std::string & constraintName) const;
-
         /// \brief Returns true if at least one constraint is active on the robot.
         bool hasConstraints() const;
 
@@ -371,6 +360,7 @@ namespace jiminy
         const std::vector<std::string> & getLogAccelerationFieldnames() const;
         const std::vector<std::string> & getLogEffortFieldnames() const;
         const std::vector<std::string> & getLogForceExternalFieldnames() const;
+        const std::vector<std::string> & getLogConstraintFieldnames() const;
 
         void getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical,
                                                 Eigen::VectorXd & qExtended) const;
@@ -381,6 +371,9 @@ namespace jiminy
         void getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended,
                                                 Eigen::VectorXd & vTheoretical) const;
 
+        virtual std::unique_ptr<LockGuardLocal> getLock();
+        bool getIsLocked() const;
+
     protected:
         void generateModelExtended(const uniform_random_bit_generator_ref<uint32_t> & g);
 
@@ -397,16 +390,14 @@ namespace jiminy
 
         void addConstraint(const std::string & constraintName,
                            const std::shared_ptr<AbstractConstraintBase> & constraint,
-                           ConstraintNodeType node);
-        void addConstraints(const ConstraintMap & constraintMap, ConstraintNodeType node);
-        void removeConstraint(const std::string & constraintName, ConstraintNodeType node);
+                           ConstraintRegistryType type);
+        void addConstraints(const ConstraintMap & constraintMap, ConstraintRegistryType type);
+        void removeConstraint(const std::string & constraintName, ConstraintRegistryType type);
         void removeConstraints(const std::vector<std::string> & constraintNames,
-                               ConstraintNodeType node);
+                               ConstraintRegistryType type);
 
         void refreshGeometryProxies();
         void refreshContactProxies();
-        /// \brief Refresh the proxies of the kinematics constraints.
-        void refreshConstraintProxies();
         virtual void refreshProxies();
 
     public:
@@ -476,8 +467,12 @@ namespace jiminy
         /// \brief Concatenated fieldnames of the external force applied at each joint of the
         ///        model, 'universe' excluded.
         std::vector<std::string> logForceExternalFieldnames_{};
+        /// \brief Concatenated fieldnames of all the constraints.
+        std::vector<std::string> logConstraintFieldnames_{};
 
     private:
+        std::unique_ptr<MutexLocal> mutexLocal_{std::make_unique<MutexLocal>()};
+
         /// \brief Vector of joints acceleration corresponding to a copy of data.a.
         //         Used for computing constraints as a temporary buffer.
         MotionVector jointSpatialAccelerations_{};

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

@@ -46,9 +46,9 @@ namespace jiminy::pinocchio_overload
                          pinocchio::DataTpl<Scalar, Options, JointCollectionTpl> & data,
                          const Eigen::MatrixBase<ConfigVectorType> & q,
                          const Eigen::MatrixBase<TangentVectorType> & v,
-                         bool update_kinematics = true)
+                         bool updateKinematics = true)
     {
-        if (update_kinematics)
+        if (updateKinematics)
         {
             pinocchio::forwardKinematics(model, data, q, v);
         }

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

@@ -15,8 +15,6 @@ namespace jiminy
     class AbstractSensorBase;
     class AbstractController;
     class TelemetryData;
-    class MutexLocal;
-    class LockGuardLocal;
 
     class JIMINY_DLLAPI Robot : public Model
     {
@@ -115,15 +113,14 @@ namespace jiminy
 
         /// \remark This method does not have to be called manually before running a simulation.
         ///         The Engine is taking care of it.
-        virtual void reset(const uniform_random_bit_generator_ref<uint32_t> & g) override;
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) override;
         virtual void configureTelemetry(std::shared_ptr<TelemetryData> telemetryData);
         void updateTelemetry();
         bool getIsTelemetryConfigured() const;
 
         const std::vector<std::string> & getLogCommandFieldnames() const;
 
-        std::unique_ptr<LockGuardLocal> getLock();
-        bool getIsLocked() const;
+        std::unique_ptr<LockGuardLocal> getLock() override;
 
         // Getters without 'get' prefix for consistency with pinocchio C++ API
         Eigen::Index nmotors() const;
@@ -161,7 +158,6 @@ namespace jiminy
         Eigen::VectorXd motorEffortLimit_;
 
     private:
-        std::unique_ptr<MutexLocal> mutexLocal_{std::make_unique<MutexLocal>()};
         std::shared_ptr<MotorSharedStorage> motorSharedStorage_;
         std::unordered_map<std::string, std::shared_ptr<SensorSharedStorage>>
             sensorSharedStorageMap_{};

core/src/constraints/abstract_constraint.cc

@@ -109,7 +109,7 @@ namespace jiminy
         return zeta / (2.0 * M_PI);
     }
 
-    uint64_t AbstractConstraintBase::getDim() const
+    uint64_t AbstractConstraintBase::getSize() const
     {
         return static_cast<uint64_t>(drift_.size());
     }