private void setRemainingCMPsToDuplicateLastComputedCMP(int lastComputedCMPIndex) { for (int i = lastComputedCMPIndex + 1; i < entryCMPs.size(); i++) entryCMPs.get(i).setIncludingFrame(entryCMPs.get(lastComputedCMPIndex)); for (int i = lastComputedCMPIndex + 1; i < exitCMPs.size(); i++) exitCMPs.get(i).setIncludingFrame(exitCMPs.get(lastComputedCMPIndex)); }
exitCMPs.get(cmpIndex).setIncludingFrame(cmp); entryCMPs.get(cmpIndex).setIncludingFrame(cmp);
entryCMPs.get(constantCMPIndex).setIncludingFrame(cmp); computeFootstepCentroid(centroidOfUpcomingFootstep, upcomingFootsteps.get(0)); boolean isUpcomingFootstepLast = upcomingFootsteps.size() == 1; computeExitCMPForSupportFoot(cmp, supportSide, centroidOfUpcomingFootstep, isUpcomingFootstepLast); cmp.changeFrame(supportSoleFrame); exitCMPs.get(constantCMPIndex).setIncludingFrame(cmp); constantCMPIndex++;
@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 } } }
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++;
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();