private void updateCoMCornerPoints() { List<? extends FramePoint3DReadOnly> comInitialDesiredPositions = planner.getReferenceCoMGenerator().getCoMPositionDesiredInitialList(); List<? extends FramePoint3DReadOnly> comFinalDesiredPositions = planner.getReferenceCoMGenerator().getCoMPositionDesiredFinalList(); comInitialCornerPoints.reset(); for (int i = 0; i < comInitialDesiredPositions.size(); i++) { comInitialCornerPoints.setBall(comInitialDesiredPositions.get(i)); } comFinalCornerPoints.reset(); for (int i = 0; i < comFinalDesiredPositions.size(); i++) { comFinalCornerPoints.setBall(comFinalDesiredPositions.get(i)); } }
private void updateICPCornerPoints() { List<? extends FramePoint3DReadOnly> icpInitialDesiredPositions = planner.getReferenceICPGenerator().getICPPositionDesiredInitialList(); List<? extends FramePoint3DReadOnly> icpFinalDesiredPositions = planner.getReferenceICPGenerator().getICPPositionDesiredFinalList(); icpInitialCornerPoints.reset(); for (int i = 0; i < icpInitialDesiredPositions.size(); i++) { icpInitialCornerPoints.setBall(icpInitialDesiredPositions.get(i)); } icpFinalCornerPoints.reset(); for (int i = 0; i < icpFinalDesiredPositions.size(); i++) { icpFinalCornerPoints.setBall(icpFinalDesiredPositions.get(i)); } }
private void updateCoPCornerPoints() { List<CoPPointsInFoot> copCornerPointPositions = planner.getReferenceCoPGenerator().getWaypoints(); copCornerPoints.reset(); for (int i = 0; i < copCornerPointPositions.size(); i++) { CoPPointsInFoot copPoints = copCornerPointPositions.get(i); for (int j = 0; j < copPoints.getNumberOfCoPPoints(); j++) { copCornerPoints.setBall(copPoints.getWaypointInWorld(j)); } } }
bagOfBalls.reset();
bagOfBalls.reset();
pointListBalls.reset();
bagOfBalls.reset(); int numberOfPoints = 30; CoMHeightPartialDerivativesData coMHeightPartialDerivativesData = new CoMHeightPartialDerivativesData();