[gym/common] Refresh state quantities only if needed.

duburcqa · Jun 16, 2024 · 5023fe5 · 5023fe5
1 parent bc40c90
commit 5023fe5
Show file tree

Hide file tree

Showing 20 changed files with 532 additions and 347 deletions.
diff --git a/.pylintrc b/.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)

diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/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);
         }

diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc
@@ -793,6 +793,7 @@ namespace jiminy
     void computeExtraTerms(
         std::shared_ptr<Robot> & robot, const RobotData & robotData, ForceVector & fExt)
     {
+        // Define some proxies for convenience
         const pinocchio::Model & model = robot->pinocchioModel_;
         pinocchio::Data & data = robot->pinocchioData_;
 
@@ -801,7 +802,7 @@ namespace jiminy
             model, data, robotData.state.q, robotData.state.v, false);
         pinocchio::computePotentialEnergy(model, data);
 
-        /* Update manually the subtree (apparent) inertia, since it is only computed by crba, which
+        /* Update manually the subtree (apparent) inertia, since it is only computed by CRBA, which
            is doing more computation than necessary. It will be used here for computing the
            centroidal kinematics, and used later for joint bounds dynamics. Note that, by doing all
            the computations here instead of 'computeForwardKinematics', we are doing the assumption
@@ -823,13 +824,25 @@ namespace jiminy
             }
         }
 
-        /* Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint
-           accelerations, joint forces and body forces, so it must be done separately:
-           - 1st step: computing the accelerations based on ForwardKinematic algorithm
-           - 2nd step: computing the forces based on RNEA algorithm */
+        /* The objective here is to compute the actual joint accelerations and the joint internal
+           forces. The latter are not involved in dynamic computations, but are useful for analysis
+           of the mechanical design. Indeed, it brings information about stresses and strains
+           applied to the mechanical structure, which may cause unexpected fatigue wear. In
+           addition, the body external forces are also evaluated, as an intermediate quantity for
+           computing the centroidal dynamics, ie the spatial momentum of the whole robot at the
+           Center of Mass along with its temporal derivative.
+
+           Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint
+           accelerations, the joint internal forces and the body external forces. Hence, it is done
+           manually, following a two computation steps procedure:
+           * joint accelerations based on ForwardKinematic algorithm
+           * joint internal forces and body external forces based on RNEA algorithm */
 
         /* Compute the true joint acceleration and the one due to the lone gravity field, then
-           the spatial momenta and the total internal and external forces acting on each body. */
+           the spatial momenta and the total internal and external forces acting on each body.
+           * `fExt` is used as a buffer for storing the total body external forces. It serves
+             no purpose other than being an intermediate quantity for other computations.
+           * `data.f` stores the joint internal forces */
         data.h[0].setZero();
         fExt[0].setZero();
         data.f[0].setZero();

diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc
@@ -592,7 +592,7 @@ namespace jiminy
             std::string backlashName = jointName;
             backlashName += BACKLASH_JOINT_SUFFIX;
 
-            // Check if constraint a joint bounds constraint exist
+            // Check if the corresponding joint bound constraint already exists
             const bool hasConstraint =
                 constraints_.exist(backlashName, ConstraintRegistryType::BOUNDS_JOINTS);
 
@@ -605,10 +605,13 @@ namespace jiminy
                 {
                     removeConstraint(backlashName, ConstraintRegistryType::BOUNDS_JOINTS);
                 }
-
                 continue;
             }
 
+            // Add backlash joint to the model
+            addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName);
+            backlashJointNames_.push_back(backlashName);
+
             // Add joint bounds constraint if necessary
             if (!hasConstraint)
             {
@@ -617,10 +620,6 @@ namespace jiminy
                 addConstraint(backlashName, constraint, ConstraintRegistryType::BOUNDS_JOINTS);
             }
 
-            // Add backlash joint to the model
-            addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName);
-            backlashJointNames_.push_back(backlashName);
-
             // Update position limits in model
             const Eigen::Index positionIndex =
                 getJointPositionFirstIndex(pinocchioModel_, backlashName);