@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationEulerAndZeroTranslation(Math.toRadians(90), 0, 0); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationEulerAndZeroTranslation(Math.toRadians(90), 0, 0); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationEulerAndZeroTranslation(Math.toRadians(-90), 0, 0); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationEulerAndZeroTranslation(Math.toRadians(-90), Math.toRadians(0), Math.toRadians(0)); } };
public void rotate(double rotationAngle, Axis axis) { switch (axis) { case X: { tempTransform.setRotationEulerAndZeroTranslation(rotationAngle, 0.0, 0.0); break; } case Y: { tempTransform.setRotationEulerAndZeroTranslation(0.0, rotationAngle, 0.0); break; } case Z: { tempTransform.setRotationEulerAndZeroTranslation(0.0, 0.0, rotationAngle); break; } } transform.set(transform); transform.multiply(tempTransform); }
public void rotateEuler(double rotateX, double rotateY, double rotateZ) { tempTransform.setRotationEulerAndZeroTranslation(rotateX, rotateY, rotateZ); transform.set(transform); transform.multiply(tempTransform); }
public void rotateEuler(Vector3DReadOnly eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); transform.set(transform); transform.multiply(tempTransform); }
private RigidBodyTransform[] createRandomCorrectionTargets(int numTargets) { RigidBodyTransform[] targets = new RigidBodyTransform[numTargets]; for (int i = 0; i < numTargets; i++) { targets[i] = new RigidBodyTransform(); targets[i].setRotationEulerAndZeroTranslation(0, 0, random.nextDouble() * 2.0 * Math.PI); targets[i].setTranslation(RandomGeometry.nextVector3D(random, 1.0)); } return targets; }
public void translateThenRotateEuler(Vector3D translationVector, Vector3D eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); tempTransform.setTranslation(translationVector); transform.set(transform); transform.multiply(tempTransform); }
void trainSingleDetector(double yaw, double pitch, double roll, double distance) { OrganizedPointCloud cloud = renderCloud(yaw, pitch, roll, distance); int[] mask = makeMaskByZThresholing(cloud, 1.5f); writeMask(mask, cloud.width, cloud.height); LineModTemplate template = LineModInterface.trainTemplateBytes(cloud, mask); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationEulerAndZeroTranslation(roll, pitch, yaw); transform.setTranslation(0, 0, distance); template.transform = transform; byteFeatures.add(template); }
public static CombinedTerrainObject3D addValveTextureBox(Vector3D position, double yaw) { YoAppearanceTexture valveTexture = new YoAppearanceTexture("/images/red-valve.jpg"); double boxSideLength = 1.0; CombinedTerrainObject3D valveTerrainObject = new CombinedTerrainObject3D("ValveBox"); RigidBodyTransform location = new RigidBodyTransform(); location.setRotationEulerAndZeroTranslation(Math.toRadians(90.0), 0.0, yaw - Math.toRadians(90.0)); location.setTranslation(position); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, boxSideLength, boxSideLength, boxSideLength), valveTexture); valveTerrainObject.addTerrainObject(newBox); return valveTerrainObject; }
public static CombinedTerrainObject3D addFiducial(Vector3D position, double yaw, Fiducial fiducial) { YoAppearanceTexture fiducialTexture = new YoAppearanceTexture(fiducial.getPathString()); double boxSideLength = 1.0; CombinedTerrainObject3D fiducualTerrainObject = new CombinedTerrainObject3D(fiducial.name()); RigidBodyTransform location = new RigidBodyTransform(); location.setRotationEulerAndZeroTranslation(Math.toRadians(90.0), 0.0, yaw - Math.toRadians(90.0)); location.setTranslation(position); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, boxSideLength, boxSideLength, boxSideLength), fiducialTexture); fiducualTerrainObject.addTerrainObject(newBox); return fiducualTerrainObject; }
@Test(expected = RuntimeException.class) public void testApplyTransformRotationXaxisException() { Random random = new Random(1776L); RigidBodyTransform transform = new RigidBodyTransform(); Point2D firstPointOnLine = randomPoint(random); Point2D secondPointOnLine = randomPoint(random); // pure translation: Vector3D translation = new Vector3D(0.0, 0.0, 0.0); Vector3D eulerAngles = new Vector3D(randomDouble(random, 2.0 * Math.PI), 0.0, 0.0); transform.setRotationEulerAndZeroTranslation(eulerAngles); transform.setTranslation(translation); Line2D line = new Line2D(firstPointOnLine, secondPointOnLine); line.applyTransform(transform); }
@Test(expected = RuntimeException.class) public void testApplyTransformRotationYaxisException() { Random random = new Random(1776L); RigidBodyTransform transform = new RigidBodyTransform(); Point2D firstPointOnLine = randomPoint(random); Point2D secondPointOnLine = randomPoint(random); // pure translation: Vector3D translation = new Vector3D(0.0, 0.0, 0.0); Vector3D eulerAngles = new Vector3D(0.0, randomDouble(random, 2.0 * Math.PI), 0.0); transform.setRotationEulerAndZeroTranslation(eulerAngles); transform.setTranslation(translation); Line2D line = new Line2D(firstPointOnLine, secondPointOnLine); line.applyTransform(transform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame); nonZUpToWorld.getRotation(nonZUpToWorldRotation); double yaw = nonZUpToWorldRotation.getYaw(); euler.set(0.0, 0.0, yaw); transformToParent.setRotationEulerAndZeroTranslation(euler); originPoint3d.set(origin); nonZUpToWorld.transform(originPoint3d); translation.set(originPoint3d); transformToParent.setTranslation(translation); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testTranslateThenRotateTransformGeneration() { RigidBodyTransformGenerator generator = new RigidBodyTransformGenerator(); generator.translate(3.3, 4.4, 5.5); generator.rotateEuler(new Vector3D(0.67, 0.89, 0.34)); RigidBodyTransform expectedTransform = new RigidBodyTransform(); expectedTransform.setRotationEulerAndZeroTranslation(0.67, 0.89, 0.34); expectedTransform.setTranslation(3.3, 4.4, 5.5); RigidBodyTransform transform = generator.getRigidBodyTransformCopy(); assertTrue(transform.epsilonEquals(expectedTransform, 1e-10)); }
@Test public void testApplyTransformTranslation() { Random random = new Random(1776L); double delta = 1.0e-5; RigidBodyTransform transform = new RigidBodyTransform(); // pure translation: Vector3D translation = new Vector3D(random.nextDouble(), random.nextDouble(), 0.0); Vector3D eulerAngles = new Vector3D(0.0, 0.0, 0.0); transform.setRotationEulerAndZeroTranslation(eulerAngles); transform.setTranslation(translation); Point2D firstPointOnLine = randomPoint(random); Point2D secondPointOnLine = randomPoint(random); Line2D line = new Line2D(firstPointOnLine, secondPointOnLine); Point2D point = new Point2D(line.getPoint()); Vector2D vector = new Vector2D(line.getDirection()); line.applyTransform(transform); assertEquals("pure translation failed", point.getX() + translation.getX(), line.getPointX(), delta); assertEquals("pure translation failed", point.getY() + translation.getY(), line.getPointY(), delta); assertEquals("pure translation failed", vector.getX(), line.getDirectionX(), delta); assertEquals("pure translation failed", vector.getY(), line.getDirectionY(), delta); }
@Test public void testApplyTransformCombination() { Random random = new Random(1776L); double delta = 1.0e-5; RigidBodyTransform transform = new RigidBodyTransform(); Point2D firstPointOnLine = randomPoint(random); Point2D secondPointOnLine = randomPoint(random); // pure translation: Vector3D translation = new Vector3D(randomDouble(random), randomDouble(random), 0.0); Vector3D eulerAngles = new Vector3D(0.0, 0.0, randomDouble(random, 2.0 * Math.PI)); transform.setRotationEulerAndZeroTranslation(eulerAngles); transform.setTranslation(translation); Line2D line = new Line2D(firstPointOnLine, secondPointOnLine); Point2D point = new Point2D(line.getPoint()); Vector2D vector = new Vector2D(line.getDirection()); line.applyTransform(transform); double alpha = eulerAngles.getZ(); double sina = Math.sin(alpha); double cosa = Math.cos(alpha); assertEquals("pure rotation failed", point.getX() * cosa - point.getY() * sina + translation.getX(), line.getPointX(), delta); assertEquals("pure rotation failed", point.getX() * sina + point.getY() * cosa + translation.getY(), line.getPointY(), delta); assertEquals("pure rotation failed", vector.getX() * cosa - vector.getY() * sina, line.getDirectionX(), delta); assertEquals("pure rotation failed", vector.getX() * sina + vector.getY() * cosa, line.getDirectionY(), delta); }
@Test// timeout = 30000 public void testBoundingBox() { double radius = 0.5; double height = 0.1; CylinderShapeDescription<?> cylinder = new CylinderShapeDescription<>(radius, height); BoundingBox3D boundingBox = new BoundingBox3D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); cylinder.getBoundingBox(boundingBox); Point3D minimumPoint = new Point3D(); boundingBox.getMinPoint(minimumPoint); Point3D maximumPoint = new Point3D(); boundingBox.getMaxPoint(maximumPoint); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(-0.5, -0.5, -0.05), minimumPoint, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(0.5, 0.5, 0.05), maximumPoint, 1e-10); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationEulerAndZeroTranslation(Math.PI / 4.0, 0.0, 0.0); cylinder.applyTransform(transform); cylinder.getBoundingBox(boundingBox); boundingBox.getMinPoint(minimumPoint); boundingBox.getMaxPoint(maximumPoint); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(-0.5, -0.38890872965260115, -0.3889087296526011), minimumPoint, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(0.5, 0.38890872965260115, 0.3889087296526011), maximumPoint, 1e-10); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testShiftInSoleFrame() { Vector3D soleTranslation = new Vector3D(); double yaw = 0.0; RigidBodyTransform soleTransform = new RigidBodyTransform(new AxisAngle(0.0, 0.0, 1.0, yaw), soleTranslation); FootstepNode node = new FootstepNode(soleTranslation.getX(), soleTranslation.getY(), yaw, RobotSide.LEFT); Vector2D shiftVector = new Vector2D(1.0, 2.0); RigidBodyTransform shiftedSoleTransform = FootstepNodeTools.shiftInSoleFrame(shiftVector, soleTransform); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(1.0, 2.0, 0.0), shiftedSoleTransform.getTranslationVector(), 1e-7); assertTrue(MathTools.epsilonEquals(node.getYaw(), yaw, 1e-7)); soleTranslation = new Vector3D(); yaw = Math.PI/2.0; soleTransform = new RigidBodyTransform(new AxisAngle(0.0, 0.0, 1.0, yaw), soleTranslation); soleTransform.setRotationEulerAndZeroTranslation(0.0, 0.0, Math.PI/2.0); node = new FootstepNode(soleTranslation.getX(), soleTranslation.getY(), yaw, RobotSide.LEFT); shiftVector = new Vector2D(1.0, 2.0); shiftedSoleTransform = FootstepNodeTools.shiftInSoleFrame(shiftVector, soleTransform); EuclidCoreTestTools.assertTuple3DEquals(new Point3D(-2.0, 1.0, 0.0), shiftedSoleTransform.getTranslationVector(), 1e-7); assertTrue(MathTools.epsilonEquals(node.getYaw(), yaw, 1e-7)); } }