Merge branch 'release/0.0.6'

build.gradle

@@ -11,7 +11,7 @@ apply plugin: "us.ihmc.ihmc-build"
 
 ihmc {
    group = "us.ihmc"
-   version = "0.0.5"
+   version = "0.0.6"
    vcsUrl = "https://github.com/ihmcrobotics/mecano"
    openSource = true
 

src/graphviz/java/us/ihmc/mecano/graphviz/MultiBodySystemViewer.java

@@ -1,6 +1,7 @@
 package us.ihmc.mecano.graphviz;
 
-import static guru.nidi.graphviz.model.Factory.*;
+import static guru.nidi.graphviz.model.Factory.mutGraph;
+import static guru.nidi.graphviz.model.Factory.mutNode;
 
 import java.io.File;
 import java.io.IOException;
@@ -17,6 +18,7 @@
 import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly;
 import us.ihmc.euclid.referenceFrame.FramePoint3D;
 import us.ihmc.euclid.tools.EuclidCoreIOTools;
+import us.ihmc.mecano.multiBodySystem.interfaces.FixedJointReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
 import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointReadOnly;
@@ -313,6 +315,8 @@ else if (joint instanceof PlanarJointReadOnly)
          return getPlanarJointStateAsString((PlanarJointReadOnly) joint, state);
       else if (joint instanceof SphericalJointReadOnly)
          return getSphericalJointStateAsString((SphericalJointReadOnly) joint, state);
+      else if (joint instanceof FixedJointReadOnly)
+         return getFixedJointStateAsString((FixedJointReadOnly) joint);
       else
          throw new UnsupportedOperationException("Unsupported joint type: " + joint.getClass().getName());
    }
@@ -416,6 +420,11 @@ private static String getOneDoFJointStateAsString(OneDoFJointReadOnly joint, Joi
       }
    }
 
+   private static String getFixedJointStateAsString(FixedJointReadOnly joint)
+   {
+      return null;
+   }
+
    private static String getMatrixLabel(Matrix3DReadOnly matrix)
    {
       String ret = EuclidCoreIOTools.getStringOf("/", " \\\\\n", ", ", matrix.getM00(), matrix.getM01(), matrix.getM02());

src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java

@@ -528,7 +528,7 @@ public void passThree(double inverseOfTotalMass)
 
       public boolean isRoot()
       {
-         return rigidBody.isRootBody();
+         return jointIndices == null;
       }
 
       public JointReadOnly getJoint()

src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java

@@ -468,7 +468,7 @@ private void addToUnitMomentumRecursively(TwistReadOnly ancestorUnitTwist, Fixed
 
       public boolean isRoot()
       {
-         return rigidBody.isRootBody();
+         return jointIndices == null;
       }
 
       public JointReadOnly getJoint()

src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java

@@ -673,7 +673,7 @@ private void addToUnitMomentumRecursively(TwistReadOnly ancestorUnitTwist, Fixed
 
       public boolean isRoot()
       {
-         return rigidBody.isRootBody();
+         return parent == null;
       }
 
       public JointReadOnly getJoint()

src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java

@@ -509,7 +509,7 @@ public void setSymmetricEntry(int row, int col, DenseMatrix64F symmetricMatrix,
 
       private boolean isRoot()
       {
-         return rigidBody.isRootBody();
+         return parent == null;
       }
 
       public int getNumberOfDoFs()

src/main/java/us/ihmc/mecano/algorithms/ForwardDynamicsCalculator.java

@@ -857,7 +857,7 @@ public void setExternalWrenchToZeroRecursive()
 
       public boolean isRoot()
       {
-         return rigidBody.isRootBody();
+         return parent == null;
       }
 
       public MovingReferenceFrame getBodyFixedFrame()

src/main/java/us/ihmc/mecano/tools/MultiBodySystemFactories.java

@@ -101,9 +101,9 @@ public static OneDoFJointBasics[] cloneOneDoFJointKinematicChain(RigidBodyBasics
     * @param originalJoints the kinematic chain to clone. Not modified.
     * @return the clone kinematic chain.
     */
-   public static OneDoFJoint[] cloneOneDoFJointKinematicChain(OneDoFJoint[] originalJoints)
+   public static OneDoFJointBasics[] cloneOneDoFJointKinematicChain(OneDoFJointBasics[] originalJoints)
    {
-      return cloneKinematicChainAndFilter(originalJoints, OneDoFJoint.class);
+      return cloneKinematicChainAndFilter(originalJoints, OneDoFJointBasics.class);
    }
 
    /**

src/main/java/us/ihmc/mecano/tools/MultiBodySystemRandomTools.java

@@ -123,6 +123,47 @@ public static void nextState(Random random, JointStateType stateToRandomize, dou
       }
    }
 
+   /**
+    * Generates a random state and update the given {@code joints} with it.
+    * <p>
+    * The random state is guaranteed to be within the joint limits. For instance, a random
+    * configuration is constrained to be in: [{@code joint.getJointLimitLower()},
+    * {@code joint.getJointLimitUpper()}].
+    * </p>
+    * 
+    * @param random the random generator to use.
+    * @param stateToRandomize the joint state that is to be randomized. As no limits are imposed on the
+    *           joint accelerations, the state to randomize cannot be the acceleration. For generating
+    *           random acceleration, please see
+    *           {@link #nextState(Random, JointStateType, double, double, OneDoFJointBasics)}.
+    * @param joints the joints to set the state of. Modified.
+    */
+   public static void nextStateWithinJointLimits(Random random, JointStateType stateToRandomize, OneDoFJointBasics[] joints)
+   {
+      for (OneDoFJointBasics joint : joints)
+         nextStateWithinJointLimits(random, stateToRandomize, joint);
+   }
+
+   /**
+    * Generates a random state and update the given {@code joints} with it.
+    * <p>
+    * The random state is guaranteed to be within the joint limits. For instance, a random
+    * configuration is constrained to be in: [{@code joint.getJointLimitLower()},
+    * {@code joint.getJointLimitUpper()}].
+    * </p>
+    * 
+    * @param random the random generator to use.
+    * @param stateToRandomize the joint state that is to be randomized. As no limits are imposed on the
+    *           joint accelerations, the state to randomize cannot be the acceleration. For generating
+    *           random acceleration, please see
+    *           {@link #nextState(Random, JointStateType, double, double, OneDoFJointBasics)}.
+    * @param joints the joints to set the state of. Modified.
+    */
+   public static void nextStateWithinJointLimits(Random random, JointStateType stateToRandomize, Iterable<? extends OneDoFJointBasics> joints)
+   {
+      joints.forEach(joint -> nextStateWithinJointLimits(random, stateToRandomize, joint));
+   }
+
    /**
     * Generates a random state and update the given {@code joint} with it.
     * <p>

src/test/java/us/ihmc/mecano/algorithms/SpatialAccelerationCalculatorTest.java

@@ -1,6 +1,7 @@
 package us.ihmc.mecano.algorithms;
 
-import static org.junit.Assert.*;
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
 
 import java.util.Arrays;
 import java.util.List;
@@ -26,6 +27,7 @@
 import us.ihmc.mecano.multiBodySystem.RevoluteJoint;
 import us.ihmc.mecano.multiBodySystem.SixDoFJoint;
 import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics;
+import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
 import us.ihmc.mecano.spatial.SpatialAcceleration;
@@ -227,7 +229,7 @@ public void testWithChainRobotAgainstFiniteDifference() throws Exception
 
       int numberOfJoints = 10;
       List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints);
-      List<OneDoFJoint> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJoint[numberOfJoints])));
+      List<OneDoFJointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJoint[numberOfJoints])));
 
       RigidBodyBasics randomBody = joints.get(random.nextInt(joints.size())).getPredecessor();
       RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(randomBody);