public ConstantPoseTrajectoryGenerator(String namePrefix, boolean allowMultipleFrames, ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry) { this.allowMultipleFrames = allowMultipleFrames; YoVariableRegistry registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames(namePrefix + "ConstantPosition", registry, referenceFrame); position = yoFramePointInMultipleFrames; YoFrameQuaternionInMultipleFrames yoFrameQuaternionInMultiplesFrames = new YoFrameQuaternionInMultipleFrames(namePrefix + "ConstantOrientation", registry, referenceFrame); orientation = yoFrameQuaternionInMultiplesFrames; multipleFramesHolders = new ArrayList<YoMultipleFramesHolder>(); multipleFramesHolders.add(yoFramePointInMultipleFrames); multipleFramesHolders.add(yoFrameQuaternionInMultiplesFrames); }
public void setInitialPosition(double x, double y, double z) { this.initialPosition.set(x, y, z); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testChangeToRegisteredFrame() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA}); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = new FramePoint3D(worldFrame); Point3D point = RandomGeometry.nextPoint3D(random, 100.0, 100.0, 100.0); yoFramePointInMultipleFrames.set(point); framePoint.set(point); yoFramePointInMultipleFrames.changeFrame(frameA); framePoint.changeFrame(frameA); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); try { yoFramePointInMultipleFrames.changeFrame(frameB); fail("Should have thrown an exception"); } catch (Exception e) { // Good } }
initialPosition = new YoFramePointInMultipleFrames(namePrefix + "InitialPosition", registry, referenceFrame); finalPosition = new YoFramePointInMultipleFrames(namePrefix + "FinalPosition", registry, referenceFrame); currentPosition = new YoFramePointInMultipleFrames(namePrefix + "CurrentPosition", registry, referenceFrame); currentVelocity = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentVelocity", registry, referenceFrame); currentAcceleration = new YoFrameVectorInMultipleFrames(namePrefix + "CurrentAcceleration", registry, referenceFrame); initialPosition.buildUpdatedYoFramePointForVisualizationOnly(), initialOrientationForViz, 0.1); final YoGraphicCoordinateSystem finalPoseViz = new YoGraphicCoordinateSystem(namePrefix + "FinalPose", finalPosition.buildUpdatedYoFramePointForVisualizationOnly(), finalOrientationForViz, 0.1); final YoGraphicCoordinateSystem currentPoseViz = new YoGraphicCoordinateSystem(namePrefix + "CurrentPose", currentPosition.buildUpdatedYoFramePointForVisualizationOnly(), currentOrientationForViz, 0.25); yoGraphicsList = new YoGraphicsList(namePrefix + "StraightLineTrajectory"); yoGraphicsList.add(currentPositionViz);
YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, worldFrame); assertEquals(1, yoFramePointInMultipleFrames.getNumberOfReferenceFramesRegistered()); yoFramePointInMultipleFrames.getRegisteredReferenceFrames(referenceFrames); yoFramePointInMultipleFrames.registerReferenceFrame(worldFrame); yoFramePointInMultipleFrames.registerReferenceFrame(frameA); assertEquals(2, yoFramePointInMultipleFrames.getNumberOfReferenceFramesRegistered()); yoFramePointInMultipleFrames.getRegisteredReferenceFrames(referenceFrames);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSetIncludingFrame() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, new ReferenceFrame[]{worldFrame, frameA}); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = EuclidFrameRandomTools.nextFramePoint3D(random, frameA, -100.0, 100.0, -100.0, 100.0, -100.0, 100.0); yoFramePointInMultipleFrames.setIncludingFrame(framePoint); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); try { yoFramePointInMultipleFrames.setIncludingFrame(new FramePoint3D(frameC)); fail("Should have thrown an exception"); } catch (Exception e) { // Good } } }
public void initialize() { tangentialPlane.update(); currentTrajectoryFrame = initialPosition.getReferenceFrame(); changeFrame(tangentialPlane, false); currentTime.set(0.0); MathTools.checkIfInRange(trajectoryTime.getDoubleValue(), 0.0, Double.POSITIVE_INFINITY); double tIntermediate = leaveTime.getDoubleValue(); xyPolynomial.setCubic(tIntermediate, trajectoryTime.getDoubleValue(), 0.0, 0.0, 1.0, 0.0); double z0 = initialPosition.getZ(); double zIntermediate = initialPosition.getZ() + leaveDistance.getDoubleValue(); double zf = finalPosition.getZ(); zPolynomial.setSexticUsingWaypoint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), z0, 0.0, 0.0, zIntermediate, zf, 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.setToZero(); currentAcceleration.setToZero(); changeFrame(currentTrajectoryFrame, false); if (visualize) visualizeTrajectory(); }
YoFramePointInMultipleFrames entryConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "EntryCMP" + i, parentRegistry, framesToRegister); entryConstantCMP.setToNaN(); entryCMPs.add(entryConstantCMP); entryCMPsInWorldFrameReadOnly.add(entryConstantCMP.buildUpdatedYoFramePointForVisualizationOnly()); YoFramePointInMultipleFrames exitConstantCMP = new YoFramePointInMultipleFrames(namePrefix + "ExitCMP" + i, parentRegistry, framesToRegister); exitConstantCMP.setToNaN(); exitCMPs.add(exitConstantCMP); exitCMPsInWorldFrameReadOnly.add(exitConstantCMP.buildUpdatedYoFramePointForVisualizationOnly());
public YoFramePoint buildUpdatedYoFramePointForVisualizationOnly() { if (yoFramePointInWorld == null) { final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); if (!isReferenceFrameRegistered(worldFrame)) registerReferenceFrame(worldFrame); yoFramePointInWorld = new YoFramePoint(namePrefix, worldFrame.getName(), worldFrame, registry); attachVariableChangedListener(new VariableChangedListener() { private final FramePoint localFramePoint = new FramePoint(); private final YoFramePoint point = yoFramePointInWorld; @Override public void variableChanged(YoVariable<?> v) { getFrameTupleIncludingFrame(localFramePoint); point.setAndMatchFrame(localFramePoint); } }); } return yoFramePointInWorld; }
@Override public String toString() { String ret = ""; ReferenceFrame currentFrame = initialPosition.getReferenceFrame(); ret += "Current time: " + currentTime.getDoubleValue() + ", trajectory time: " + trajectoryTime.getDoubleValue(); ret += "\nCurrent position: " + currentPosition.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent velocity: " + currentVelocity.toStringForASingleReferenceFrame(currentFrame); ret += "\nCurrent acceleration: " + currentAcceleration.toStringForASingleReferenceFrame(currentFrame); return ret; } }
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
public void setInitialPose(FramePose initialPose) { initialPose.getPoseIncludingFrame(tempPosition, tempOrientation); tempPosition.changeFrame(initialPosition.getReferenceFrame()); initialPosition.set(tempPosition); tempOrientation.changeFrame(initialOrientation.getReferenceFrame()); initialOrientation.set(tempOrientation); initialOrientationForViz.setAndMatchFrame(tempOrientation); }
@Override public void getPosition(FramePoint positionToPack) { currentPosition.getFrameTupleIncludingFrame(positionToPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testSetToZero() { YoVariableRegistry registry = new YoVariableRegistry("youhou"); YoFramePointInMultipleFrames yoFramePointInMultipleFrames = new YoFramePointInMultipleFrames("blop", registry, allFrames); yoFramePointInMultipleFrames.switchCurrentReferenceFrame(worldFrame); FramePoint3D framePoint = new FramePoint3D(worldFrame); framePoint.setToZero(worldFrame); assertTrue(framePoint.epsilonEquals(yoFramePointInMultipleFrames, 1e-10)); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(point); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint.setIncludingFrame(currentReferenceFrame, point); framePoint.changeFrame(desiredFrame); framePoint.get(point); set(point); }
List<YoFramePoint> exitCMPs = referenceCMPsCalculator.getExitCMPs(); switchCornerPointsToWorldFrame(); singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame); singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame); singleSupportInitialICP.setIncludingFrame(actualICPToHold); singleSupportFinalICP.setIncludingFrame(actualICPToHold); singleSupportInitialICPVelocity.set(0.0, 0.0, 0.0); setCornerPointsToNaN(); singleSupportInitialICP.setIncludingFrame(entryCMPs.get(0)); singleSupportFinalICP.setIncludingFrame(singleSupportInitialICP); singleSupportInitialICPVelocity.set(0.0, 0.0, 0.0); setCornerPointsToNaN(); swingTimeSplitFraction.getDoubleValue(), transferTimeSplitFraction.getDoubleValue(), omega0); computeDesiredCapturePointPosition(omega0, timeToSpendOnExitCMPBeforeNextDoubleSupport, exitCornerPoints.get(1), exitCMPs.get(1), singleSupportFinalICP); exitCornerPoints.get(0).changeFrame(initialFrame); exitCornerPoints.get(1).changeFrame(finalFrame); entryCornerPoints.get(0).changeFrame(initialFrame); entryCornerPoints.get(1).changeFrame(finalFrame); changeFrameOfRemainingCornerPoints(2, worldFrame); isHoldingPosition.set(false); singleSupportInitialICP.changeFrame(finalFrame); singleSupportFinalICP.changeFrame(worldFrame);
entryCMPs.get(cmpIndex).setToNaN(); entryCMPs.get(cmpIndex).setIncludingFrame(cmp); exitCMPs.get(cmpIndex).setIncludingFrame(cmp); cmpIndex++; entryCMPs.get(cmpIndex).setIncludingFrame(cmp); firstEntryCMPForSingleSupport.setByProjectionOntoXYPlaneIncludingFrame(cmp); computeFootstepCentroid(centroidOfUpcomingFootstep, upcomingFootsteps.get(0)); computeExitCMPForSupportFoot(cmp, transferToSide, centroidOfUpcomingFootstep, isUpcomingFootstepLast); cmp.changeFrame(transferToSoleFrame); exitCMPs.get(cmpIndex).setIncludingFrame(cmp); cmpIndex++;
currentPosition.set(finalPosition); currentVelocity.setToZero(); currentAcceleration.setToZero(); currentPosition.interpolate(initialPosition, finalPosition, quinticParameterPolynomial.getPosition()); currentVelocity.subAndScale(alphaVel, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaAcc, finalPosition, initialPosition);
singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame); singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame); icpSingleSupportTrajectoryGenerator.initialize(); exitCornerPoints.get(0).changeFrame(supportSoleFrame); singleSupportInitialICP.changeFrame(supportSoleFrame); entryCornerPoints.get(0).changeFrame(supportSoleFrame); singleSupportFinalICP.changeFrame(worldFrame); changeFrameOfRemainingCornerPoints(1, worldFrame);