/** * Sets the z-coordinate of this shape position. * * @param z the new z-coordinate for this shape. */ public final void setPositionZ(double z) { shapePose.setTranslationZ(z); }
public void getTransform(RigidBodyTransform transformToPack) { transformToPack.setRotation(orientation); transformToPack.setTranslationX(stateVector.get(positionStart + 0)); transformToPack.setTranslationY(stateVector.get(positionStart + 1)); transformToPack.setTranslationZ(stateVector.get(positionStart + 2)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testVerticalTranslation() { FootstepNode nodeToSnap = new FootstepNode(2.5, -0.5); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setTranslationZ(-1.0); doAFullFootholdTest(transformToWorld, nodeToSnap); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimpleTranslationAndRotation() { FootstepNode nodeToSnap = new FootstepNode(1.1, 0.0); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setRotation(new AxisAngle(0.0, 1.0, 0.0, 0.25 * Math.PI)); transformToWorld.setTranslationZ(-1.0); doAFullFootholdTest(transformToWorld, nodeToSnap); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { poseOne.setToZero(frameOne); poseTwo.setToZero(frameTwo); poseOne.changeFrame(worldFrame); poseTwo.changeFrame(worldFrame); framePose.interpolate(poseOne, poseTwo, 0.5); transformToParent.setIdentity(); transformToParent.setRotationYawAndZeroTranslation(framePose.getYaw()); transformToParent.setTranslation(framePose.getPosition()); transformToParent.setTranslationZ(Math.min(poseOne.getZ(), poseTwo.getZ())); } }
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointTransform) { jointTransform.setRotationToZero(); jointTransform.setTranslationX(getQ() * getJointAxis().getX()); jointTransform.setTranslationY(getQ() * getJointAxis().getY()); jointTransform.setTranslationZ(getQ() * getJointAxis().getZ()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomFullFootholds() { int numTests = 100; RigidBodyTransform nodeToWorldTransform = new RigidBodyTransform(); for (int i = 0; i < numTests; i++) { FootstepNode node = FootstepNode.generateRandomFootstepNode(random, 5.0); FootstepNodeTools.getNodeTransform(node, nodeToWorldTransform); RigidBodyTransform regionToWorld = new RigidBodyTransform(nodeToWorldTransform); double xRotation = EuclidCoreRandomTools.nextDouble(random, 0.15 * Math.PI); double yRotation = EuclidCoreRandomTools.nextDouble(random, 0.15 * Math.PI); regionToWorld.setRotationEuler(xRotation, yRotation, 0.0); regionToWorld.setTranslationZ(EuclidCoreRandomTools.nextDouble(random, 2.0)); doAFullFootholdTest(regionToWorld, node); } }
snapTransform1.setTranslationZ(parameters.getMaximumStepZ() + 1.0e-10); snapper.addSnapData(node0, new FootstepNodeSnapData(snapTransform0)); snapper.addSnapData(node1, new FootstepNodeSnapData(snapTransform1)); snapTransform2.setTranslationZ(-parameters.getMaximumStepZ() - 1.0e-10); snapper.addSnapData(node2, new FootstepNodeSnapData(snapTransform2)); Assert.assertFalse(checker.isNodeValid(node0, node2));
private void processFootMesh(FootstepNode node, FootstepNodeSnapData snapData, boolean valid) { meshBuilder.clear(); RigidBodyTransform planarTransformToWorld = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(node, planarTransformToWorld); RigidBodyTransform snappedTransformToWorld = new RigidBodyTransform(); ConvexPolygon2D foothold = snapData.getCroppedFoothold(); FootstepNodeTools.getSnappedNodeTransform(node, snapData.getSnapTransform(), snappedTransformToWorld); snappedTransformToWorld.appendTranslation(0.0, 0.0, 0.01); planarTransformToWorld.setTranslationZ(snappedTransformToWorld.getTranslationZ() + 0.1); Color regionColor = valid ? Color.GREEN : Color.RED; regionColor = Color.hsb(regionColor.getHue(), 0.9, 1.0); Point2D[] vertices = new Point2D[foothold.getNumberOfVertices()]; for (int j = 0; j < vertices.length; j++) { vertices[j] = new Point2D(foothold.getVertex(j)); } meshBuilder.addMultiLine(snappedTransformToWorld, vertices, 0.01, regionColor, true); meshBuilder.addPolygon(snappedTransformToWorld, foothold, regionColor); // TODO add mesh of planar footstep meshView.setOpacity(0.9); meshView.setMesh(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSimplePartialFoothold() { FootstepNode nodeToSnap = new FootstepNode(1.0, 0.0); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setRotation(new AxisAngle(0.0, 1.0, 0.0, 0.25 * Math.PI)); transformToWorld.setTranslationZ(-1.0); ConvexPolygon2D partialFootholdPolygon = new ConvexPolygon2D(footPolygons.get(RobotSide.LEFT)); partialFootholdPolygon.scale(0.5); partialFootholdPolygon.translate(Math.sqrt(2.0), 0.0); PlanarRegion planarRegion = new PlanarRegion(transformToWorld, partialFootholdPolygon); PlanarRegionsList planarRegionsList = new PlanarRegionsList(planarRegion); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(nodeToSnap); if(visualize) { visualizer.addPlanarRegionsList(planarRegionsList, YoAppearance.AliceBlue()); visualizer.setSnappedPolygon(nodeTransform, snapData.getSnapTransform(), snapData.getCroppedFoothold()); ThreadTools.sleepForever(); } assertEquals(snapData.getCroppedFoothold().getArea(), partialFootholdPolygon.getArea(), epsilon); PlanarRegionPolygonSnapperTest.assertSurfaceNormalsMatchAndSnapPreservesXFromAbove(snapData.getSnapTransform(), transformToWorld); }
actualTransform.setTranslationX(expectedTranslation.getX()); actualTransform.setTranslationY(expectedTranslation.getY()); actualTransform.setTranslationZ(expectedTranslation.getZ()); assertTrue(actualTransform.hasRotation()); assertTrue(actualTransform.hasTranslation()); actualTransform.setTranslationZ(0.0); assertFalse(actualTransform.hasTranslation()); assertFalse(actualTransform.hasTranslation()); actualTransform.setTranslationZ(1.0); assertTrue(actualTransform.hasTranslation()); actualTransform.setTranslationZ(0.0); assertFalse(actualTransform.hasTranslation());