@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setTranslationAndIdentityRotation(initialRecursionStep.centerOfMass); } };
/** {@inheritDoc} */ @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setTranslationAndIdentityRotation(centerOfMassJacobian.getCenterOfMass()); }
public void translate(Tuple3DReadOnly translationVector) { tempTransform.setTranslationAndIdentityRotation(translationVector); transform.set(transform); transform.multiply(tempTransform); }
public void translate(double x, double y, double z) { tempTransform.setTranslationAndIdentityRotation(x, y, z); transform.set(transform); transform.multiply(tempTransform); }
public static RigidBodyTransform poseToTransform(String pose) { RigidBodyTransform ret = new RigidBodyTransform(); if(pose == null) { return ret; } pose = pose.trim(); String[] data = pose.split("\\s+"); RigidBodyTransform translation = new RigidBodyTransform(); Vector3D translationVector = new Vector3D(); translationVector.setX(Double.parseDouble(data[0])); translationVector.setY(Double.parseDouble(data[1])); translationVector.setZ(Double.parseDouble(data[2])); translation.setTranslationAndIdentityRotation(translationVector); RigidBodyTransform rotation = new RigidBodyTransform(); Vector3D eulerAngels = new Vector3D(); eulerAngels.setX(Double.parseDouble(data[3])); eulerAngels.setY(Double.parseDouble(data[4])); eulerAngels.setZ(Double.parseDouble(data[5])); rotation.setRotationEulerAndZeroTranslation(eulerAngels); ret.set(translation); ret.multiply(rotation); return ret; }
initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.05, 0.0); initialFoot.applyTransform(initialFootTransform, false);
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }
@Test public void testSurfaceNormalAt_above_translated() { double height = 2.0; double radius = 1.0; RigidBodyTransform transform = new RigidBodyTransform(); double tx = 1.5, ty = 1, tz = 2; Vector3D translation = new Vector3D(tx, ty, tz); transform.setTranslationAndIdentityRotation(translation); Cylinder3D cylinder3d = new Cylinder3D(transform, height, radius); Vector3D normalToPack = new Vector3D(); Point3D closestPointToPack = new Point3D(); Point3D pointToCheck = new Point3D(tx, ty, tz + 3.0); Vector3D expectedNormal = new Vector3D(0, 0, 1.0); cylinder3d.checkIfInside(pointToCheck, closestPointToPack, normalToPack); assertEquals(expectedNormal, normalToPack); }
@Test public void testCheckInside() { double height = 2.0; double radius = 1.0; RigidBodyTransform transform = new RigidBodyTransform(); double tx = 1.5, ty = 1, tz = 2; Vector3D translation = new Vector3D(tx, ty, tz); transform.setTranslationAndIdentityRotation(translation); Cylinder3D cylinder3d = new Cylinder3D(transform, height, radius); Vector3D normalToPack = new Vector3D(); Point3D closestPointToPack = new Point3D(); Point3D pointToCheck = new Point3D(tx, ty, tz + 3.0); Vector3D expectedNormal = new Vector3D(0, 0, 1.0); boolean isInside = cylinder3d.checkIfInside(pointToCheck, closestPointToPack, normalToPack); EuclidCoreTestTools.assertTuple3DEquals(expectedNormal, normalToPack, 1e-7); assertFalse(isInside); pointToCheck.set(tx, ty, tz + height / 2.0 - 0.011); isInside = cylinder3d.checkIfInside(pointToCheck, closestPointToPack, normalToPack); assertTrue(isInside); EuclidCoreTestTools.assertTuple3DEquals(expectedNormal, normalToPack, 1e-7); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectionTranslationLimits() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(-0.2, 0.25, 0.0); initialFoot.applyTransform(initialFootTransform, false); WiggleParameters parameters = new WiggleParameters(); parameters.maxX = 0.1; ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, parameters); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } assertTrue(foot == null); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testProjectionOnlyTranslation() { ConvexPolygon2D plane = new ConvexPolygon2D(); plane.addVertex(0.0, 0.0); plane.addVertex(0.5, 0.0); plane.addVertex(0.0, 0.5); plane.addVertex(0.5, 0.5); plane.update(); ConvexPolygon2D initialFoot = PlannerTools.createDefaultFootPolygon(); RigidBodyTransform initialFootTransform = new RigidBodyTransform(); initialFootTransform.setTranslationAndIdentityRotation(-0.2, 0.25, 0.0); initialFoot.applyTransform(initialFootTransform, false); ConvexPolygon2D foot = PolygonWiggler.wigglePolygon(initialFoot, plane, new WiggleParameters()); if (visualize) { addPolygonToArtifacts("Plane", plane, Color.BLACK); addPolygonToArtifacts("InitialFoot", initialFoot, Color.RED); addPolygonToArtifacts("Foot", foot, Color.BLUE); showPlotterAndSleep(artifacts); } assertTrue(ConvexPolygon2dCalculator.isPolygonInside(foot, 1.0e-5, plane)); }
initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.0, 0.0); initialFoot.applyTransform(initialFootTransform, false);
transform3D.setTranslationAndIdentityRotation(translation);
initialFootTransform.setTranslationAndIdentityRotation(1.0, 1.5, 0.0); initialFoot.applyTransform(initialFootTransform, false);
initialFootTransform.setTranslationAndIdentityRotation(1.0, 1.5, 0.0); initialFoot.applyTransform(initialFootTransform, false);
initialFootTransform.setTranslationAndIdentityRotation(0.5, 0.0, 0.0); initialFoot.applyTransform(initialFootTransform, false);
toOriginTransform.setTranslationAndIdentityRotation(offset); rotationMatrix.transform(translation); RigidBodyTransform translationTransform = new RigidBodyTransform(); translationTransform.setTranslationAndIdentityRotation(translation); fullTransform.multiply(translationTransform);
actualTransform.setTranslationAndIdentityRotation(expectedTranslation); assertFalse(actualTransform.hasRotation()); assertTrue(actualTransform.hasTranslation()); actualTransform.setTranslationAndIdentityRotation(expectedTranslation.getX(), expectedTranslation.getY(), expectedTranslation.getZ()); assertFalse(actualTransform.hasRotation()); assertTrue(actualTransform.hasTranslation());