incorporated all the code from open robotics

ihmcrobotics · Oct 28, 2024 · b3a1b17 · b3a1b17
1 parent 20ef706
commit b3a1b17
Show file tree

Hide file tree

Showing 26 changed files with 49 additions and 131 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,4 +1,9 @@
+# Top level files to ignore
+gradlew
+gradlew.bat
+
 # Top level directories to ignore
+/gradle
 /build
 /classes
 /bin

diff --git a/build.gradle.kts b/build.gradle.kts
@@ -47,5 +47,11 @@ graphvizDependencies {
 yovariablesDependencies {
    api(ihmc.sourceSetProject("main"))
 
-   api("us.ihmc:ihmc-yovariables:0.12.2")
+   api("us.ihmc:ihmc-yovariables:0.13.1")
+}
+
+yovariablesFiltersDependencies {
+   api(ihmc.sourceSetProject("yovariables"))
+
+   api("us.ihmc:ihmc-yovariables-filters:0.13.1")
 }
diff --git a/gradle.properties b/gradle.properties
@@ -1,4 +1,4 @@
 title = Mecano
-extraSourceSets = ["test", "graphviz", "yovariables"]
+extraSourceSets = ["test", "graphviz", "yovariables", "yovariables-filters"]
 compositeSearchHeight = 0
 excludeFromCompositeBuild = false
diff --git a/mecano-update/build.gradle.kts b/mecano-update/build.gradle.kts
diff --git a/mecano-update/gradle.properties b/mecano-update/gradle.properties
diff --git a/mecano-update/settings.gradle.kts b/mecano-update/settings.gradle.kts
diff --git a/mecano-update/src/main/java/us/ihmc/mecano/MultiBodySystemMissingTools.java b/mecano-update/src/main/java/us/ihmc/mecano/MultiBodySystemMissingTools.java
diff --git a/...o/DifferentialIDMassMatrixCalculator.java → ...s/DifferentialIDMassMatrixCalculator.java b/...o/DifferentialIDMassMatrixCalculator.java → ...s/DifferentialIDMassMatrixCalculator.java
@@ -1,12 +1,12 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.LinkedHashMap;
 
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
-import us.ihmc.mecano.algorithms.InverseDynamicsCalculator;
+import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.mecano.spatial.SpatialForce;

diff --git a/...micallyConsistentNullspaceCalculator.java → ...micallyConsistentNullspaceCalculator.java b/...micallyConsistentNullspaceCalculator.java → ...micallyConsistentNullspaceCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.ArrayList;
 import java.util.Arrays;
@@ -13,7 +13,6 @@
 import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
 import org.ejml.interfaces.linsol.LinearSolverDense;
 
-import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator;
 import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;

diff --git a/...atingBaseRigidBodyDynamicsCalculator.java → ...atingBaseRigidBodyDynamicsCalculator.java b/...atingBaseRigidBodyDynamicsCalculator.java → ...atingBaseRigidBodyDynamicsCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;

diff --git a/...riolisExternalWrenchMatrixCalculator.java → ...riolisExternalWrenchMatrixCalculator.java b/...riolisExternalWrenchMatrixCalculator.java → ...riolisExternalWrenchMatrixCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.ArrayList;
 import java.util.Collection;
@@ -12,8 +12,6 @@
 import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
 import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly;
 import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
-import us.ihmc.mecano.algorithms.InverseDynamicsCalculator;
-import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator;
 import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider;
 import us.ihmc.mecano.frames.MovingReferenceFrame;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;

diff --git a/.../mecano/MassMatrixCalculatorComparer.java → ...orithms/MassMatrixCalculatorComparer.java b/.../mecano/MassMatrixCalculatorComparer.java → ...orithms/MassMatrixCalculatorComparer.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.ArrayList;
 import java.util.Random;
@@ -7,7 +7,7 @@
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.tuple3D.Vector3D;
-import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator;
+import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator;
 import us.ihmc.mecano.multiBodySystem.RevoluteJoint;
 import us.ihmc.mecano.multiBodySystem.RigidBody;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;

diff --git a/...va/us/ihmc/mecano/MomentumCalculator.java → ...mecano/algorithms/MomentumCalculator.java b/...va/us/ihmc/mecano/MomentumCalculator.java → ...mecano/algorithms/MomentumCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.stream.Stream;
 

diff --git a/...ihmc/mecano/RigidBodyTwistCalculator.java → .../algorithms/RigidBodyTwistCalculator.java b/...ihmc/mecano/RigidBodyTwistCalculator.java → .../algorithms/RigidBodyTwistCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import org.ejml.data.DMatrix;
 import org.ejml.data.DMatrixRMaj;

diff --git a/.../us/ihmc/mecano/MassMatrixCalculator.java → ...thms/interfaces/MassMatrixCalculator.java b/.../us/ihmc/mecano/MassMatrixCalculator.java → ...thms/interfaces/MassMatrixCalculator.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms.interfaces;
 
 import org.ejml.data.DMatrixRMaj;
 

diff --git a/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java b/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java
@@ -5,6 +5,7 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.*;
 import us.ihmc.mecano.multiBodySystem.iterators.SubtreeStreams;
 import us.ihmc.mecano.spatial.SpatialInertia;
+import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
 
 import java.lang.reflect.Array;
 import java.util.ArrayList;
@@ -34,6 +35,24 @@ public static SpatialInertia computeSubtreeInertia(JointReadOnly joint)
       return computeSubtreeInertia(joint.getSuccessor());
    }
 
+   /**
+    * Computes the mass of all the links contained in the subtree of {@param rootBody}
+    * @param rootBody
+    * @return subtree mass
+    */
+   public static double computeSubTreeMass(RigidBodyReadOnly rootBody)
+   {
+      SpatialInertiaReadOnly inertia = rootBody.getInertia();
+      double ret = inertia == null ? 0.0 : inertia.getMass();
+
+      for (int i = 0; i < rootBody.getChildrenJoints().size(); i++)
+      {
+         ret += computeSubTreeMass(rootBody.getChildrenJoints().get(i).getSuccessor());
+      }
+
+      return ret;
+   }
+
    /**
     * Sums the inertia of all the rigid-bodies composing the subtree that originates at
     * {@code rootBody} including {@code rootBody}.
@@ -1736,4 +1755,5 @@ public static List<JointReadOnly> sortLoopClosureInChildrenJoints(RigidBodyReadO
       }
       return childrenJoints;
    }
+
 }
diff --git a/...fferentialIDMassMatrixCalculatorTest.java → ...fferentialIDMassMatrixCalculatorTest.java b/...fferentialIDMassMatrixCalculatorTest.java → ...fferentialIDMassMatrixCalculatorTest.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 

diff --git a/...isExternalWrenchMatrixCalculatorTest.java → ...isExternalWrenchMatrixCalculatorTest.java b/...isExternalWrenchMatrixCalculatorTest.java → ...isExternalWrenchMatrixCalculatorTest.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import static org.junit.jupiter.api.Assertions.assertNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -16,7 +16,6 @@
 
 import us.ihmc.euclid.tools.EuclidCoreIOTools;
 import us.ihmc.euclid.tools.EuclidCoreRandomTools;
-import us.ihmc.mecano.algorithms.InverseDynamicsCalculator;
 import us.ihmc.mecano.multiBodySystem.Joint;
 import us.ihmc.mecano.multiBodySystem.OneDoFJoint;
 import us.ihmc.mecano.multiBodySystem.PrismaticJoint;

diff --git a/...ihmc/mecano/MassMatrixCalculatorTest.java → .../algorithms/MassMatrixCalculatorTest.java b/...ihmc/mecano/MassMatrixCalculatorTest.java → .../algorithms/MassMatrixCalculatorTest.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 
 import java.util.ArrayList;
@@ -14,7 +14,7 @@
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
 import us.ihmc.euclid.tuple3D.Vector3D;
-import us.ihmc.mecano.algorithms.CompositeRigidBodyMassMatrixCalculator;
+import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator;
 import us.ihmc.mecano.multiBodySystem.RevoluteJoint;
 import us.ihmc.mecano.multiBodySystem.RigidBody;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;

diff --git a/...s/ihmc/mecano/MomentumCalculatorTest.java → ...no/algorithms/MomentumCalculatorTest.java b/...s/ihmc/mecano/MomentumCalculatorTest.java → ...no/algorithms/MomentumCalculatorTest.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import java.util.ArrayList;
 import java.util.Random;
@@ -15,7 +15,6 @@
 import us.ihmc.euclid.tools.EuclidCoreTestTools;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.euclid.tuple3D.Vector3D;
-import us.ihmc.mecano.algorithms.CentroidalMomentumCalculator;
 import us.ihmc.mecano.frames.CenterOfMassReferenceFrame;
 import us.ihmc.mecano.multiBodySystem.PrismaticJoint;
 import us.ihmc.mecano.multiBodySystem.RevoluteJoint;

diff --git a/.../mecano/RigidBodyTwistCalculatorTest.java → ...orithms/RigidBodyTwistCalculatorTest.java b/.../mecano/RigidBodyTwistCalculatorTest.java → ...orithms/RigidBodyTwistCalculatorTest.java
@@ -1,4 +1,4 @@
-package us.ihmc.mecano;
+package us.ihmc.mecano.algorithms;
 
 import org.ejml.data.DMatrixRMaj;
 import org.junit.jupiter.api.Test;

diff --git a/...aFilteredYoMutableFrameSpatialVector.java → ...aFilteredYoMutableFrameSpatialVector.java b/...aFilteredYoMutableFrameSpatialVector.java → ...aFilteredYoMutableFrameSpatialVector.java
diff --git a/...filters/AlphaFilteredYoSpatialVector.java → ...filters/AlphaFilteredYoSpatialVector.java b/...filters/AlphaFilteredYoSpatialVector.java → ...filters/AlphaFilteredYoSpatialVector.java
diff --git a/...rs/RateLimitedYoMutableSpatialVector.java → ...rs/RateLimitedYoMutableSpatialVector.java b/...rs/RateLimitedYoMutableSpatialVector.java → ...rs/RateLimitedYoMutableSpatialVector.java
diff --git a/...s/filters/RateLimitedYoSpatialVector.java → ...s/filters/RateLimitedYoSpatialVector.java b/...s/filters/RateLimitedYoSpatialVector.java → ...s/filters/RateLimitedYoSpatialVector.java
diff --git a/.../spatial/YoMutableFrameSpatialVector.java → .../spatial/YoMutableFrameSpatialVector.java b/.../spatial/YoMutableFrameSpatialVector.java → .../spatial/YoMutableFrameSpatialVector.java