Merge branch 'release/0.7.2'

ihmcrobotics · Dec 2, 2020 · 01ef71c · 01ef71c
2 parents fedfca3 + 92b681a
commit 01ef71c
Show file tree

Hide file tree

Showing 10 changed files with 143 additions and 17 deletions.
diff --git a/build.gradle.kts b/build.gradle.kts
@@ -1,12 +1,12 @@
 plugins {
-   id("us.ihmc.ihmc-build") version "0.21.0"
-   id("us.ihmc.ihmc-ci") version "5.9"
-   id("us.ihmc.ihmc-cd") version "1.14"
+   id("us.ihmc.ihmc-build")
+   id("us.ihmc.ihmc-ci") version "7.4"
+   id("us.ihmc.ihmc-cd") version "1.17"
 }
 
 ihmc {
    group = "us.ihmc"
-   version = "0.7.1"
+   version = "0.7.2"
    vcsUrl = "https://github.com/ihmcrobotics/mecano"
    openSource = true
 
@@ -18,9 +18,9 @@ mainDependencies {
    api("org.ejml:ejml-core:0.39")
    api("org.ejml:ejml-ddense:0.39")
 
-   api("us.ihmc:euclid:0.15.0")
-   api("us.ihmc:euclid-frame:0.15.0")
-   api("us.ihmc:euclid-geometry:0.15.0")
+   api("us.ihmc:euclid:0.15.2")
+   api("us.ihmc:euclid-frame:0.15.2")
+   api("us.ihmc:euclid-geometry:0.15.2")
 }
 
 testDependencies {
@@ -38,5 +38,5 @@ graphvizDependencies {
 yovariablesDependencies {
    api(ihmc.sourceSetProject("main"))
 
-   api("us.ihmc:ihmc-yovariables:0.9.1")
+   api("us.ihmc:ihmc-yovariables:0.9.7")
 }
diff --git a/settings.gradle.kts b/settings.gradle.kts
@@ -1,10 +1,16 @@
+pluginManagement {
+   plugins {
+      id("us.ihmc.ihmc-build") version "0.23.0"
+   }
+}
+
 buildscript {
    repositories {
       maven { url = uri("https://plugins.gradle.org/m2/") }
       mavenLocal()
    }
    dependencies {
-      classpath("us.ihmc:ihmc-build:0.21.0")
+      classpath("us.ihmc:ihmc-build:0.23.0")
    }
 }
 

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java b/src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java
@@ -231,7 +231,24 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
       List<RecursionStep> recursionSteps = new ArrayList<>();
       recursionSteps.add(parent);
 
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java
@@ -156,7 +156,24 @@ private List<IterativeStep> buildMultiBodyTree(IterativeStep parent, Collection<
       List<IterativeStep> iterativeSteps = new ArrayList<>();
       iterativeSteps.add(parent);
 
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java
@@ -217,7 +217,24 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
       List<RecursionStep> recursionSteps = new ArrayList<>();
       recursionSteps.add(parent);
 
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java
@@ -217,7 +217,24 @@ private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyIne
    {
       List<CompositeRigidBodyInertia> inertiaList = new ArrayList<>();
 
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/algorithms/ForwardDynamicsCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/ForwardDynamicsCalculator.java
@@ -177,7 +177,24 @@ public ForwardDynamicsCalculator(MultiBodySystemReadOnly input, boolean consider
 
    private void buildMultiBodyTree(ArticulatedBodyRecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore)
    {
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/algorithms/InverseDynamicsCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/InverseDynamicsCalculator.java
@@ -262,7 +262,24 @@ public InverseDynamicsCalculator(MultiBodySystemReadOnly input, boolean consider
 
    private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore)
    {
-      for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints())
+      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);
+      }
+
+      for (JointReadOnly childJoint : childrenJoints)
       {
          if (jointsToIgnore.contains(childJoint))
             continue;

diff --git a/src/main/java/us/ihmc/mecano/frames/MovingReferenceFrame.java b/src/main/java/us/ihmc/mecano/frames/MovingReferenceFrame.java
@@ -9,6 +9,7 @@
 import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
 import us.ihmc.mecano.exceptions.ScrewTheoryException;
 import us.ihmc.mecano.spatial.Twist;
+import us.ihmc.mecano.spatial.interfaces.TwistBasics;
 import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
 
 /**
@@ -343,7 +344,7 @@ public TwistReadOnly getTwistOfFrame()
     *
     * @param twistToPack the twist in which the absolute velocity of this frame is stored. Modified.
     */
-   public void getTwistOfFrame(Twist twistToPack)
+   public void getTwistOfFrame(TwistBasics twistToPack)
    {
       twistToPack.setIncludingFrame(getTwistOfFrame());
    }
@@ -362,7 +363,7 @@ public void getTwistOfFrame(Twist twistToPack)
     * @param base                the frame with respect to which the twist is to be computed.
     * @param relativeTwistToPack the twist of {@code this} with respect to {@code base}. Modified.
     */
-   public void getTwistRelativeToOther(ReferenceFrame base, Twist relativeTwistToPack)
+   public void getTwistRelativeToOther(ReferenceFrame base, TwistBasics relativeTwistToPack)
    {
       verifySameRoots(base);
 

diff --git a/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java b/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java
@@ -1124,6 +1124,23 @@ public static boolean areJointsInContinuousOrder(List<? extends JointReadOnly> j
       return true;
    }
 
+   public static boolean doesSubtreeContainLoopClosure(RigidBodyReadOnly start)
+   {
+      for (int i = 0; i < start.getChildrenJoints().size(); i++)
+      {
+         if (start.getChildrenJoints().get(i).isLoopClosure())
+            return true;
+      }
+
+      for (int i = 0; i < start.getChildrenJoints().size(); i++)
+      {
+         if (doesSubtreeContainLoopClosure(start.getChildrenJoints().get(i).getSuccessor()))
+            return true;
+      }
+
+      return false;
+   }
+
    /**
     * Iterates through the given array and compute how many do implement the given {@code clazz}.
     *