public FramePoint2d getFrameVertexCopy(int vertexIndex) { FramePoint2d frameVertexCopy = new FramePoint2d(); getFrameVertex(vertexIndex, frameVertexCopy); return frameVertexCopy; }
public void getPreviousFrameVertex(int vertexIndex, FramePoint2d vertexToPack) { getFrameVertex(convexPolygon.getPreviousVertexIndex(vertexIndex), vertexToPack); }
public void getNextFrameVertex(int vertexIndex, FramePoint2d vertexToPack) { getFrameVertex(convexPolygon.getNextVertexIndex(vertexIndex), vertexToPack); }
@Override public String toString() { FramePoint2d vertex = new FramePoint2d(); String ret = ""; for (int i = 0; i < getNumberOfVertices(); i++) { this.getFrameVertex(i, vertex); ret = ret + vertex.toString() + "\n"; } return ret; }
public void updateCMPConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver) { FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(supportSide); solver.setNumberOfCMPVertices(supportPolygon.getNumberOfVertices()); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, tempVertex); solver.setSupportPolygonVertex(i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPSingleSupportExitForward.getDoubleValue(), maxCMPSingleSupportExitSideways.getDoubleValue()); } }
for (int contactPointIndex = 0; contactPointIndex < footSupportPolygon.getNumberOfVertices(); contactPointIndex++) footSupportPolygon.getFrameVertex(contactPointIndex, contactPoint2d); findProjectionOntoPlaneFrame(soleFrame, contactPoint2d, contactPoints.add());
private void sequenceShiftWeight() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitPelvisHomeCommand(false); }
public void updateCMPConstraintForDoubleSupport(ICPOptimizationSolver solver) { int numberOfVertices = 0; for (RobotSide robotSide : RobotSide.values) numberOfVertices += bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide).getNumberOfVertices(); solver.setNumberOfCMPVertices(numberOfVertices); numberOfVertices = 0; for (RobotSide robotSide : RobotSide.values) { FrameConvexPolygon2d supportPolygon = bipedSupportPolygons.getFootPolygonInMidFeetZUp(robotSide); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, tempVertex); solver.setSupportPolygonVertex(numberOfVertices + i, tempVertex, supportPolygon.getReferenceFrame(), maxCMPDoubleSupportExitForward.getDoubleValue(), maxCMPDoubleSupportExitSideways.getDoubleValue()); } numberOfVertices += supportPolygon.getNumberOfVertices(); } }
private void sequenceMediumWarmup() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(), 0.0); sequenceSquats(); sequenceChestRotations(0.55); //TODO increase/decrease limit? sequencePelvisRotations(0.3); //TODO increase/decrease limit? } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitChestHomeCommand(false); submitPelvisHomeCommand(false); }
private void computeToePoints(RobotSide supportSide) { FrameConvexPolygon2d footDefaultPolygon = footDefaultPolygons.get(supportSide); toePoints[0].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); toePoints[1].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); for (int i = 0; i < footDefaultPolygon.getNumberOfVertices(); i++) { footDefaultPolygon.getFrameVertex(i, footPoint); if (footPoint.getX() > toePoints[0].getX()) { toePoints[1].set(toePoints[0]); toePoints[0].setXY(footPoint); } else if (footPoint.getX() > toePoints[1].getX()) { toePoints[1].setXY(footPoint); } } middleToePoint.setToZero(footDefaultPolygon.getReferenceFrame()); middleToePoint.interpolate(toePoints[0], toePoints[1], 0.5); }
yoCurrentCorner.set(corner); supportPolygon.getFrameVertex(corner, currentCorner); FramePoint2d centroid = supportPolygon.getCentroid();
supportPolygon.getFrameVertex(i, frameVertexBefore); supportPolygon.getFrameVertex((i + 1) % numberOfVertices, frameVertexCurrentlyChecked); supportPolygon.getFrameVertex((i + 2) % numberOfVertices, frameVertexAfter);
yoCurrentCorner.set(currentCornerIdx); supportPolygon.getFrameVertex(currentCornerIdx, currentCorner); FramePoint2d centroid = supportPolygon.getCentroid();