Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/finite difference #61

Merged
merged 7 commits into from
Feb 8, 2024
Merged

Feature/finite difference #61

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
1863fa9
Implemented finite difference for orientation 3D.
SylvainBertrand Jan 18, 2024
460f002
Implemented finite difference for tuples, transform, orientation 2D.
SylvainBertrand Jan 19, 2024
f794f39
Implemented first order integration.
SylvainBertrand Jan 19, 2024
aa3dec1
Updated the github workflow to include a test report.
SylvainBertrand Jan 19, 2024
5737f56
Fixed tests
SylvainBertrand Jan 22, 2024
6b299de
Forgot to update the permissions for the GitHub workflow
SylvainBertrand Jan 22, 2024
07fccbc
Missed on test.
SylvainBertrand Jan 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions src/main/java/us/ihmc/euclid/rotationConversion/RotationMatrixConversion.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ private RotationMatrixConversion()
}

/**
* Sets the given rotation matrix to represent a counter clockwise rotation around the z-axis of an
* Sets the given rotation matrix to represent a counter-clockwise rotation around the z-axis of an
* angle {@code yaw}.
*
* <pre>
Expand Down Expand Up @@ -429,7 +429,6 @@ public static void convertYawPitchRollToMatrix(double yaw, double pitch, double
((RotationMatrixBasics) matrixToPack).setUnsafe(m00, m01, m02, m10, m11, m12, m20, m21, m22);
else
matrixToPack.set(m00, m01, m02, m10, m11, m12, m20, m21, m22);

}

/**
Expand Down
22 changes: 11 additions & 11 deletions src/main/java/us/ihmc/euclid/rotationConversion/RotationVectorConversion.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,7 @@ public static void convertMatrixToRotationVector(RotationMatrixReadOnly rotation
double m21 = rotationMatrix.getM21();
double m22 = rotationMatrix.getM22();

convertMatrixToRotationVectorImpl(m00, m01, m02, m10, m11, m12, m20, m21, m22, rotationVectorToPack);
convertMatrixToRotationVector(m00, m01, m02, m10, m11, m12, m20, m21, m22, rotationVectorToPack);
}

/**
Expand Down Expand Up @@ -220,16 +220,16 @@ public static void convertMatrixToRotationVector(RotationMatrixReadOnly rotation
* conversion.
* @param rotationVectorToPack the vector in which the result is stored. Modified.
*/
static void convertMatrixToRotationVectorImpl(double m00,
double m01,
double m02,
double m10,
double m11,
double m12,
double m20,
double m21,
double m22,
Vector3DBasics rotationVectorToPack)
public static void convertMatrixToRotationVector(double m00,
double m01,
double m02,
double m10,
double m11,
double m12,
double m20,
double m21,
double m22,
Vector3DBasics rotationVectorToPack)
{
if (EuclidCoreTools.containsNaN(m00, m01, m02, m10, m11, m12, m20, m21, m22))
{
Expand Down
83 changes: 75 additions & 8 deletions src/main/java/us/ihmc/euclid/tools/AxisAngleTools.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,12 @@
import us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics;
import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly;
import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly;
import us.ihmc.euclid.rotationConversion.AxisAngleConversion;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
import us.ihmc.euclid.tuple4D.interfaces.Vector4DBasics;
Expand Down Expand Up @@ -156,7 +158,7 @@ public static void addTransform(AxisAngleReadOnly axisAngle, Tuple3DReadOnly tup
* @param checkIfTransformInXYPlane whether this method should assert that the axis-angle represents
* a transformation in the XY plane.
* @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle
* does not represent a transformation in the XY plane.
* does not represent a transformation in the XY plane.
*/
public static void transform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleOriginal, Tuple2DBasics tupleTransformed, boolean checkIfTransformInXYPlane)
{
Expand All @@ -181,7 +183,7 @@ public static void transform(AxisAngleReadOnly axisAngle, Tuple2DReadOnly tupleO
* @param checkIfTransformInXYPlane whether this method should assert that the axis-angle represents
* a transformation in the XY plane.
* @throws NotAMatrix2DException if {@code checkIfTransformInXYPlane == true} and the axis-angle
* does not represent a transformation in the XY plane.
* does not represent a transformation in the XY plane.
*/
public static void inverseTransform(AxisAngleReadOnly axisAngle,
Tuple2DReadOnly tupleOriginal,
Expand Down Expand Up @@ -548,9 +550,8 @@ public static void multiply(Orientation3DReadOnly orientation1,
}

double beta, u2x, u2y, u2z;
if (orientation2 instanceof AxisAngleReadOnly)
if (orientation2 instanceof AxisAngleReadOnly aa2)
{ // In this case orientation2 might be the same object as axisAngleToPack, so let's save its components first.
AxisAngleReadOnly aa2 = (AxisAngleReadOnly) orientation2;
beta = aa2.getAngle();
u2x = aa2.getX();
u2y = aa2.getY();
Expand Down Expand Up @@ -1110,6 +1111,41 @@ public static void appendRollRotation(AxisAngleReadOnly axisAngleOriginal, doubl
axisAngleToPack.set(sinHalfGammaUx * sinHalfGammaInv, sinHalfGammaUy * sinHalfGammaInv, sinHalfGammaUz * sinHalfGammaInv, gamma);
}

/**
* Append a rotation vector to {@code original} and stores the result in {@code output}.
*
* @param original the orientation to append the rotation vector to. Not modified.
* @param rx the x-component of the rotation vector.
* @param ry the y-component of the rotation vector.
* @param rz the z-component of the rotation vector.
* @param output the axis-angle in which the result is stored. Modified.
*/
public static void appendRotationVector(Orientation3DReadOnly original, double rx, double ry, double rz, AxisAngleBasics output)
{
double norm = EuclidCoreTools.norm(rx, ry, rz);

if (norm <= AxisAngleConversion.EPS)
{
output.set(original);
return;
}

double angle = norm;
norm = 1.0 / norm;
double ux = rx * norm;
double uy = ry * norm;
double uz = rz * norm;
if (original instanceof AxisAngleReadOnly aaOriginal)
{
multiplyImpl(aaOriginal.getAngle(), aaOriginal.getX(), aaOriginal.getY(), aaOriginal.getZ(), false, angle, ux, uy, uz, false, output);
}
else
{
output.set(original);
multiplyImpl(output.getAngle(), output.getX(), output.getY(), output.getZ(), false, angle, ux, uy, uz, false, output);
}
}

/**
* Performs a cross platform angular distance calculation between axis angle and any other 3D
* orientation systems.
Expand All @@ -1118,7 +1154,7 @@ public static void appendRollRotation(AxisAngleReadOnly axisAngleOriginal, doubl
* @param orientation3D the orientation3D to be used for comparison. Not modified
* @param limitToPi limits the result to [0 , <i>pi</i>].
* @return angular distance between the two orientations in range: [0, 2<i>pi</i>] when limitToPi =
* false.
* false.
*/
public static double distance(AxisAngleReadOnly axisAngle, Orientation3DReadOnly orientation3D, boolean limitToPi)
{
Expand Down Expand Up @@ -1151,7 +1187,7 @@ public static double distance(AxisAngleReadOnly axisAngle, Orientation3DReadOnly
* @param quaternion the quaternion to be used for comparison. Not modified
* @param limitToPi limits the result to [0 , <i>pi</i>] if set true.
* @return angular distance between the two orientations in range: [0, 2<i>pi</i>] when limitToPi =
* false.
* false.
*/
public static double distance(AxisAngleReadOnly axisAngle, QuaternionReadOnly quaternion, boolean limitToPi)
{
Expand Down Expand Up @@ -1242,7 +1278,7 @@ public static double distance(AxisAngleReadOnly axisAngle, RotationMatrixReadOnl
* @param yawPitchRoll the yawPitchRoll to be used for comparison. Not modified
* @param limitToPi Limits the result to [0, <i>pi</i>].
* @return angular distance between the two orientations in range: [0, 2<i>pi</i>] when limitToPi =
* false.
* false.
*/
public static double distance(AxisAngleReadOnly axisAngle, YawPitchRollReadOnly yawPitchRoll, boolean limitToPi)
{
Expand Down Expand Up @@ -1305,7 +1341,7 @@ public static double distance(AxisAngleReadOnly axisAngle, YawPitchRollReadOnly
* @param aa2 the second axis-angle to measure the distance. Not modified.
* @param limitToPi Limits the result to [0, <i>pi</i>].
* @return the angle representing the distance between the two axis-angles. It is contained in [0,
* 2<i>pi</i>] when limitToPi = false.
* 2<i>pi</i>] when limitToPi = false.
*/
public static double distance(AxisAngleReadOnly aa1, AxisAngleReadOnly aa2, boolean limitToPi)
{
Expand Down Expand Up @@ -1352,4 +1388,35 @@ static double distance(AxisAngleReadOnly aa1, double u2x, double u2y, double u2z
}
return Math.abs(gamma);
}

/**
* Computes the angular velocity from the finite difference of two orientations.
*
* @param previousOrientation the orientation at the previous time step. Not modified.
* @param currentOrientation the orientation at the current time step. Not modified.
* @param dt the time step.
* @param angularVelocityToPack the vector used to store the angular velocity expressed in the orientation's local coordinates. Modified.
* @see EuclidCoreTools#finiteDifference(Orientation3DReadOnly, Orientation3DReadOnly, double, Vector3DBasics)
*/
public static void finiteDifference(AxisAngleReadOnly previousOrientation,
AxisAngleReadOnly currentOrientation,
double dt,
Vector3DBasics angularVelocityToPack)
{
double halfAnglePrev = 0.5 * previousOrientation.getAngle();
double sinHalfAnglePrev = Math.sin(halfAnglePrev);
double xPrev = previousOrientation.getX() * sinHalfAnglePrev;
double yPrev = previousOrientation.getY() * sinHalfAnglePrev;
double zPrev = previousOrientation.getZ() * sinHalfAnglePrev;
double sPrev = Math.cos(halfAnglePrev);

double halfAngleCurr = 0.5 * currentOrientation.getAngle();
double sinHalfAngleCurr = Math.sin(halfAngleCurr);
double xCurr = currentOrientation.getX() * sinHalfAngleCurr;
double yCurr = currentOrientation.getY() * sinHalfAngleCurr;
double zCurr = currentOrientation.getZ() * sinHalfAngleCurr;
double sCurr = Math.cos(halfAngleCurr);

QuaternionTools.finiteDifference(xPrev, yPrev, zPrev, sPrev, xCurr, yCurr, zCurr, sCurr, dt, angularVelocityToPack);
}
}
11 changes: 4 additions & 7 deletions src/main/java/us/ihmc/euclid/tools/EuclidCoreRandomTools.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,9 +1,6 @@
package us.ihmc.euclid.tools;

import java.util.Random;

import org.ejml.data.DMatrixRMaj;

import us.ihmc.euclid.Axis2D;
import us.ihmc.euclid.Axis3D;
import us.ihmc.euclid.axisAngle.AxisAngle;
Expand Down Expand Up @@ -40,6 +37,8 @@
import us.ihmc.euclid.tuple4D.Vector4D32;
import us.ihmc.euclid.yawPitchRoll.YawPitchRoll;

import java.util.Random;

/**
* This class provides random generators to generate random geometry objects.
* <p>
Expand Down Expand Up @@ -1556,11 +1555,9 @@ public static void randomizeAxisAngle(Random random, AxisAngleBasics axisAngleTo
*/
public static void randomizeAxisAngle(Random random, double minMaxAngle, AxisAngleBasics axisAngleToRandomize)
{
// Generate uniformly random point on unit sphere (based on http://mathworld.wolfram.com/SpherePointPicking.html )
double height = 2.0 * random.nextDouble() - 1.0;
// Generate uniformly random point on the unit-sphere (based on http://mathworld.wolfram.com/SpherePointPicking.html )
double angle = nextDouble(random, minMaxAngle);
double radius = EuclidCoreTools.squareRoot(1.0 - height * height);
axisAngleToRandomize.set(radius * EuclidCoreTools.cos(angle), radius * EuclidCoreTools.sin(angle), height, angle);
axisAngleToRandomize.set(EuclidCoreRandomTools.nextVector3D(random), angle);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The old implementation was incorrect in the sense that it wouldn't generate random axes that would be uniformly distributed.
Fixing this seems to break other tests. I'll work on fixing them

}

/**
Expand Down
Loading
Loading