public void clearContactPoints() { contactPoints.clear(); }
public boolean isEmpty() { return contactPoints.isEmpty(); }
public void copyFromListAndTrimSize(FrameTupleArrayList<?> otherList) { ensureCapacity(otherList.size()); size = otherList.size; for (int i = 0; i < size; i++) { unsafeSet(i, otherList.unsafeGet(i)); } }
public void copyFromListAndTrimSize(List<? extends FrameTuple3DReadOnly> otherList) { for (int i = 0; i < otherList.size(); i++) { getAndGrowIfNeeded(i).setIncludingFrame(otherList.get(i)); } while(size() > otherList.size()) { remove(size() - 1); } }
FrameTupleArrayList<FramePoint3D> footstepList = FrameTupleArrayList.createFramePointArrayList(nFootsteps); ArrayList<YoFramePoint3D> arrayToPack = new ArrayList<YoFramePoint3D>(); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("test", new FramePose3D()); poseReferenceFrame.getPosition(pointToPack); poseReferenceFrame.getOrientation(orientation); FramePoint3D footstepData = footstepList.getAndGrowIfNeeded(i); footstepData.setIncludingFrame(poseReferenceFrame.getRootFrame(), pointToPack); int lastFootstepIndex = Math.min(footstepList.size(), numberFootstepsToConsider) - 1; CapturePointTools.computeConstantCMPs(arrayToPack, footstepList, 0, lastFootstepIndex, startStanding, endStanding); pointBetweenFeet.set(new Point3D(footstepList.get(0))); pointBetweenFeet.add(new Point3D(footstepList.get(1))); pointBetweenFeet.scale(0.5); EuclidCoreTestTools.assertTuple3DEquals("", new Point3D(footstepList.get(0)), arrayToPack.get(0), 1e-10); for (int i = 1; i < footstepList.size(); i++)
FrameTupleArrayList<FramePoint3D> footstepList = FrameTupleArrayList.createFramePointArrayList(nFootsteps); ArrayList<YoFramePoint3D> arrayToPack = new ArrayList<YoFramePoint3D>(); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("test", new FramePose3D()); poseReferenceFrame.getPosition(pointToPack); poseReferenceFrame.getOrientation(orientation); FramePoint3D footstepData = footstepList.getAndGrowIfNeeded(i); footstepData.setIncludingFrame(poseReferenceFrame.getRootFrame(), pointToPack); EuclidCoreTestTools.assertTuple3DEquals("", arrayToPack.get(i), new Point3D(footstepList.get(i)), 1e-10);
public void getContactPoint(int index, FramePoint contactPointToPack) { contactPointToPack.setIncludingFrame(contactPoints.get(index)); }
public int getNumberOfContactPoints() { return contactPoints.size(); }
public static FrameTupleArrayList<FrameVector3D> createFrameVectorArrayList(int initialSize) { return new FrameTupleArrayList<>(initialSize, FrameVector3D.class); } }
public void addPointInContact(FramePoint newPointInContact) { contactPoints.add().setIncludingFrame(newPointInContact); }
public void setOrCreate(int i, FrameTuple<?, ?> frameTuple) { if (i >= size) { size = i + 1; ensureCapacity(size); } unsafeGet(i).setIncludingFrame(frameTuple); }
private void unsafeSet(int i, FrameTuple3DReadOnly frameTuple) { unsafeGet(i).setIncludingFrame(frameTuple); }
public void setOrCreate(int i, FrameTuple3DReadOnly frameTuple) { getAndGrowIfNeeded(i).setIncludingFrame(frameTuple); }
FrameTupleArrayList<FramePoint3D> footstepList = FrameTupleArrayList.createFramePointArrayList(nFootsteps); ArrayList<YoFramePoint3D> arrayToPack = new ArrayList<YoFramePoint3D>(); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("test", new FramePose3D()); poseReferenceFrame.getPosition(pointToPack); poseReferenceFrame.getOrientation(orientation); FramePoint3D footstepData = footstepList.getAndGrowIfNeeded(i); footstepData.setIncludingFrame(poseReferenceFrame.getRootFrame(), pointToPack); int lastFootstepIndex = Math.min(footstepList.size(), numberFootstepsToConsider) - 1; CapturePointTools.computeConstantCMPs(arrayToPack, footstepList, 0, lastFootstepIndex, startStanding, endStanding); pointBetweenFeet.set(new Point3D(footstepList.get(0))); pointBetweenFeet.add(new Point3D(footstepList.get(1))); pointBetweenFeet.scale(0.5); for (int i = 0; i < footstepList.size(); i++)
FrameTupleArrayList<FramePoint3D> footstepList = FrameTupleArrayList.createFramePointArrayList(nFootsteps); ArrayList<YoFramePoint3D> constantCentersOfPressures = new ArrayList<YoFramePoint3D>(); ArrayList<YoFramePoint3D> capturePointsToPack = new ArrayList<YoFramePoint3D>(); poseReferenceFrame.getPosition(pointToPack); poseReferenceFrame.getOrientation(orientation); FramePoint3D footstepData = footstepList.getAndGrowIfNeeded(i); footstepData.setIncludingFrame(poseReferenceFrame.getRootFrame(), pointToPack); CapturePointTools.putConstantCMPBetweenFeet(constantCentersOfPressures.get(0), footstepList.get(0), footstepList.get(1)); CapturePointTools.computeConstantCMPsOnFeet(constantCentersOfPressures, footstepList, 1, lastFootstepIndex); CapturePointTools.putConstantCMPBetweenFeet(constantCentersOfPressures.get(lastFootstepIndex), footstepList.get(lastFootstepIndex - 1), footstepList.get(lastFootstepIndex)); double omega0 = 0.5; double time = 0.5;
public void set(int i, FrameTuple<?, ?> frameTuple) { get(i).setIncludingFrame(frameTuple); }