Merge branch 'release/0.7.0'

build.gradle.kts

@@ -1,12 +1,12 @@
 plugins {
-   id("us.ihmc.ihmc-build") version "0.20.2"
+   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"
 }
 
 ihmc {
    group = "us.ihmc"
-   version = "0.6.0"
+   version = "0.7.0"
    vcsUrl = "https://github.com/ihmcrobotics/mecano"
    openSource = true
 
@@ -38,5 +38,5 @@ graphvizDependencies {
 yovariablesDependencies {
    api(ihmc.sourceSetProject("main"))
 
-   api("us.ihmc:ihmc-yovariables:0.8.0")
+   api("us.ihmc:ihmc-yovariables:0.9.0")
 }

settings.gradle.kts

@@ -4,7 +4,7 @@ buildscript {
       mavenLocal()
    }
    dependencies {
-      classpath("us.ihmc:ihmc-build:0.20.2")
+      classpath("us.ihmc:ihmc-build:0.21.0")
    }
 }
 

src/main/java/us/ihmc/mecano/spatial/interfaces/FixedFrameSpatialVectorBasics.java

@@ -450,6 +450,24 @@ default void add(SpatialVectorReadOnly other)
       add((Vector3DReadOnly) other.getAngularPart(), (Vector3DReadOnly) other.getLinearPart());
    }
 
+   /**
+    * Sets this vector to the sum of the two given vectors.
+    * <p>
+    * {@code this = vector1 + vector2}
+    * </p>
+    * 
+    * @param vector1 the first vector to sum. Not modified.
+    * @param vector2 the second vector to sum. Not modified.
+    * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same
+    *                                         reference frame as {@code this}.
+    */
+   default void add(SpatialVectorReadOnly vector1, SpatialVectorReadOnly vector2)
+   {
+      checkReferenceFrameMatch(vector1, vector2);
+      getAngularPart().add((Tuple3DReadOnly) vector1.getAngularPart(), (Tuple3DReadOnly) vector2.getAngularPart());
+      getLinearPart().add((Tuple3DReadOnly) vector1.getLinearPart(), (Tuple3DReadOnly) vector2.getLinearPart());
+   }
+
    /**
     * Adds to this vector's components the given column vector starting to read from its first row
     * index.
@@ -552,6 +570,24 @@ default void sub(SpatialVectorReadOnly other)
       sub((Vector3DReadOnly) other.getAngularPart(), (Vector3DReadOnly) other.getLinearPart());
    }
 
+   /**
+    * Sets this vector to the difference of the two given vectors.
+    * <p>
+    * {@code this = vector1 - vector2}
+    * </p>
+    * 
+    * @param vector1 the first vector. Not modified.
+    * @param vector2 the second vector to subtract to {@code vector2}. Not modified.
+    * @throws ReferenceFrameMismatchException if any of the arguments is not expressed in the same
+    *                                         reference frame as {@code this}.
+    */
+   default void sub(SpatialVectorReadOnly vector1, SpatialVectorReadOnly vector2)
+   {
+      checkReferenceFrameMatch(vector1, vector2);
+      getAngularPart().sub((Tuple3DReadOnly) vector1.getAngularPart(), (Tuple3DReadOnly) vector2.getAngularPart());
+      getLinearPart().sub((Tuple3DReadOnly) vector1.getLinearPart(), (Tuple3DReadOnly) vector2.getLinearPart());
+   }
+
    /**
     * Subtracts from this vector's components the given column vector starting to read from its first
     * row index.
@@ -639,6 +675,47 @@ default void sub(FrameVector3DReadOnly angular, FrameVector3DReadOnly linear)
       getLinearPart().sub(linear);
    }
 
+   /**
+    * Performs a linear interpolation from this vector to {@code other} given the percentage
+    * {@code alpha}.
+    * <p>
+    * this = (1.0 - alpha) * this + alpha * other
+    * </p>
+    *
+    * @param other the other vector used for the interpolation. Not modified.
+    * @param alpha the percentage used for the interpolation. A value of 0 will result in not modifying
+    *              this vector, while a value of 1 is equivalent to setting this vector to
+    *              {@code other}.
+    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference
+    *                                         frame as {@code this}.
+    */
+   default void interpolate(SpatialVectorReadOnly other, double alpha)
+   {
+      interpolate(this, other, alpha);
+   }
+
+   /**
+    * Performs a linear interpolation from {@code vector1} to {@code vector2} given the percentage
+    * {@code alpha}.
+    * <p>
+    * this = (1.0 - alpha) * vector1 + alpha * vector2
+    * </p>
+    *
+    * @param vector1 the first vector used in the interpolation. Not modified.
+    * @param vector2 the second vector used in the interpolation. Not modified.
+    * @param alpha   the percentage to use for the interpolation. A value of 0 will result in setting
+    *                this vector to {@code vector1}, while a value of 1 is equivalent to setting this
+    *                vector to {@code vector2}.
+    * @throws ReferenceFrameMismatchException if any of the arguments are not expressed in the same
+    *                                         reference frame as {@code this}.
+    */
+   default void interpolate(SpatialVectorReadOnly vector1, SpatialVectorReadOnly vector2, double alpha)
+   {
+      checkReferenceFrameMatch(vector1, vector2);
+      getAngularPart().interpolate((Tuple3DReadOnly) vector1.getAngularPart(), (Tuple3DReadOnly) vector2.getAngularPart(), alpha);
+      getLinearPart().interpolate((Tuple3DReadOnly) vector1.getLinearPart(), (Tuple3DReadOnly) vector2.getLinearPart(), alpha);
+   }
+
    /**
     * Calculates the cross product of {@code tuple1} and {@code tuple2} and adds the result to this
     * vector's angular part.

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoMultiBodySystem.java

@@ -16,7 +16,7 @@
 import us.ihmc.mecano.tools.MultiBodySystemFactories.JointBuilder;
 import us.ihmc.mecano.tools.MultiBodySystemFactories.RigidBodyBuilder;
 import us.ihmc.mecano.yoVariables.tools.YoMultiBodySystemFactories;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
+import us.ihmc.yoVariables.registry.YoRegistry;
 
 /**
  * A {@code YoMultiBodySystem} can be created from any {@code MultiBodySystemReadOnly}.
@@ -44,7 +44,7 @@ public class YoMultiBodySystem implements MultiBodySystemBasics
     * @param stationaryFrame the root frame to which yo-multi-body system is attached.
     * @param registry        the registry to register child variables to.
     */
-   public YoMultiBodySystem(MultiBodySystemReadOnly input, ReferenceFrame stationaryFrame, YoVariableRegistry registry)
+   public YoMultiBodySystem(MultiBodySystemReadOnly input, ReferenceFrame stationaryFrame, YoRegistry registry)
    {
       this(input, stationaryFrame, registry, MultiBodySystemFactories.DEFAULT_RIGID_BODY_BUILDER, YoMultiBodySystemFactories.newYoJointBuilder(registry));
    }
@@ -59,9 +59,9 @@ public YoMultiBodySystem(MultiBodySystemReadOnly input, ReferenceFrame stationar
     * @param rigidBodyBuilder use a custom rigid-body builder. Can be {@code null}, the default builder
     *                         is {@link MultiBodySystemFactories#DEFAULT_RIGID_BODY_BUILDER}.
     * @param yoJointBuilder   use a custom yo-joint builder, the default used is
-    *                         {@link YoMultiBodySystemFactories#newYoJointBuilder(YoVariableRegistry)}.
+    *                         {@link YoMultiBodySystemFactories#newYoJointBuilder(YoRegistry)}.
     */
-   public YoMultiBodySystem(MultiBodySystemReadOnly input, ReferenceFrame stationaryFrame, YoVariableRegistry registry, RigidBodyBuilder rigidBodyBuilder,
+   public YoMultiBodySystem(MultiBodySystemReadOnly input, ReferenceFrame stationaryFrame, YoRegistry registry, RigidBodyBuilder rigidBodyBuilder,
                             JointBuilder yoJointBuilder)
    {
       yoRootBody = MultiBodySystemFactories.cloneMultiBodySystem(input.getRootBody(), stationaryFrame, "", rigidBodyBuilder, yoJointBuilder);

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoOneDoFJoint.java

@@ -4,7 +4,7 @@
 import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
 import us.ihmc.mecano.multiBodySystem.OneDoFJoint;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
+import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 
 /**
@@ -34,7 +34,7 @@ public abstract class YoOneDoFJoint extends OneDoFJoint
     * @param registry             the registry to register child variables to.
     */
    public YoOneDoFJoint(String name, RigidBodyBasics predecessor, Vector3DReadOnly jointAxisAngularPart, Vector3DReadOnly jointAxisLinearPart,
-                        RigidBodyTransform transformToParent, YoVariableRegistry registry)
+                        RigidBodyTransform transformToParent, YoRegistry registry)
    {
       super(name, predecessor, jointAxisAngularPart, jointAxisLinearPart, transformToParent);
 

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoPlanarJoint.java

@@ -14,7 +14,7 @@
 import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
 import us.ihmc.mecano.tools.MecanoTools;
 import us.ihmc.mecano.yoVariables.tools.YoMecanoFactories;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
+import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 
 /**
@@ -26,7 +26,7 @@
 public class YoPlanarJoint extends Joint implements PlanarJointBasics
 {
    /** Registry in which the {@code YoVariable}s created for this joint are attached to. */
-   private final YoVariableRegistry registry;
+   private final YoRegistry registry;
    /** The 3D pose of this joint restricted in the XZ-plane. */
    private final Pose3DBasics jointPose;
    /** The twist of this joint restricted in the XZ-plane. */
@@ -51,7 +51,7 @@ public class YoPlanarJoint extends Joint implements PlanarJointBasics
     * @param predecessor the rigid-body connected to and preceding this joint.
     * @param registry    the registry to register child variables to.
     */
-   public YoPlanarJoint(String name, RigidBodyBasics predecessor, YoVariableRegistry registry)
+   public YoPlanarJoint(String name, RigidBodyBasics predecessor, YoRegistry registry)
    {
       this(name, predecessor, null, registry);
    }
@@ -64,7 +64,7 @@ public YoPlanarJoint(String name, RigidBodyBasics predecessor, YoVariableRegistr
     * @param transformToParent the transform to the frame after the parent joint. Not modified.
     * @param registry          the registry to register child variables to.
     */
-   public YoPlanarJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoVariableRegistry registry)
+   public YoPlanarJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoRegistry registry)
    {
       super(name, predecessor, transformToParent);
       this.registry = registry;

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoPrismaticJoint.java

@@ -9,7 +9,7 @@
 import us.ihmc.euclid.tuple4D.Quaternion;
 import us.ihmc.mecano.multiBodySystem.interfaces.PrismaticJointBasics;
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
+import us.ihmc.yoVariables.registry.YoRegistry;
 
 /**
  * A {@code YoPrismaticJoint} is a joint has 1 degree of freedom of translation and has its state
@@ -33,7 +33,7 @@ public class YoPrismaticJoint extends YoOneDoFJoint implements PrismaticJointBas
     * @param jointAxis   the axis along which this joint can translate. Not modified.
     * @param registry    the registry to register child variables to.
     */
-   public YoPrismaticJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, Vector3DReadOnly jointAxis, YoVariableRegistry registry)
+   public YoPrismaticJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, Vector3DReadOnly jointAxis, YoRegistry registry)
    {
       this(name, predecessor, new RigidBodyTransform(new Quaternion(), jointOffset), jointAxis, registry);
    }
@@ -48,7 +48,7 @@ public YoPrismaticJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnl
     * @param registry          the registry to register child variables to.
     */
    public YoPrismaticJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, Vector3DReadOnly jointAxis,
-                           YoVariableRegistry registry)
+                           YoRegistry registry)
    {
       super(name, predecessor, new Vector3D(), jointAxis, transformToParent, registry);
       this.jointAxis = new FrameVector3D(beforeJointFrame, jointAxis);

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoRevoluteJoint.java

@@ -12,7 +12,7 @@
 import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
 import us.ihmc.mecano.tools.MecanoFactories;
 import us.ihmc.mecano.tools.MecanoFactories.RevoluteJointTransformUpdater;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
+import us.ihmc.yoVariables.registry.YoRegistry;
 
 /**
  * A {@code YoRevoluteJoint} is a joint has 1 degree of freedom of rotation and has its state backed
@@ -40,7 +40,7 @@ public class YoRevoluteJoint extends YoOneDoFJoint implements RevoluteJointBasic
     * @param jointAxis   the axis around which this joint can rotate. Not modified.
     * @param registry    the registry to register child variables to.
     */
-   public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Vector3DReadOnly jointAxis, YoVariableRegistry registry)
+   public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Vector3DReadOnly jointAxis, YoRegistry registry)
    {
       this(name, predecessor, (RigidBodyTransform) null, jointAxis, registry);
    }
@@ -56,7 +56,7 @@ public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Vector3DReadOnl
     * @param jointAxis   the axis around which this joint can rotate. Not modified.
     * @param registry    the registry to register child variables to.
     */
-   public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, Vector3DReadOnly jointAxis, YoVariableRegistry registry)
+   public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, Vector3DReadOnly jointAxis, YoRegistry registry)
    {
       this(name, predecessor, new RigidBodyTransform(new Quaternion(), jointOffset), jointAxis, registry);
    }
@@ -71,7 +71,7 @@ public YoRevoluteJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly
     * @param registry          the registry to register child variables to.
     */
    public YoRevoluteJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, Vector3DReadOnly jointAxis,
-                          YoVariableRegistry registry)
+                          YoRegistry registry)
    {
       super(name, predecessor, jointAxis, new Vector3D(), transformToParent, registry);
       this.jointAxis = new FrameVector3D(beforeJointFrame, jointAxis);

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoSixDoFJoint.java

@@ -12,8 +12,8 @@
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialAcceleration;
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameTwist;
 import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameWrench;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
-import us.ihmc.yoVariables.variable.YoFramePose3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
 
 /**
  * A {@code SixDoFJoint} can be used to represent a 3D floating joint and has its state backed by
@@ -24,7 +24,7 @@
 public class YoSixDoFJoint extends Joint implements SixDoFJointBasics
 {
    /** Registry in which the {@code YoVariable}s created for this joint are attached to. */
-   private final YoVariableRegistry registry;
+   private final YoRegistry registry;
 
    /** The 3D pose of this joint. */
    private final YoFramePose3D jointPose;
@@ -51,7 +51,7 @@ public class YoSixDoFJoint extends Joint implements SixDoFJointBasics
     * @param predecessor the rigid-body connected to and preceding this joint.
     * @param registry    the registry to register child variables to.
     */
-   public YoSixDoFJoint(String name, RigidBodyBasics predecessor, YoVariableRegistry registry)
+   public YoSixDoFJoint(String name, RigidBodyBasics predecessor, YoRegistry registry)
    {
       this(name, predecessor, null, registry);
    }
@@ -64,7 +64,7 @@ public YoSixDoFJoint(String name, RigidBodyBasics predecessor, YoVariableRegistr
     * @param transformToParent the transform to the frame after the parent joint. Not modified.
     * @param registry          the registry to register child variables to.
     */
-   public YoSixDoFJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoVariableRegistry registry)
+   public YoSixDoFJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoRegistry registry)
    {
       super(name, predecessor, transformToParent);
       this.registry = registry;

src/yovariables/java/us/ihmc/mecano/yoVariables/multiBodySystem/YoSphericalJoint.java

@@ -18,9 +18,9 @@
 import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly;
 import us.ihmc.mecano.tools.MecanoFactories;
 import us.ihmc.mecano.tools.MecanoTools;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
-import us.ihmc.yoVariables.variable.YoFrameQuaternion;
-import us.ihmc.yoVariables.variable.YoFrameVector3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
 
 /**
  * A {@code YoSphericalJoint} has 3 degrees of freedom of rotation and has its state backed by
@@ -69,7 +69,7 @@ public class YoSphericalJoint extends Joint implements SphericalJointBasics
     *                    Not modified.
     * @param registry    the registry to register child variables to.
     */
-   public YoSphericalJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, YoVariableRegistry registry)
+   public YoSphericalJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset, YoRegistry registry)
    {
       this(name, predecessor, new RigidBodyTransform(new Quaternion(), jointOffset), registry);
    }
@@ -82,7 +82,7 @@ public YoSphericalJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnl
     * @param transformToParent the transform to the frame after the parent joint. Not modified.
     * @param registry          the registry to register child variables to.
     */
-   public YoSphericalJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoVariableRegistry registry)
+   public YoSphericalJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent, YoRegistry registry)
    {
       super(name, predecessor, transformToParent);
       jointOrientation = new YoFrameQuaternion(name, beforeJointFrame, registry);

src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameSpatialAcceleration.java

@@ -11,8 +11,8 @@
 import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly;
 import us.ihmc.mecano.spatial.interfaces.SpatialVectorReadOnly;
 import us.ihmc.mecano.tools.MecanoIOTools;
-import us.ihmc.yoVariables.registry.YoVariableRegistry;
-import us.ihmc.yoVariables.variable.YoFrameVector3D;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoVariable;
 
 /**
@@ -84,7 +84,7 @@ public class YoFixedFrameSpatialAcceleration implements FixedFrameSpatialAcceler
     * @param registry         the registry to register child variables to.
     */
    public YoFixedFrameSpatialAcceleration(String namePrefix, ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame,
-                                          YoVariableRegistry registry)
+                                          YoRegistry registry)
    {
       this(bodyFrame, baseFrame, new YoFixedFrameSpatialVector(namePrefix, expressedInFrame, registry));
    }
@@ -104,7 +104,7 @@ public YoFixedFrameSpatialAcceleration(String namePrefix, ReferenceFrame bodyFra
     * @param registry         the registry to register child variables to.
     */
    public YoFixedFrameSpatialAcceleration(String namePrefix, String nameSuffix, ReferenceFrame bodyFrame, ReferenceFrame baseFrame,
-                                          ReferenceFrame expressedInFrame, YoVariableRegistry registry)
+                                          ReferenceFrame expressedInFrame, YoRegistry registry)
    {
       this(bodyFrame, baseFrame, new YoFixedFrameSpatialVector(namePrefix, nameSuffix, expressedInFrame, registry));
    }