private void changeFrameOfRemainingCornerPoints(int fromIndex, ReferenceFrame desiredFrame) { for (int i = fromIndex; i < entryCornerPoints.size(); i++) entryCornerPoints.get(i).changeFrame(desiredFrame); for (int i = fromIndex; i < exitCornerPoints.size(); i++) exitCornerPoints.get(i).changeFrame(desiredFrame); }
private void changeFrameOfCMPs(int fromIndex, ReferenceFrame desiredFrame) { for (int i = fromIndex; i < entryCMPs.size(); i++) entryCMPs.get(i).changeFrame(desiredFrame); for (int i = fromIndex; i < exitCMPs.size(); i++) exitCMPs.get(i).changeFrame(desiredFrame); }
icpSingleSupportTrajectoryGenerator.initialize(); exitCornerPoints.get(0).changeFrame(supportSoleFrame); singleSupportInitialICP.changeFrame(supportSoleFrame); entryCornerPoints.get(0).changeFrame(supportSoleFrame); singleSupportFinalICP.changeFrame(worldFrame); changeFrameOfRemainingCornerPoints(1, 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 } }
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); computeDesiredCapturePointVelocity(omega0, 0.0, singleSupportInitialICP, entryCMPs.get(1), singleSupportInitialICPVelocity); entryCornerPoints.get(0).changeFrame(initialFrame); entryCornerPoints.get(1).changeFrame(finalFrame); changeFrameOfRemainingCornerPoints(2, worldFrame); isHoldingPosition.set(false); singleSupportInitialICP.changeFrame(finalFrame); singleSupportFinalICP.changeFrame(worldFrame);