Merge branch 'release/0.0.4'

ihmcrobotics · Nov 6, 2018 · 4647e1d · 4647e1d
2 parents 6dd5771 + 3c7f5a8
commit 4647e1d
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 30 deletions.
diff --git a/build.gradle b/build.gradle
@@ -11,7 +11,7 @@ apply plugin: "us.ihmc.ihmc-build"
 
 ihmc {
    group = "us.ihmc"
-   version = "0.0.3"
+   version = "0.0.4"
    vcsUrl = "https://github.com/ihmcrobotics/mecano"
    openSource = true
 

diff --git a/src/main/java/us/ihmc/mecano/spatial/interfaces/FixedFrameSpatialVectorBasics.java b/src/main/java/us/ihmc/mecano/spatial/interfaces/FixedFrameSpatialVectorBasics.java
@@ -151,13 +151,12 @@ default void setLinearPartZ(double z)
    }
 
    /**
-    * Selects a component of this spatial vector based on {@code index} and sets it to
-    * {@code value}.
+    * Selects a component of this spatial vector based on {@code index} and sets it to {@code value}.
     * <ul>
-    * <li>For an {@code index} &in; [0, 2], the corresponding components are {@code x}, {@code y},
-    * and {@code z} of this vector's angular part.
-    * <li>For an {@code index} &in; [3, 5], the corresponding components are {@code x}, {@code y},
-    * and {@code z} of this vector's linear part.
+    * <li>For an {@code index} &in; [0, 2], the corresponding components are {@code x}, {@code y}, and
+    * {@code z} of this vector's angular part.
+    * <li>For an {@code index} &in; [3, 5], the corresponding components are {@code x}, {@code y}, and
+    * {@code z} of this vector's linear part.
     * </ul>
     *
     * @param index the index of the component to set.
@@ -285,8 +284,8 @@ default void set(int startRow, DenseMatrix64F matrix)
    }
 
    /**
-    * Sets this vector's components from the given column vector starting to read from
-    * {@code startRow} at the column index {@code column}.
+    * Sets this vector's components from the given column vector starting to read from {@code startRow}
+    * at the column index {@code column}.
     * <p>
     * The components are read in the following order: {@code angularPartX}, {@code angularPartY},
     * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}.
@@ -319,8 +318,8 @@ default void set(Vector3DReadOnly angularPart, Vector3DReadOnly linearPart)
     * Sets this vector to {@code other}.
     *
     * @param other the other vector to copy. Not modified.
-    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same
-    *            reference frame as {@code this}.
+    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference
+    *            frame as {@code this}.
     */
    default void set(SpatialVectorReadOnly other)
    {
@@ -334,8 +333,8 @@ default void set(SpatialVectorReadOnly other)
     * {@link #set(SpatialVectorReadOnly)}.
     * </p>
     * <p>
-    * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set
-    * to {@code other} once transformed to be expressed in {@code this.getReferenceFrame()}.
+    * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set to
+    * {@code other} once transformed to be expressed in {@code this.getReferenceFrame()}.
     * </p>
     *
     * @param other the other vector to copy. Not modified.
@@ -347,6 +346,31 @@ default void setMatchingFrame(SpatialVectorReadOnly other)
       other.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this);
    }
 
+   /**
+    * Sets this vector given an angular part and linear part.
+    * <p>
+    * If the arguments are expressed in the frame as {@code this}, then this method is equivalent to
+    * {@link #set(SpatialVectorReadOnly)}.
+    * </p>
+    * <p>
+    * If the arguments are expressed in a different frame than {@code this}, then {@code this} is set
+    * to {@code angularPart} and {@code linearPart} once transformed to be expressed in
+    * {@code this.getReferenceFrame()}.
+    * </p>
+    *
+    * @param angularPart the vector holding the new values for the angular part. Not modified.
+    * @param linearPart the vector holding the new values for the linear part. Not modified.
+    * @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference
+    *            frame.
+    */
+   default void setMatchingFrame(FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linearPart)
+   {
+      angularPart.checkReferenceFrameMatch(linearPart);
+      getAngularPart().set((Vector3DReadOnly) angularPart);
+      getLinearPart().set((Vector3DReadOnly) linearPart);
+      angularPart.getReferenceFrame().transformFromThisToDesiredFrame(getReferenceFrame(), this);
+   }
+
    /**
     * Sets this vector given an angular part and linear part.
     *
@@ -367,8 +391,8 @@ default void set(FrameVector3DReadOnly angularPart, FrameVector3DReadOnly linear
     * @param expressedInFrame the reference frame in which the vectors are expressed.
     * @param angularPart the vector holding the new values for the angular part. Not modified.
     * @param linearPart the vector holding the new values for the linear part. Not modified.
-    * @throws ReferenceFrameMismatchException if {@code expressedInFrame} is not equal to the frame
-    *            in which this spatial vector is currently expressed.
+    * @throws ReferenceFrameMismatchException if {@code expressedInFrame} is not equal to the frame in
+    *            which this spatial vector is currently expressed.
     */
    default void set(ReferenceFrame expressedInFrame, Vector3DReadOnly angularPart, Vector3DReadOnly linearPart)
    {
@@ -415,8 +439,8 @@ default void normalize()
     * </p>
     *
     * @param other the other vector to add to this vector. Not modified.
-    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same
-    *            reference frame as {@code this}.
+    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference
+    *            frame as {@code this}.
     */
    default void add(SpatialVectorReadOnly other)
    {
@@ -465,8 +489,8 @@ default void add(FrameVector3DReadOnly angular, FrameVector3DReadOnly linear)
     * </p>
     *
     * @param other the other vector to subtract to this vector. Not modified.
-    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same
-    *            reference frame as {@code this}.
+    * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference
+    *            frame as {@code this}.
     */
    default void sub(SpatialVectorReadOnly other)
    {

diff --git a/src/main/java/us/ihmc/mecano/spatial/interfaces/SpatialAccelerationReadOnly.java b/src/main/java/us/ihmc/mecano/spatial/interfaces/SpatialAccelerationReadOnly.java
@@ -4,7 +4,6 @@
 import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
 import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DBasics;
 import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
-import us.ihmc.mecano.spatial.Twist;
 import us.ihmc.mecano.tools.MecanoTools;
 
 /**
@@ -140,7 +139,7 @@ default void getLinearAccelerationAt(TwistReadOnly bodyTwist, FramePoint3DReadOn
     *            reference frames as {@code this}, or if this spatial acceleration is not expressed
     *            in the body frame.
     */
-   default void getLinearAccelerationAtBodyOrigin(Twist bodyTwist, FrameVector3DBasics linearAccelerationToPack)
+   default void getLinearAccelerationAtBodyOrigin(TwistReadOnly bodyTwist, FrameVector3DBasics linearAccelerationToPack)
    {
       getBodyFrame().checkReferenceFrameMatch(getReferenceFrame());
       checkReferenceFrameMatch(bodyTwist);

diff --git a/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameSpatialAcceleration.java b/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameSpatialAcceleration.java
@@ -3,7 +3,6 @@
 import us.ihmc.euclid.interfaces.GeometryObject;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.euclid.transform.interfaces.Transform;
 import us.ihmc.euclid.tuple3D.Point3D;
@@ -243,14 +242,14 @@ public ReferenceFrame getReferenceFrame()
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getAngularPart()
+   public YoFrameVector3D getAngularPart()
    {
       return spatialVector.getAngularPart();
    }
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getLinearPart()
+   public YoFrameVector3D getLinearPart()
    {
       return spatialVector.getLinearPart();
    }

diff --git a/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameSpatialForce.java b/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameSpatialForce.java
@@ -3,7 +3,6 @@
 import us.ihmc.euclid.interfaces.GeometryObject;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
 import us.ihmc.euclid.transform.RigidBodyTransform;
 import us.ihmc.euclid.transform.interfaces.Transform;
 import us.ihmc.euclid.tuple3D.Point3D;
@@ -125,14 +124,14 @@ public ReferenceFrame getReferenceFrame()
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getAngularPart()
+   public YoFrameVector3D getAngularPart()
    {
       return spatialVector.getAngularPart();
    }
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getLinearPart()
+   public YoFrameVector3D getLinearPart()
    {
       return spatialVector.getLinearPart();
    }

diff --git a/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameWrench.java b/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoFixedFrameWrench.java
@@ -3,7 +3,6 @@
 import us.ihmc.euclid.interfaces.GeometryObject;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
 import us.ihmc.euclid.transform.interfaces.Transform;
 import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
 import us.ihmc.mecano.spatial.SpatialForce;
@@ -142,14 +141,14 @@ public ReferenceFrame getReferenceFrame()
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getAngularPart()
+   public YoFrameVector3D getAngularPart()
    {
       return spatialForceVector.getAngularPart();
    }
 
    /** {@inheritDoc} */
    @Override
-   public FixedFrameVector3DBasics getLinearPart()
+   public YoFrameVector3D getLinearPart()
    {
       return spatialForceVector.getLinearPart();
    }