@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(GraphName.of("baz"), frameTransform.getSourceFrame()); assertEquals(GraphName.of("fuz"), frameTransform.getTargetFrame()); assertTrue( String.format("Expected %s != %s", transform, frameTransform.getTransform()), transform.almostEquals(frameTransform.getTransform(), 1e-9)); }
public FrameTransform transform(GraphName source, GraphName target) { Preconditions.checkNotNull(source); Preconditions.checkNotNull(target); GraphName relativeSource = source.toRelative(); GraphName relativeTarget = target.toRelative(); if (relativeSource.equals(relativeTarget)) { return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null); } FrameTransform sourceToRoot = transformToRoot(relativeSource); FrameTransform targetToRoot = transformToRoot(relativeTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(relativeSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(relativeTarget)) { return sourceToRoot; } else { return null; } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime()); } return null; }
FrameTransformTree { public FrameTransform transform(GraphName source, GraphName target) { Preconditions.checkNotNull(source); Preconditions.checkNotNull(target); GraphName relativeSource = source.toRelative(); GraphName relativeTarget = target.toRelative(); if (relativeSource.equals(relativeTarget)) { return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null); } FrameTransform sourceToRoot = transformToRoot(relativeSource); FrameTransform targetToRoot = transformToRoot(relativeTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(relativeSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(relativeTarget)) { return sourceToRoot; } else { return null; } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime()); } return null; } }
FrameTransformTree { public FrameTransform transform(GraphName source, GraphName target) { Preconditions.checkNotNull(source); Preconditions.checkNotNull(target); GraphName relativeSource = source.toRelative(); GraphName relativeTarget = target.toRelative(); if (relativeSource.equals(relativeTarget)) { return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null); } FrameTransform sourceToRoot = transformToRoot(relativeSource); FrameTransform targetToRoot = transformToRoot(relativeTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(relativeSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(relativeTarget)) { return sourceToRoot; } else { return null; } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime()); } return null; } FrameTransformTree(); }
FrameTransformTree { public FrameTransform transform(GraphName source, GraphName target) { Preconditions.checkNotNull(source); Preconditions.checkNotNull(target); GraphName relativeSource = source.toRelative(); GraphName relativeTarget = target.toRelative(); if (relativeSource.equals(relativeTarget)) { return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null); } FrameTransform sourceToRoot = transformToRoot(relativeSource); FrameTransform targetToRoot = transformToRoot(relativeTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(relativeSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(relativeTarget)) { return sourceToRoot; } else { return null; } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime()); } return null; } FrameTransformTree(); void update(geometry_msgs.TransformStamped transformStamped); FrameTransform lookUp(GraphName source); FrameTransform get(String source); FrameTransform lookUp(GraphName source, Time time); FrameTransform get(String source, Time time); FrameTransform transform(GraphName source, GraphName target); FrameTransform transform(String source, String target); }
FrameTransformTree { public FrameTransform transform(GraphName source, GraphName target) { Preconditions.checkNotNull(source); Preconditions.checkNotNull(target); GraphName relativeSource = source.toRelative(); GraphName relativeTarget = target.toRelative(); if (relativeSource.equals(relativeTarget)) { return new FrameTransform(Transform.identity(), relativeSource, relativeTarget, null); } FrameTransform sourceToRoot = transformToRoot(relativeSource); FrameTransform targetToRoot = transformToRoot(relativeTarget); if (sourceToRoot == null && targetToRoot == null) { return null; } if (sourceToRoot == null) { if (targetToRoot.getTargetFrame().equals(relativeSource)) { return targetToRoot.invert(); } else { return null; } } if (targetToRoot == null) { if (sourceToRoot.getTargetFrame().equals(relativeTarget)) { return sourceToRoot; } else { return null; } } if (sourceToRoot.getTargetFrame().equals(targetToRoot.getTargetFrame())) { Transform transform = targetToRoot.getTransform().invert().multiply(sourceToRoot.getTransform()); return new FrameTransform(transform, relativeSource, relativeTarget, sourceToRoot.getTime()); } return null; } FrameTransformTree(); void update(geometry_msgs.TransformStamped transformStamped); FrameTransform lookUp(GraphName source); FrameTransform get(String source); FrameTransform lookUp(GraphName source, Time time); FrameTransform get(String source, Time time); FrameTransform transform(GraphName source, GraphName target); FrameTransform transform(String source, String target);  }