ihmcrobotics / euclid Goto Github PK
View Code? Open in Web Editor NEWVector math, geometry, reference frame, and shapes 2D & 3D
License: Apache License 2.0
Vector math, geometry, reference frame, and shapes 2D & 3D
License: Apache License 2.0
AffineTransform getTranslation and getLinearTransform say they return a "read-only" yet they return modifiable objects.
Packing Quaternions directly with non-Euclid data, can result in NotARotationMatrix exceptions, as shown below. Currently there is a workaround to go through LinearTransform3D, that fixes the issue.
It'd be nice if there was just a method to sanitize these orientations, as LinearTransform3D must do somehow, with a static Tools method or something.
// Going through LinearTransform3D first prevents NotARotationMatrix exceptions.
LinearTransform3D linearTransform3D = new LinearTransform3D(EuclidGeometryTools.axisAngleFromZUpToVector3D(gpuPlanarRegion.getNormal()));
linearTransform3D.normalize();
FrameQuaternion orientation = new FrameQuaternion();
orientation.setIncludingFrame(cameraFrame, linearTransform3D.getAsQuaternion());
orientation.changeFrame(ReferenceFrame.getWorldFrame());
us.ihmc.euclid.exceptions.NotARotationMatrixException: The matrix is not a rotation matrix:
/ 0.305, 0.663, 0.683 \
|-0.220, 0.747, -0.627 |
\-0.927, 0.041, 0.374 /
at us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly.checkIfRotationMatrix(Matrix3DReadOnly.java:393)
at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.set(RotationMatrixBasics.java:101)
at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:511)
at us.ihmc.euclid.tools.RotationMatrixTools.multiplyImpl(RotationMatrixTools.java:409)
at us.ihmc.euclid.tools.RotationMatrixTools.multiply(RotationMatrixTools.java:308)
at us.ihmc.euclid.matrix.interfaces.RotationMatrixBasics.append(RotationMatrixBasics.java:326)
at us.ihmc.euclid.transform.interfaces.RigidBodyTransformBasics.multiply(RigidBodyTransformBasics.java:673)
at us.ihmc.euclid.referenceFrame.ReferenceFrame.efficientComputeTransform(ReferenceFrame.java:924)
at us.ihmc.euclid.referenceFrame.ReferenceFrame.getTransformToRoot(ReferenceFrame.java:862)
at us.ihmc.euclid.referenceFrame.ReferenceFrame.transformFromThisToDesiredFrame(ReferenceFrame.java:811)
at us.ihmc.euclid.referenceFrame.interfaces.FrameChangeable.changeFrame(FrameChangeable.java:36)
at us.ihmc.avatar.gpuPlanarRegions.GPUPlanarRegionExtraction.lambda$computePlanarRegions$6(GPUPlanarRegionExtraction.java:459)
Put together a document listing all of the *Tools
classes with a short summary for each one
To make it possible to reset the state of the controller to start, ReferenceFrameTools.clearWorldFrameTree() should set the framesAddedToTree variable in ReferenceFrame worldframe to 0.
For the colinear case, it would be nice if the return value could be identified as such.
Since a return of 0.0 is already a successful return value for non-parallel lines, 0.0 doesn't feel right as a return value.
I suggest Double.POSITIVE_INFINITY since the return value is Double.NaN for the parallel non-intersecting case.
What do you think?
public static double percentageOfIntersectionBetweenTwoLine2Ds(double pointOnLine1x, double pointOnLine1y, double lineDirection1x, double lineDirection1y,
double pointOnLine2x, double pointOnLine2y, double lineDirection2x, double lineDirection2y)
{
// We solve for x the problem of the form: A * x = b
// A * x = b
// / lineDirection1x -lineDirection2x \ / alpha \ / pointOnLine2x - pointOnLine1x \
// | | * | | = | |
// \ lineDirection1y -lineDirection2y / \ beta / \ pointOnLine2y - pointOnLine1y /
// Here, only alpha or beta is needed.
double determinant = -lineDirection1x * lineDirection2y + lineDirection1y * lineDirection2x;
double dx = pointOnLine2x - pointOnLine1x;
double dy = pointOnLine2y - pointOnLine1y;
if (Math.abs(determinant) < EuclidGeometryTools.ONE_TRILLIONTH)
{ // The lines are parallel
// Check if they are collinear
double cross = dx * lineDirection1y - dy * lineDirection1x;
if (Math.abs(cross) < EuclidGeometryTools.ONE_TRILLIONTH)
{
/*
* The two lines are collinear. There's an infinite number of intersection. Let's just set the
* result to pointOnLine1, i.e. alpha = 0.0.
*/
return Double.POSITIVE_INFINITY;
}
else
{
return Double.NaN;
}
}
else
{
double oneOverDeterminant = 1.0 / determinant;
double AInverse00 = oneOverDeterminant * -lineDirection2y;
double AInverse01 = oneOverDeterminant * lineDirection2x;
double alpha = AInverse00 * dx + AInverse01 * dy;
return alpha;
}
}
Hi, I'm looking into euclid and like it - it seems there are not that many robotics-centric spatial-math libs out there for java!
For my use-case, I often have to transform a point from one frame/pose to another frame/pose. For this, I'm used to functions like
poseA.between(poseB)
giving me the transform between two poses and pose.inverse()
giving me the inverse of the pose's transform - where pose would be in 2D or 3D. How would I do that in euclid?
For some context, I'm coming from the GTSAM way of pose calculus: https://github.com/devbharat/gtsam/blob/master/gtsam/geometry/Pose3.h
Thanks!
Example: EuclidFrameIOTools.getFrameTuple3DString(String, FrameTuple3DReadOnly)
does not do anything with the given format string.
The following case current fails:
There are two random polygons, in random frames, polygonA
and polygonB
.
polygonA.clear();
polygonB.clear();
polygonB.setIncludingFrame(polygonA);
assert(polygonA.epsilonEquals(polygonB, 1e-5);
This fails because of a reference frame mismatch.
RigidBodyTranform:259
should be:
public void set(RigidBodyTransformReadOnly other)
For when you need a lot of frames and you don't care too much what the name is and you just want to make sure each one is unique. I suggest just putting the calling classname and an index. I've been using a ThreadLocal MutableInt for this, but maybe a global AtomicInteger is better.
import org.apache.commons.lang3.mutable.MutableInt;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
import us.ihmc.euclid.transform.RigidBodyTransform;
public static ThreadLocal<MutableInt> INDEX = ThreadLocal.withInitial(MutableInt::new);
public static String getCallingClassName()
{
StackTraceElement callerStackElement = new Throwable().getStackTrace()[2];
return callerStackElement.getClassName();
}
public static ReferenceFrame constructFrameWithUnchangingTransformFromParent(ReferenceFrame parentFrame, RigidBodyTransform transformFromParent)
{
return ReferenceFrameTools.constructFrameWithUnchangingTransformFromParent(getCallingClassName() + INDEX.get(),
parentFrame,
transformFromParent);
}
public static ReferenceFrame constructFrameWithUnchangingTransformToParent(ReferenceFrame parentFrame, RigidBodyTransform transformToParent)
{
return ReferenceFrameTools.constructFrameWithUnchangingTransformToParent(getCallingClassName() + INDEX.get(),
parentFrame,
transformToParent);
}
public static ReferenceFrame constructFrameWithChangingTransformFromParent(ReferenceFrame parentFrame, RigidBodyTransform transformFromParent)
{
return ReferenceFrameTools.constructFrameWithChangingTransformFromParent(getCallingClassName() + INDEX.get(),
parentFrame,
transformFromParent);
}
public static ReferenceFrame constructFrameWithChangingTransformToParent(ReferenceFrame parentFrame, RigidBodyTransform transformToParent)
{
return ReferenceFrameTools.constructFrameWithChangingTransformToParent(getCallingClassName() + INDEX.get(),
parentFrame,
transformToParent);
}
Statically initialized seeds lead to same values in every sequence. E.g:
Quaternion Based TransformTest
Random random = new Random(45L);
QuaternionBasedTransform transform = EuclidCoreRandomTools.nextQuaternionBasedTransform(random);
will always result in:
x = -0.5599634331913013
y = 0.5960433401296669
z = 0.41656366724225924
s = 0.3970491169985922
Recommended solution is initializing with a source of random: System.nanoTime()
Method exists for 3D line segments, but not 2D. Similar to overall statement from Issue #18
The FrameLineSegment and FrameLineSegment2d classes in ihmc-robotics-toolkit needs to be moved to Euclid. They are math classes, and the dependency tree from SCS to YoVariables necessitates they exist lower down the chain.
ReferenceFrame.getTransformToParent() returns a copy of transformToParent, while ReferenceFrame.getTransformToParent(RigidBodyTransform transformToPack) calls updateTransformToParent(transformToPack)
ReferenceFrames with unchanging transform to parent have an empty updateTransformToParent and set transformToParent in the constructor. Hence the result does not match ReferenceFrame.getTransformToParent()
I am pretty sure an exception is always thrown when trying to create a ConvexPolygon2D from a ConvexPolygon2DReadOnly that has not been updated under the hood. This is problematic since you can't update a ReadOnly one.
Exception in thread "GDXFootstepPlanGraphic-pool-28346-thread-5" us.ihmc.euclid.geometry.exceptions.OutdatedPolygonException: Call the update method before doing any other calculation!
at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly.checkIfUpToDate(ConvexPolygon2DReadOnly.java:57)
at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly.getVertex(ConvexPolygon2DReadOnly.java:168)
at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics.addVertices(ConvexPolygon2DBasics.java:183)
at us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DBasics.set(ConvexPolygon2DBasics.java:237)
at us.ihmc.euclid.geometry.ConvexPolygon2D.<init>(ConvexPolygon2D.java:117)
ConvexPolygon2D updatedFoothold = new ConvexPolygon2D(minimalFootstep.getFoothold());
foothold.set(updatedFoothold);
The following test passes:
Point2D pointA = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
Point2D pointB = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
FrameConvexPolygon2D polygon = new FrameConvexPolygon2D();
polygon.addVertex(pointA);
polygon.addVertex(pointA);
polygon.addVertex(pointB);
polygon.addVertex(pointB);
polygon.update();
assertEquals(2, polygon.getNumberOfVertices());
while the following test fails:
Point2D pointA = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
Point2D pointB = EuclidCoreRandomTools.nextPoint2D(random, 10.0);
FrameConvexPolygon2D polygon = new FrameConvexPolygon2D();
polygon.addVertex(pointA);
polygon.addVertex(pointB);
polygon.addVertex(pointA);
polygon.addVertex(pointB);
polygon.update();
assertEquals(2, polygon.getNumberOfVertices());
Something in the giftwrapping of the vertices is not functioning properly.
Should be Pose3D(Tuple3DReadOnly, Orientation3DReadOnly)
instead of Pose3D(Point3DReadOnly, Orientation3DReadOnly)
Something like this which allows to make (UI stuff especially) abstract over yaw, pitch, and roll. These are often the gizmo handles or how you want to tell the chest to yaw, pitch, roll, etc..
public enum YawPitchRollAxis
{
YAW(Axis3D.Z), PITCH(Axis3D.Y), ROLL(Axis3D.X);
private final Axis3D axis3D;
private final String lowerCasedName;
private final String pascalCasedName;
YawPitchRollAxis(Axis3D axis3D)
{
this.axis3D = axis3D;
lowerCasedName = name().toLowerCase();
pascalCasedName = StringUtils.capitalize(lowerCasedName);
}
public YawPitchRoll createYawPitchRoll(double angle)
{
YawPitchRoll yawPitchRoll = new YawPitchRoll();
switch (this)
{
case YAW -> yawPitchRoll.setYaw(angle);
case PITCH -> yawPitchRoll.setPitch(angle);
case ROLL -> yawPitchRoll.setRoll(angle);
}
return yawPitchRoll;
}
private double getFromYawPitchRoll(YawPitchRollReadOnly yawPitchRollReadOnly)
{
return switch (this)
{
case YAW -> yawPitchRollReadOnly.getYaw();
case PITCH -> yawPitchRollReadOnly.getPitch();
case ROLL -> yawPitchRollReadOnly.getRoll();
};
}
public Axis3D getAxis3D()
{
return axis3D;
}
public String getLowerCasedName()
{
return lowerCasedName;
}
public String getPascalCasedName()
{
return pascalCasedName;
}
}
Not as critical as FrameLine and FramePose, but all FrameGeometryObjects should make their way into Euclid to solve the dependency tree issues.
There is very often int intersectionWith(Line3DReadOnly line, Point3DBasics firstIntersectionToPack, Point3DBasics secondIntersectionToPack)
, but when doing graphics picking, I have a ray and I don't want to collide with things behind the screen.
This just requests that wherever there's an intersectWith line, there's also one for a ray.
For example, the method EuclidGeometryTools.closestPoint3DsBetweenTwoLineSegment3Ds
exists, but not EuclidGeometryTools.closestPoint2DsBetweenTwoLineSegment2Ds
, which I just pattern matched. Same thing with EuclidGeometryTools.distanceBetweenTwoLineSegment3Ds()
. I'm doing them in the PlanarRegionTools for a lot of that stuff, but am not really testing it as thoroughly as I should.
This should be straightforward as RotationMatrix already implement interpolate.
us.ihmc.euclid.geometry.tools.EuclidGeometryPolygonTools#computeConvexPolyong2DArea
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.