public void setYawFromQuaternion(Quaternion quaternion) { setYaw(quaternion.getYaw()); }
@Override public void handle(long now) { if (startRotationEditModeEnabled.get()) startArrow.setMaterial(startTransparentMaterial); else startArrow.setMaterial(startOpaqueMaterial); Point3D startPosition = startPositionReference.get(); if (startPosition != null) { setArrowPose(startArrow, startPosition, startQuaternionReference.get().getYaw()); } if (goalRotationEditModeEnabled.get()) goalArrow.setMaterial(goalTransparentMaterial); else goalArrow.setMaterial(goalOpaqueMaterial); Point3D goalPosition = goalPositionReference.get(); if (goalPosition != null) { setArrowPose(goalArrow, goalPosition, goalQuaternionReference.get().getYaw()); } Point3D lowLevelGoalPosition = lowLevelGoalPositionReference.get(); if (goalPosition != null) { setArrowPose(lowLevelGoalArrow, lowLevelGoalPosition, lowLevelGoalQuaternionReference.get().getYaw()); } }
@Override public void handle(long now) { if(nodeCheckerEnabled == null) return; if(!nodeCheckerEnabled.get()) return; Point3D footPosition = footPositionReference.get(); double footOrientation = footOrientationReference.get().getYaw(); PlanarRegionsList planarRegionsList = planarRegionsReference.get(); if(footPosition == null || planarRegionsList == null) return; FootstepNode node = new FootstepNode(footPosition.getX(), footPosition.getY(), footOrientation, initialSupportSideReference.get()); snapper.setPlanarRegions(planarRegionsList); FootstepNodeSnapData snapData = snapper.snapFootstepNode(node); checker.setPlanarRegions(planarRegionsList); boolean isValid = checker.isNodeValid(node, null); processFootMesh(node, snapData, isValid); }
public double getBodyEstimateYaw() { bodyOrientation.set(bodyCurrentOrientationQx.getDoubleValue(), bodyCurrentOrientationQy.getDoubleValue(), bodyCurrentOrientationQz.getDoubleValue(), bodyCurrentOrientationQs.getDoubleValue()); return bodyOrientation.getYaw(); }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3D planeNormal) { Point3D position = footstep.getLocation(); Quaternion orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = orientation.getYaw(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomGetQuaternionFromYawAndZNormal() { int numTests = 100; Random random = new Random(7362L); Vector3D normal = new Vector3D(); Quaternion quatSolution = new Quaternion(); for (int i = 0; i < numTests; i++) { Quaternion quatToPack = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrixToPack = new RotationMatrix(); double yaw = quatToPack.getYaw(); rotationMatrixToPack.set(quatToPack); rotationMatrixToPack.getColumn(2, normal); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON); assertTrue(quaternionsAreEqual); } }
Assert.assertEquals(polygonPose.getOrientation().getYaw(), -Math.PI / 4, tolerance);
assertEquals(ypr.getYaw(), q.getYaw(), getEpsilon()); assertEquals(ypr.getPitch(), q.getPitch(), getEpsilon()); assertEquals(ypr.getRoll(), q.getRoll(), getEpsilon());
orientation.set(originalFootstepFound.getOrientation()); orientation.getColumn(2, zOrientation); double originalYaw = originalFootstepFound.getOrientation().getYaw();
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3D translation = new Vector3D(); Quaternion rotation = new Quaternion(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2D solePose2d = new FramePose2D(soleFrame, new Point2D(translation.getX(), translation.getY()), rotation.getYaw()); Footstep foot = createFootstep(side, solePose2d); return foot; }
public void testFootstepAndPointsFromDataFile() throws NumberFormatException, InsufficientDataException, IOException { QuadTreeFootstepSnappingParameters snappingParameters = new AtlasFootstepSnappingParameters(); ConvexHullFootstepSnapper footstepSnapper = new ConvexHullFootstepSnapper(new SimpleFootstepValueFunction(snappingParameters), snappingParameters); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); String baseName = "footstepListsForTesting/"; String resourceName = baseName + "DataFromConvexHullSnapper1422988400956.txt"; InputStream resourceAsStream = getClass().getClassLoader().getResourceAsStream(resourceName); FootstepPointsDataReader dataReader = new FootstepPointsDataReader(resourceAsStream); FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.setRobotSide(RobotSide.LEFT.toByte()); FootSpoof spoof = new FootSpoof("basicSpoof"); FramePose2D desiredPose = new FramePose2D(ReferenceFrame.getWorldFrame()); List<Point3D> listOfPoints = new ArrayList<>(); while (dataReader.hasAnotherFootstepAndPoints()) { listOfPoints = dataReader.getNextSetPointsAndFootstep(footstepData); desiredPose.setIncludingFrame(ReferenceFrame.getWorldFrame(), footstepData.getLocation().getX(), footstepData.getLocation().getY(), footstepData.getOrientation().getYaw()); Footstep footstep = footstepSnapper.generateFootstepUsingHeightMap(desiredPose, spoof.getRigidBody(), spoof.getSoleFrame(), RobotSide.fromByte(footstepData.getRobotSide()), listOfPoints, 0.0); assertTrue(footstep.getFootstepType() != Footstep.FootstepType.BAD_FOOTSTEP); } }
double yaw = orientation.getYaw(); addLowerBoundaryPointsToHullPointList(convexHullPointsList, position.getX(), position.getY(), yaw);
quaternion = EuclidCoreRandomTools.nextQuaternion(random); allDoubles = new Pose3D(x, y, z, quaternion.getYaw(), quaternion.getPitch(), quaternion.getRoll()); assertEquals(quaternion.getYaw(), allDoubles.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), allDoubles.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), allDoubles.getRoll(), EPSILON); assertEquals(y, fromComponents.getY(), EPSILON); assertEquals(z, fromComponents.getZ(), EPSILON); assertEquals(quaternion.getYaw(), fromComponents.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), fromComponents.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), fromComponents.getRoll(), EPSILON); assertEquals(y, fromRBT.getY(), EPSILON); assertEquals(z, fromRBT.getZ(), EPSILON); assertEquals(quaternion.getYaw(), fromRBT.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), fromRBT.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), fromRBT.getRoll(), EPSILON);
double yaw = orientation.getYaw();
@Override public Footstep.FootstepType snapFootstep(FootstepDataMessage footstep, HeightMapWithPoints heightMap) { FootstepDataMessage originalFootstepFound = new FootstepDataMessage(footstep); Point3D position = originalFootstepFound.getLocation(); double yaw = originalFootstepFound.getOrientation().getYaw(); if (!useMask) { pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength()); } else { footstepMask.setPositionAndYaw(position.getX(), position.getY(), yaw); pointList = heightMap.getAllPointsWithinArea(position.getX(), position.getY(), parameters.getBoundingSquareSizeLength(), parameters.getBoundingSquareSizeLength(), footstepMask); } double height = heightMap.getHeightAtPoint(position.getX(), position.getY()); return snapFootstep(footstep, pointList, height); }
assertEquals(y, toSet.getY(), EPSILON); assertEquals(z, toSet.getZ(), EPSILON); assertEquals(quaternion.getYaw(), toSet.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), toSet.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), toSet.getRoll(), EPSILON); quaternion = EuclidCoreRandomTools.nextQuaternion(random); toSet.set(x, y, z, quaternion.getYaw(), quaternion.getPitch(), quaternion.getRoll()); assertEquals(quaternion.getYaw(), toSet.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), toSet.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), toSet.getRoll(), EPSILON); assertEquals(y, toSet.getY(), EPSILON); assertEquals(z, toSet.getZ(), EPSILON); assertEquals(quaternion.getYaw(), toSet.getYaw(), EPSILON); assertEquals(quaternion.getPitch(), toSet.getPitch(), EPSILON); assertEquals(quaternion.getRoll(), toSet.getRoll(), EPSILON);
assertEquals(quaternion.getYaw(), orientation.getYaw(), yprEpsilon); assertEquals(quaternion.getPitch(), orientation.getPitch(), yprEpsilon); assertEquals(quaternion.getRoll(), orientation.getRoll(), yprEpsilon);