[core] Fix broken 'getRigidVelocityFromFlexible'.

duburcqa · Mar 3, 2024 · f39e6c7 · f39e6c7
1 parent 509d95b
commit f39e6c7
Showing 1 changed file with 24 additions and 26 deletions.
diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc
@@ -1672,22 +1672,25 @@ namespace jiminy
         }
     }
 
-    void Model::getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex,
-                                             Eigen::VectorXd & qRigid) const
+
+    void Model::getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid,
+                                             Eigen::VectorXd & vFlex) const
     {
         // Define some proxies
-        uint32_t nqFlex = pncModelFlexibleOrig_.nq;
+        uint32_t nvRigid = pinocchioModelOrig_.nv;
+        uint32_t nvFlex = pncModelFlexibleOrig_.nv;
 
         // Check the size of the input state
-        if (qFlex.size() != nqFlex)
+        if (vRigid.size() != nvRigid)
         {
-            THROW_ERROR(std::invalid_argument, "Size of qFlex inconsistent with flexible model.");
+            THROW_ERROR(std::invalid_argument,
+                        "Size of vRigid inconsistent with theoretical model.");
         }
 
-        // Initialize the rigid state
-        qRigid = pinocchio::neutral(pinocchioModelOrig_);
+        // Initialize the flexible state
+        vFlex.setZero(nvFlex);
 
-        // Compute the rigid state based on the flexible state
+        // Compute the flexible state based on the rigid state
         int32_t idxRigid = 0;
         int32_t idxFlex = 0;
         for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex)
@@ -1700,32 +1703,30 @@ namespace jiminy
                 const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex];
                 if (jointRigid.idx_q() >= 0)
                 {
-                    qRigid.segment(jointRigid.idx_q(), jointRigid.nq()) =
-                        qFlex.segment(jointFlex.idx_q(), jointFlex.nq());
+                    vFlex.segment(jointFlex.idx_v(), jointFlex.nv()) =
+                        vRigid.segment(jointRigid.idx_v(), jointRigid.nv());
                 }
                 ++idxRigid;
             }
         }
     }
 
-    void Model::getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid,
-                                             Eigen::VectorXd & vFlex) const
+    void Model::getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex,
+                                             Eigen::VectorXd & qRigid) const
     {
         // Define some proxies
-        uint32_t nvRigid = pinocchioModelOrig_.nv;
-        uint32_t nvFlex = pncModelFlexibleOrig_.nv;
+        uint32_t nqFlex = pncModelFlexibleOrig_.nq;
 
         // Check the size of the input state
-        if (vRigid.size() != nvRigid)
+        if (qFlex.size() != nqFlex)
         {
-            THROW_ERROR(std::invalid_argument,
-                        "Size of vRigid inconsistent with theoretical model.");
+            THROW_ERROR(std::invalid_argument, "Size of qFlex inconsistent with flexible model.");
         }
 
-        // Initialize the flexible state
-        vFlex.setZero(nvFlex);
+        // Initialize the rigid state
+        qRigid = pinocchio::neutral(pinocchioModelOrig_);
 
-        // Compute the flexible state based on the rigid state
+        // Compute the rigid state based on the flexible state
         int32_t idxRigid = 0;
         int32_t idxFlex = 0;
         for (; idxRigid < pinocchioModelOrig_.njoints; ++idxFlex)
@@ -1738,8 +1739,8 @@ namespace jiminy
                 const auto & jointFlex = pncModelFlexibleOrig_.joints[idxFlex];
                 if (jointRigid.idx_q() >= 0)
                 {
-                    vFlex.segment(jointFlex.idx_v(), jointFlex.nv()) =
-                        vRigid.segment(jointRigid.idx_v(), jointRigid.nv());
+                    qRigid.segment(jointRigid.idx_q(), jointRigid.nq()) =
+                        qFlex.segment(jointFlex.idx_q(), jointFlex.nq());
                 }
                 ++idxRigid;
             }
@@ -1778,10 +1779,7 @@ namespace jiminy
                     vRigid.segment(jointRigid.idx_v(), jointRigid.nv()) =
                         vFlex.segment(jointFlex.idx_v(), jointFlex.nv());
                 }
-            }
-            else
-            {
-                ++idxFlex;
+                ++idxRigid;
             }
         }
     }