private void switchCornerPointsToWorldFrame() { for (int i = 0; i < entryCornerPoints.size(); i++) entryCornerPoints.get(i).switchCurrentReferenceFrame(worldFrame); for (int i = 0; i < exitCornerPoints.size(); i++) exitCornerPoints.get(i).switchCurrentReferenceFrame(worldFrame); }
singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame); singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame);
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)); }
@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)); }
@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 } } }
List<YoFramePoint> exitCMPs = referenceCMPsCalculator.getExitCMPs(); switchCornerPointsToWorldFrame(); singleSupportInitialICP.switchCurrentReferenceFrame(worldFrame); singleSupportFinalICP.switchCurrentReferenceFrame(worldFrame);
@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 } }