Merge pull request #13 from ihmcrobotics/feature/fwd-dyn-joint-wrench

Add the option to compute the full joint wrench.
ihmcrobotics · Jan 10, 2024 · dc7dd83 · dc7dd83
2 parents f5bb2b5 + c0946a8
commit dc7dd83
Show file tree

Hide file tree

Showing 15 changed files with 502 additions and 507 deletions.
diff --git a/src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java b/src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java
@@ -1,21 +1,12 @@
 package us.ihmc.mecano.algorithms;
 
-import java.util.ArrayList;
-import java.util.Collection;
-import java.util.List;
-
 import org.ejml.data.DMatrix1Row;
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
-
 import us.ihmc.euclid.referenceFrame.FramePoint3D;
 import us.ihmc.euclid.referenceFrame.FrameVector3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
-import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
-import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DBasics;
-import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
-import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;
+import us.ihmc.euclid.referenceFrame.interfaces.*;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointMatrixIndexProvider;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
@@ -27,6 +18,10 @@
 import us.ihmc.mecano.tools.MecanoFactories;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.List;
+
 /**
  * Computes the center of mass Jacobian that maps from joint velocity space to center of mass
  * Cartesian velocity space.
@@ -190,7 +185,9 @@ public CenterOfMassJacobian(MultiBodySystemReadOnly input, ReferenceFrame jacobi
       this(input, jacobianFrame, null, considerIgnoredSubtreesInertia);
    }
 
-   private CenterOfMassJacobian(MultiBodySystemReadOnly input, ReferenceFrame jacobianFrame, String centerOfMassFrameName,
+   private CenterOfMassJacobian(MultiBodySystemReadOnly input,
+                                ReferenceFrame jacobianFrame,
+                                String centerOfMassFrameName,
                                 boolean considerIgnoredSubtreesInertia)
    {
       this.input = input;
@@ -231,22 +228,7 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
       List<RecursionStep> recursionSteps = new ArrayList<>();
       recursionSteps.add(parent);
 
-      List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());
-
-      if (childrenJoints.size() > 1)
-      { // Reorganize the joints in the children to ensure that loop closures are treated last.
-         List<JointReadOnly> loopClosureAncestors = new ArrayList<>();
-
-         for (int i = 0; i < childrenJoints.size();)
-         {
-            if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
-               loopClosureAncestors.add(childrenJoints.remove(i));
-            else
-               i++;
-         }
-
-         childrenJoints.addAll(loopClosureAncestors);
-      }
+      List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);
 
       for (JointReadOnly childJoint : childrenJoints)
       {

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java
@@ -1,13 +1,8 @@
 package us.ihmc.mecano.algorithms;
 
-import java.util.ArrayList;
-import java.util.Collection;
-import java.util.List;
-
 import org.ejml.data.DMatrix1Row;
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
-
 import us.ihmc.euclid.referenceFrame.FrameVector3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
@@ -29,6 +24,10 @@
 import us.ihmc.mecano.tools.JointStateType;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.List;
+
 /**
  * Computes the centroidal momentum matrix that maps from joint velocity space to the system linear
  * and angular momentum.
@@ -156,22 +155,7 @@ private List<IterativeStep> buildMultiBodyTree(IterativeStep parent, Collection<
       List<IterativeStep> iterativeSteps = new ArrayList<>();
       iterativeSteps.add(parent);
 
-      List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());
-
-      if (childrenJoints.size() > 1)
-      { // Reorganize the joints in the children to ensure that loop closures are treated last.
-         List<JointReadOnly> loopClosureAncestors = new ArrayList<>();
-
-         for (int i = 0; i < childrenJoints.size();)
-         {
-            if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
-               loopClosureAncestors.add(childrenJoints.remove(i));
-            else
-               i++;
-         }
-
-         childrenJoints.addAll(loopClosureAncestors);
-      }
+      List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);
 
       for (JointReadOnly childJoint : childrenJoints)
       {

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java
@@ -1,17 +1,8 @@
 package us.ihmc.mecano.algorithms;
 
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collection;
-import java.util.List;
-import java.util.Map;
-import java.util.function.Function;
-import java.util.stream.Collectors;
-
 import org.ejml.data.DMatrix1Row;
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
-
 import us.ihmc.euclid.referenceFrame.FrameVector3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
@@ -24,24 +15,16 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
-import us.ihmc.mecano.spatial.Momentum;
-import us.ihmc.mecano.spatial.SpatialAcceleration;
-import us.ihmc.mecano.spatial.SpatialForce;
-import us.ihmc.mecano.spatial.SpatialInertia;
-import us.ihmc.mecano.spatial.Twist;
-import us.ihmc.mecano.spatial.Wrench;
-import us.ihmc.mecano.spatial.interfaces.FixedFrameMomentumBasics;
-import us.ihmc.mecano.spatial.interfaces.FixedFrameSpatialForceBasics;
-import us.ihmc.mecano.spatial.interfaces.MomentumBasics;
-import us.ihmc.mecano.spatial.interfaces.MomentumReadOnly;
-import us.ihmc.mecano.spatial.interfaces.SpatialForceBasics;
-import us.ihmc.mecano.spatial.interfaces.SpatialForceReadOnly;
-import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
-import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
+import us.ihmc.mecano.spatial.*;
+import us.ihmc.mecano.spatial.interfaces.*;
 import us.ihmc.mecano.tools.JointStateType;
 import us.ihmc.mecano.tools.MecanoTools;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 
+import java.util.*;
+import java.util.function.Function;
+import java.util.stream.Collectors;
+
 /**
  * Computes the centroidal momentum matrix that maps from joint velocity space to the system linear
  * and angular momentum and the convective term representing the Coriolis and centrifugal wrenches
@@ -219,22 +202,7 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
       List<RecursionStep> recursionSteps = new ArrayList<>();
       recursionSteps.add(parent);
 
-      List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());
-
-      if (childrenJoints.size() > 1)
-      { // Reorganize the joints in the children to ensure that loop closures are treated last.
-         List<JointReadOnly> loopClosureAncestors = new ArrayList<>();
-
-         for (int i = 0; i < childrenJoints.size();)
-         {
-            if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
-               loopClosureAncestors.add(childrenJoints.remove(i));
-            else
-               i++;
-         }
-
-         childrenJoints.addAll(loopClosureAncestors);
-      }
+      List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);
 
       for (JointReadOnly childJoint : childrenJoints)
       {
@@ -544,11 +512,11 @@ public SpatialForceReadOnly getBiasSpatialForce()
 
    /**
     * Computes the convective term while considering only a subset of the multi-body system.
-    * 
+    *
     * @param jointSelection         the only joints that are considered.
     * @param biasSpatialForceToPack the vector used to store the result. Modified.
     * @return {@code true} if the convective term was successfully computed, {@code false} if not all
-    *         the joints could be found and the result is inaccurate.
+    *       the joints could be found and the result is inaccurate.
     */
    public boolean getBiasSpatialForceMatrix(List<? extends JointReadOnly> jointSelection, SpatialForceBasics biasSpatialForceToPack)
    {
@@ -586,11 +554,11 @@ public DMatrixRMaj getBiasSpatialForceMatrix()
 
    /**
     * Computes the convective term while considering only a subset of the multi-body system.
-    * 
-    * @param jointSelection         the only joints that are considered.
-    * @param biasSpatialForceToPack the matrix used to store the result. Modified.
+    *
+    * @param jointSelection               the only joints that are considered.
+    * @param biasSpatialForceMatrixToPack the matrix used to store the result. Modified.
     * @return {@code true} if the convective term was successfully computed, {@code false} if not all
-    *         the joints could be found and the result is inaccurate.
+    *       the joints could be found and the result is inaccurate.
     */
    public boolean getBiasSpatialForceMatrix(List<? extends JointReadOnly> jointSelection, DMatrixRMaj biasSpatialForceMatrixToPack)
    {
@@ -762,7 +730,6 @@ public void passTwo()
             int column = jointIndices[dofIndex];
             CommonOps_DDRM.extract(centroidalMomentumMatrixBlock, 0, 6, dofIndex, dofIndex + 1, centroidalMomentumMatrix, 0, column);
          }
-
       }
 
       /**

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java
@@ -1,29 +1,23 @@
 package us.ihmc.mecano.algorithms;
 
-import java.util.ArrayList;
-import java.util.Collection;
-import java.util.List;
-
 import org.ejml.data.DMatrix;
 import org.ejml.data.DMatrixRMaj;
-
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.mecano.frames.MovingReferenceFrame;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
-import us.ihmc.mecano.spatial.Momentum;
-import us.ihmc.mecano.spatial.SpatialAcceleration;
-import us.ihmc.mecano.spatial.SpatialForce;
-import us.ihmc.mecano.spatial.SpatialInertia;
-import us.ihmc.mecano.spatial.Twist;
-import us.ihmc.mecano.spatial.Wrench;
+import us.ihmc.mecano.spatial.*;
 import us.ihmc.mecano.spatial.interfaces.SpatialForceReadOnly;
 import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
 import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
 import us.ihmc.mecano.tools.MultiBodySystemTools;
 
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.List;
+
 /**
  * This calculator computes:
  * <ul>
@@ -242,22 +236,7 @@ private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyIne
    {
       List<CompositeRigidBodyInertia> inertiaList = new ArrayList<>();
 
-      List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());
-
-      if (childrenJoints.size() > 1)
-      { // Reorganize the joints in the children to ensure that loop closures are treated last.
-         List<JointReadOnly> loopClosureAncestors = new ArrayList<>();
-
-         for (int i = 0; i < childrenJoints.size();)
-         {
-            if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
-               loopClosureAncestors.add(childrenJoints.remove(i));
-            else
-               i++;
-         }
-
-         childrenJoints.addAll(loopClosureAncestors);
-      }
+      List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);
 
       for (JointReadOnly childJoint : childrenJoints)
       {
@@ -292,7 +271,7 @@ private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyIne
     * Note that enabling the calculation of the Coriolis and centrifugal matrix will increase the
     * computational load when updating the mass matrix.
     * </p>
-    * 
+    *
     * @param enableCoriolisMatrixCalculation whether to enable or disable the calculation of the
     *                                        Coriolis and centrifugal matrix. Disabled by default.
     */
@@ -370,10 +349,10 @@ public DMatrixRMaj getMassMatrix()
 
    /**
     * Gets the Coriolis and centrifugal matrix for this multi-body system.
-    * 
+    *
     * @return the Coriolis and centrifugal matrix.
     * @throws UnsupportedOperationException if the calculation of the Coriolis and centrifugal matrix
-    *                                       was not enabled.
+    *       was not enabled.
     * @see #setEnableCoriolisMatrixCalculation(boolean)
     */
    public DMatrixRMaj getCoriolisMatrix()