diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java index 1d0e99d4f26917fb69ee16a46d57a773a473225e..be9616185adfbec71c91d9c9cd9a71a3da2e7162 100644 --- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java +++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransform.java @@ -79,6 +79,10 @@ public class FrameTransform { return target; } + public FrameTransform invert() { + return new FrameTransform(transform.invert(), target, source, time); + } + /** * @return the time associated with the {@link FrameTransform} or {@code null} * if there is no associated time diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java index e4e9571de165f3330e635cb406154128975c4cac..4f25095906d00e966be70292cf36f0e298d2f98f 100644 --- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java +++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/FrameTransformTree.java @@ -196,27 +196,34 @@ public class FrameTransformTree { return new FrameTransform(Transform.identity(), resolvedSource, resolvedTarget, null); } FrameTransform sourceToRoot = transformToRoot(resolvedSource); - if (sourceToRoot != null && sourceToRoot.getTargetFrame().equals(resolvedTarget)) { - return sourceToRoot; - } FrameTransform targetToRoot = transformToRoot(resolvedTarget); - if (targetToRoot != null) { - if (targetToRoot.getTargetFrame().equals(resolvedTarget)) { - return targetToRoot; - } + if (sourceToRoot == null && targetToRoot == null) { + return null; + } + if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(resolvedSource)) { - Transform transform = targetToRoot.getTransform().invert(); - return new FrameTransform(transform, resolvedSource, resolvedTarget, targetToRoot.getTime()); + // resolvedSource is root. + return targetToRoot.invert(); + } else { + return null; } } - if (sourceToRoot == null || targetToRoot == null) { - return null; + if (targetToRoot == null) { + if (sourceToRoot.getTargetFrame().equals(resolvedTarget)) { + // resolvedTarget is root. + return sourceToRoot; + } else { + return null; + } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { + // Neither resolvedSource nor resolvedTarget is root and both share the + // same root. Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, resolvedSource, resolvedTarget, sourceToRoot.getTime()); } + // No known transform. return null; } @@ -234,7 +241,8 @@ public class FrameTransformTree { * the resolved source frame * @return the {@link Transform} from {@code source} to root */ - private FrameTransform transformToRoot(GraphName resolvedSource) { + @VisibleForTesting + FrameTransform transformToRoot(GraphName resolvedSource) { FrameTransform result = getLatest(resolvedSource); if (result == null) { return null; diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java index f0d399ce726ed32d474f63e0382519a96c078b50..55d33b6041becfffe41a2268738b41b0f1c449b1 100644 --- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java +++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Quaternion.java @@ -124,6 +124,10 @@ public class Quaternion { return Math.sqrt(getMagnitudeSquared()); } + public boolean isAlmostNeutral(double epsilon) { + return Math.abs(1 - x * x - y * y - z * z - w * w) < epsilon; + } + public geometry_msgs.Quaternion toQuaternionMessage(geometry_msgs.Quaternion result) { result.setX(x); result.setY(y); diff --git a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java index ba2439cc94e7161085f176741e3b4a4e36dab9fc..6d23eb7999ad789670f1cb363d582299e24d2185 100644 --- a/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java +++ b/rosjava_geometry/src/main/java/org/ros/rosjava_geometry/Transform.java @@ -16,18 +16,24 @@ package org.ros.rosjava_geometry; +import com.google.common.annotations.VisibleForTesting; +import com.google.common.collect.Lists; + import org.ros.message.Time; import org.ros.namespace.GraphName; +import java.util.List; + /** - * A transformation in terms of translation and rotation. + * A transformation in terms of translation, rotation, and scale. * + * @author damonkohler@google.com (Damon Kohler) * @author moesenle@google.com (Lorenz Moesenlechner) */ public class Transform { private Vector3 translation; - private Quaternion rotation; + private Quaternion rotationAndScale; public static Transform fromTransformMessage(geometry_msgs.Transform message) { return new Transform(Vector3.fromVector3Message(message.getTranslation()), @@ -65,7 +71,7 @@ public class Transform { public Transform(Vector3 translation, Quaternion rotation) { this.translation = translation; - this.rotation = rotation; + this.rotationAndScale = rotation; } /** @@ -76,24 +82,29 @@ public class Transform { * @return the resulting {@link Transform} */ public Transform multiply(Transform other) { - return new Transform(apply(other.getTranslation()), apply(other.getRotation())); + return new Transform(apply(other.translation), apply(other.rotationAndScale)); } public Transform invert() { - Quaternion inverseRotation = rotation.invert(); - return new Transform(inverseRotation.rotateVector(translation.invert()), inverseRotation); + Quaternion inverseRotationAndScale = rotationAndScale.invert(); + return new Transform(inverseRotationAndScale.rotateVector(translation.invert()), + inverseRotationAndScale); } public Vector3 apply(Vector3 vector) { - return rotation.rotateVector(vector).add(translation); + return rotationAndScale.rotateVector(vector).add(translation); } public Quaternion apply(Quaternion quaternion) { - return rotation.multiply(quaternion); + return rotationAndScale.multiply(quaternion); } public Transform scale(double factor) { - return new Transform(translation, rotation.scale(factor)); + return new Transform(translation, rotationAndScale.scale(Math.sqrt(factor))); + } + + public double getScale() { + return rotationAndScale.getMagnitudeSquared(); } /** @@ -102,28 +113,28 @@ public class Transform { * rotation matrix</a> */ public double[] toMatrix() { - double x = getRotation().getX(); - double y = getRotation().getY(); - double z = getRotation().getZ(); - double w = getRotation().getW(); - double mm = getRotation().getMagnitudeSquared(); + double x = rotationAndScale.getX(); + double y = rotationAndScale.getY(); + double z = rotationAndScale.getZ(); + double w = rotationAndScale.getW(); + double mm = rotationAndScale.getMagnitudeSquared(); return new double[] { mm - 2 * y * y - 2 * z * z, 2 * x * y + 2 * z * w, 2 * x * z - 2 * y * w, 0, 2 * x * y - 2 * z * w, mm - 2 * x * x - 2 * z * z, 2 * y * z + 2 * x * w, 0, 2 * x * z + 2 * y * w, 2 * y * z - 2 * x * w, mm - 2 * x * x - 2 * y * y, 0, - getTranslation().getX(), getTranslation().getY(), getTranslation().getZ(), 1 + translation.getX(), translation.getY(), translation.getZ(), 1 }; } public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) { result.setTranslation(translation.toVector3Message(result.getTranslation())); - result.setRotation(rotation.toQuaternionMessage(result.getRotation())); + result.setRotation(rotationAndScale.toQuaternionMessage(result.getRotation())); return result; } public geometry_msgs.Pose toPoseMessage(geometry_msgs.Pose result) { result.setPosition(translation.toPointMessage(result.getPosition())); - result.setOrientation(rotation.toQuaternionMessage(result.getOrientation())); + result.setOrientation(rotationAndScale.toQuaternionMessage(result.getOrientation())); return result; } @@ -135,24 +146,43 @@ public class Transform { return result; } - public Vector3 getTranslation() { + public boolean almostEquals(Transform other, double epsilon) { + List<Double> epsilons = Lists.newArrayList(); + epsilons.add(translation.getX() - other.getTranslation().getX()); + epsilons.add(translation.getY() - other.getTranslation().getY()); + epsilons.add(translation.getZ() - other.getTranslation().getZ()); + epsilons.add(rotationAndScale.getX() - other.getRotationAndScale().getX()); + epsilons.add(rotationAndScale.getY() - other.getRotationAndScale().getY()); + epsilons.add(rotationAndScale.getZ() - other.getRotationAndScale().getZ()); + epsilons.add(rotationAndScale.getW() - other.getRotationAndScale().getW()); + for (double e : epsilons) { + if (Math.abs(e) > epsilon) { + return false; + } + } + return true; + } + + @VisibleForTesting + Vector3 getTranslation() { return translation; } - public Quaternion getRotation() { - return rotation; + @VisibleForTesting + Quaternion getRotationAndScale() { + return rotationAndScale; } @Override public String toString() { - return String.format("Transform<%s, %s>", translation, rotation); + return String.format("Transform<%s, %s>", translation, rotationAndScale); } @Override public int hashCode() { final int prime = 31; int result = 1; - result = prime * result + ((rotation == null) ? 0 : rotation.hashCode()); + result = prime * result + ((rotationAndScale == null) ? 0 : rotationAndScale.hashCode()); result = prime * result + ((translation == null) ? 0 : translation.hashCode()); return result; } @@ -166,10 +196,10 @@ public class Transform { if (getClass() != obj.getClass()) return false; Transform other = (Transform) obj; - if (rotation == null) { - if (other.rotation != null) + if (rotationAndScale == null) { + if (other.rotationAndScale != null) return false; - } else if (!rotation.equals(other.rotation)) + } else if (!rotationAndScale.equals(other.rotationAndScale)) return false; if (translation == null) { if (other.translation != null) diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java index 13fcd367513da738d47e2f033dbf652871ae65b7..b41da59b53de7f9335157038b65c9f61f428df13 100644 --- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java +++ b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/FrameTransformTreeTest.java @@ -129,31 +129,97 @@ public class FrameTransformTreeTest { } } - @Test - public void testTransformToRoot() { + /** + * Fills the {@link FrameTransformTree} with the following frame topography: + * + * <pre> + * foo + * bar bop + * baz fuz + * </pre> + */ + private void updateFrameTransformTree() { + { + Transform transform = Transform.translation(0, 1, 0); + frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time())); + } { - Vector3 vector = Vector3.zero(); - Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5)); - Transform transform = new Transform(vector, quaternion); + Transform transform = Transform.xRotation(Math.PI / 2); frameTransformTree.update(newTransformStampedMessage(transform, "baz", "bar", new Time())); } - { - Vector3 vector = new Vector3(0, 1, 0); - Quaternion quaternion = Quaternion.identity(); - Transform transform = new Transform(vector, quaternion); - frameTransformTree.update(newTransformStampedMessage(transform, "bar", "foo", new Time())); + Transform transform = Transform.translation(1, 0, 0); + frameTransformTree.update(newTransformStampedMessage(transform, "bop", "foo", new Time())); + } + { + Transform transform = Transform.yRotation(Math.PI / 2); + frameTransformTree.update(newTransformStampedMessage(transform, "fuz", "bop", new Time())); } + } - FrameTransform frameTransform = frameTransformTree.transform("baz", "foo"); + private void checkBazToFooTransform(FrameTransform frameTransform) { // If we were to reverse the order of the transforms in our implementation, // we would expect the translation vector to be <0, 0, 1> instead. - Vector3 vector = new Vector3(0, 1, 0); - Quaternion quaternion = new Quaternion(Math.sqrt(0.5), 0, 0, Math.sqrt(0.5)); - Transform transform = new Transform(vector, quaternion); + Transform transform = Transform.translation(0, 1, 0).multiply(Transform.xRotation(Math.PI / 2)); + Quaternion rotationAndScale = transform.getRotationAndScale(); + assertTrue(String.format("%s is not neutral.", rotationAndScale), + rotationAndScale.isAlmostNeutral(1e-9)); assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame()); assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame()); - assertEquals(transform, frameTransform.getTransform()); + assertTrue(transform.almostEquals(frameTransform.getTransform(), 1e-9)); + } + + @Test + public void testTransformBazToRoot() { + updateFrameTransformTree(); + checkBazToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("baz"))); + } + + @Test + public void testTransformBazToFoo() { + updateFrameTransformTree(); + checkBazToFooTransform(frameTransformTree.transform("baz", "foo")); + checkBazToFooTransform(frameTransformTree.transform("foo", "baz").invert()); + } + + private void checkFuzToFooTransform(FrameTransform frameTransform) { + // If we were to reverse the order of the transforms in our implementation, + // we would expect the translation vector to be <0, 0, 1> instead. + Transform transform = Transform.translation(1, 0, 0).multiply(Transform.yRotation(Math.PI / 2)); + Quaternion rotationAndScale = transform.getRotationAndScale(); + assertTrue(String.format("%s is not neutral.", rotationAndScale), + rotationAndScale.isAlmostNeutral(1e-9)); + assertEquals(nameResolver.resolve("fuz"), frameTransform.getSourceFrame()); + assertEquals(nameResolver.resolve("foo"), frameTransform.getTargetFrame()); + assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()), + transform.almostEquals(frameTransform.getTransform(), 1e-9)); + } + + @Test + public void testTransformFuzToRoot() { + updateFrameTransformTree(); + checkFuzToFooTransform(frameTransformTree.transformToRoot(nameResolver.resolve("fuz"))); + } + + @Test + public void testTransformFuzToFoo() { + updateFrameTransformTree(); + checkFuzToFooTransform(frameTransformTree.transform("fuz", "foo")); + checkFuzToFooTransform(frameTransformTree.transform("foo", "fuz").invert()); + } + + @Test + public void testTransformBazToFuz() { + updateFrameTransformTree(); + FrameTransform frameTransform = frameTransformTree.transform("baz", "fuz"); + Transform transform = + Transform.yRotation(Math.PI / 2).invert().multiply(Transform.translation(1, 0, 0).invert()) + .multiply(Transform.translation(0, 1, 0)).multiply(Transform.xRotation(Math.PI / 2)); + assertTrue(transform.getRotationAndScale().isAlmostNeutral(1e-9)); + assertEquals(nameResolver.resolve("baz"), frameTransform.getSourceFrame()); + assertEquals(nameResolver.resolve("fuz"), frameTransform.getTargetFrame()); + assertTrue(String.format("Expected %s != %s", transform, frameTransform.getTransform()), + transform.almostEquals(frameTransform.getTransform(), 1e-9)); } @Test diff --git a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java index f5bbd4b0518469db7cc7ebf99ed67105aa7af910..8373829cd7abe099483a645636d584c6d1f09a23 100644 --- a/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java +++ b/rosjava_geometry/src/test/java/org/ros/rosjava_geometry/TransformTest.java @@ -17,9 +17,12 @@ package org.ros.rosjava_geometry; import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; import org.junit.Test; +import java.util.Random; + /** * @author damonkohler@google.com (Damon Kohler) */ @@ -27,52 +30,60 @@ public class TransformTest { @Test public void testMultiply() { - Transform transform1 = new Transform(new Vector3(1, 0, 0), new Quaternion(0, 0, 0, 1)); + Transform transform1 = new Transform(Vector3.xAxis(), Quaternion.identity()); Transform transform2 = - new Transform(new Vector3(0, 1, 0), Quaternion.fromAxisAngle(new Vector3(0, 0, 1), - Math.PI / 2)); + new Transform(Vector3.yAxis(), Quaternion.fromAxisAngle(Vector3.zAxis(), Math.PI / 2)); Transform result1 = transform1.multiply(transform2); assertEquals(1.0, result1.getTranslation().getX(), 1e-9); assertEquals(1.0, result1.getTranslation().getY(), 1e-9); assertEquals(0.0, result1.getTranslation().getZ(), 1e-9); - assertEquals(0.0, result1.getRotation().getX(), 1e-9); - assertEquals(0.0, result1.getRotation().getY(), 1e-9); - assertEquals(0.7071067811865475, result1.getRotation().getZ(), 1e-9); - assertEquals(0.7071067811865475, result1.getRotation().getW(), 1e-9); + assertEquals(0.0, result1.getRotationAndScale().getX(), 1e-9); + assertEquals(0.0, result1.getRotationAndScale().getY(), 1e-9); + assertEquals(0.7071067811865475, result1.getRotationAndScale().getZ(), 1e-9); + assertEquals(0.7071067811865475, result1.getRotationAndScale().getW(), 1e-9); Transform result2 = transform2.multiply(transform1); assertEquals(0.0, result2.getTranslation().getX(), 1e-9); assertEquals(2.0, result2.getTranslation().getY(), 1e-9); assertEquals(0.0, result2.getTranslation().getZ(), 1e-9); - assertEquals(0.0, result2.getRotation().getX(), 1e-9); - assertEquals(0.0, result2.getRotation().getY(), 1e-9); - assertEquals(0.7071067811865475, result2.getRotation().getZ(), 1e-9); - assertEquals(0.7071067811865475, result2.getRotation().getW(), 1e-9); + assertEquals(0.0, result2.getRotationAndScale().getX(), 1e-9); + assertEquals(0.0, result2.getRotationAndScale().getY(), 1e-9); + assertEquals(0.7071067811865475, result2.getRotationAndScale().getZ(), 1e-9); + assertEquals(0.7071067811865475, result2.getRotationAndScale().getW(), 1e-9); } @Test public void testInvert() { Transform transform = - new Transform(new Vector3(0, 1, 0), Quaternion.fromAxisAngle(new Vector3(0, 0, 1), - Math.PI / 2)); - Transform transformInverse = transform.invert(); + new Transform(Vector3.yAxis(), Quaternion.fromAxisAngle(Vector3.zAxis(), Math.PI / 2)); + Transform inverse = transform.invert(); + + assertEquals(-1.0, inverse.getTranslation().getX(), 1e-9); + assertEquals(0.0, inverse.getTranslation().getY(), 1e-9); + assertEquals(0.0, inverse.getTranslation().getZ(), 1e-9); + assertEquals(0.0, inverse.getRotationAndScale().getX(), 1e-9); + assertEquals(0.0, inverse.getRotationAndScale().getY(), 1e-9); + assertEquals(-0.7071067811865475, inverse.getRotationAndScale().getZ(), 1e-9); + assertEquals(0.7071067811865475, inverse.getRotationAndScale().getW(), 1e-9); - assertEquals(-1.0, transformInverse.getTranslation().getX(), 1e-9); - assertEquals(0.0, transformInverse.getTranslation().getY(), 1e-9); - assertEquals(0.0, transformInverse.getTranslation().getZ(), 1e-9); - assertEquals(0.0, transformInverse.getRotation().getX(), 1e-9); - assertEquals(0.0, transformInverse.getRotation().getY(), 1e-9); - assertEquals(-0.7071067811865475, transformInverse.getRotation().getZ(), 1e-9); - assertEquals(0.7071067811865475, transformInverse.getRotation().getW(), 1e-9); + Transform neutral = transform.multiply(inverse); + assertTrue(neutral.almostEquals(Transform.identity(), 1e-9)); + } - Transform neutral = transform.multiply(transformInverse); - assertEquals(0.0, neutral.getTranslation().getX(), 1e-9); - assertEquals(0.0, neutral.getTranslation().getY(), 1e-9); - assertEquals(0.0, neutral.getTranslation().getZ(), 1e-9); - assertEquals(0.0, neutral.getRotation().getX(), 1e-9); - assertEquals(0.0, neutral.getRotation().getY(), 1e-9); - assertEquals(0.0, neutral.getRotation().getZ(), 1e-9); - assertEquals(1.0, neutral.getRotation().getW(), 1e-9); + @Test + public void testInvertRandom() { + Random random = new Random(); + random.setSeed(42); + for (int i = 0; i < 10000; i++) { + Vector3 vector = new Vector3(random.nextDouble(), random.nextDouble(), random.nextDouble()); + Quaternion quaternion = + new Quaternion(random.nextDouble(), random.nextDouble(), random.nextDouble(), + random.nextDouble()); + Transform transform = new Transform(vector, quaternion); + Transform inverse = transform.invert(); + Transform neutral = transform.multiply(inverse); + assertTrue(neutral.almostEquals(Transform.identity(), 1e-9)); + } } }