public void getFramePoint2d(FramePoint2d framePoint2d) { framePoint2d.setToZero(referenceFrame); line.getPoint(framePoint2d.tuple); }
public void initialize() { for (int i = 0; i < contactablePlaneBodies.size(); i++) { ContactablePlaneBody contactablePlaneBody = contactablePlaneBodies.get(i); cops.get(contactablePlaneBody).setToZero((contactablePlaneBody.getSoleFrame())); } }
@Override public void setToZero() { position.setToZero(); orientation.setToZero(); }
private void checkAndSetFrames(AbstractReferenceFrameHolder frameObject1, AbstractReferenceFrameHolder frameObject2) { frameObject1.checkReferenceFrameMatch(frameObject2); intersectionPointOne.setToZero(frameObject1.getReferenceFrame()); intersectionPointTwo.setToZero(frameObject1.getReferenceFrame()); }
/** * Computes the intersections of a ray with this polygon. Since the polygon is convex the maximum * number of intersections is two. Returns the number of intersections found. If there are less * then two intersections the FramePoints are set to NaN. * * @param ray ray to intersect this polygon with * @param intersectionToPack1 modified - is set to the first intersection * If the are no intersections this will be set to NaN. * @param intersectionToPack2 modified - is set to the second intersection * If there is only one intersection this will be set to NaN. * @return The number of intersections 0, 1, or 2 */ public int intersectionWithRay(FrameLine2d ray, FramePoint2d intersectionToPack1, FramePoint2d intersectionToPack2) { checkReferenceFrameMatch(ray); intersectionToPack1.setToZero(referenceFrame); intersectionToPack2.setToZero(referenceFrame); return convexPolygon.intersectionWithRay(ray.getLine2d(), intersectionToPack1.getPoint(), intersectionToPack2.getPoint()); }
public void orthogonalProjection(FramePoint2d point, FramePoint2d projectedPointToPack) { checkReferenceFrameMatch(point); projectedPointToPack.setToZero(referenceFrame); lineSegment.orthogonalProjection(point.getPoint(), projectedPointToPack.getPoint()); }
public boolean getClosestPointWithRay(FramePoint2d closestVertexToPack, FrameLine2d ray) { ray.checkReferenceFrameMatch(referenceFrame); closestVertexToPack.setToZero(referenceFrame); boolean success = convexPolygon.getClosestPointWithRay(closestVertexToPack.getPoint(), ray.getLine2d()); return success; }
public void getFootstepSolutionLocation(int footstepIndex, FramePoint2d footstepLocationToPack) { footstepLocationToPack.setToZero(worldFrame); footstepLocationToPack.setX(footstepLocationSolution.get(2 * footstepIndex, 0)); footstepLocationToPack.setY(footstepLocationSolution.get(2 * footstepIndex + 1, 0)); }
/** * Compute the capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeCapturePointPosition(double omega0, double time, FramePoint2d initialCapturePoint, FramePoint2d initialCMP, FramePoint2d desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); } }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
public void updateReachabilityConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver) { solver.setNumberOfReachabilityVertices(4); double lateralInnerLimit = lateralReachabilityInnerLimit.getDoubleValue(); double lateralOuterLimit = lateralReachabilityOuterLimit.getDoubleValue(); lateralInnerLimit = supportSide.negateIfLeftSide(lateralInnerLimit); lateralOuterLimit = supportSide.negateIfLeftSide(lateralOuterLimit); ReferenceFrame supportSoleFrame = bipedSupportPolygons.getSoleZUpFrames().get(supportSide); tempVertex.setToZero(supportSoleFrame); tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralInnerLimit); solver.setReachabilityVertex(0, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralOuterLimit); solver.setReachabilityVertex(1, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralInnerLimit); solver.setReachabilityVertex(2, tempVertex, supportSoleFrame); tempVertex.setToZero(supportSoleFrame); tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralOuterLimit); solver.setReachabilityVertex(3, tempVertex, supportSoleFrame); }
public void computeCMPOffsetRecursionEffect(FramePoint2d cmpOffsetRecursionEffectToPack, ArrayList<YoFramePoint2d> upcomingFootstepLocations, int numberOfFootstepsToConsider) { computeTwoCMPOffsets(upcomingFootstepLocations, numberOfFootstepsToConsider); cmpOffsetRecursionEffectToPack.setToZero(); for (int i = 0; i < numberOfFootstepsToConsider; i++) { totalOffsetEffect.set(exitOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); totalOffsetEffect.set(entryOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionEntryMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); } cmpOffsetRecursionEffectToPack.scale(footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier()); }
public void computeAchievedCMP(FrameVector achievedLinearMomentumRate, FramePoint2d achievedCMPToPack) { if (achievedLinearMomentumRate.containsNaN()) return; centerOfMass2d.setToZero(centerOfMassFrame); centerOfMass2d.changeFrame(worldFrame); achievedCoMAcceleration2d.setByProjectionOntoXYPlaneIncludingFrame(achievedLinearMomentumRate); achievedCoMAcceleration2d.scale(1.0 / totalMass); achievedCoMAcceleration2d.changeFrame(worldFrame); achievedCMPToPack.set(achievedCoMAcceleration2d); achievedCMPToPack.scale(-1.0 / (omega0 * omega0)); achievedCMPToPack.add(centerOfMass2d); }
public void computeConvexHull(FrameConvexPolygon2d convexHullToPack) { convexHullToPack.clear(soleFrame); tempCellCenter.setToZero(soleFrame); for (int xIndex = 0; xIndex < nLengthSubdivisions.getIntegerValue(); xIndex++) { for (int yIndex = 0; yIndex < nWidthSubdivisions.getIntegerValue(); yIndex++) { if (isCellOccupied(xIndex, yIndex)) { getCellCenter(tempCellCenter, xIndex, yIndex); convexHullToPack.addVertex(tempCellCenter); } } } convexHullToPack.update(); }
private void putExitCMPOnToes(FrameConvexPolygon2d footSupportPolygon, FramePoint2d exitCMPToPack) { // Set x to have the CMP slightly inside the support polygon exitCMPToPack.setToZero(footSupportPolygon.getReferenceFrame()); exitCMPToPack.setX(footSupportPolygon.getMaxX() - 1.6e-2); exitCMPToPack.setY(footSupportPolygon.getCentroid().getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdgesWhenSteppingDown.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(exitCMPToPack); }
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
@Override public void projectCMPIntoSupportPolygonIfOutside(FramePoint2d capturePoint, FrameConvexPolygon2d supportPolygon, FramePoint2d finalDesiredCapturePoint, FramePoint2d desiredCMPToPack) { ReferenceFrame returnFrame = desiredCMPToPack.getReferenceFrame(); desiredCMPToPack.changeFrame(supportPolygon.getReferenceFrame()); capturePoint.changeFrame(supportPolygon.getReferenceFrame()); projectedCMP.setToZero(supportPolygon.getReferenceFrame()); projectedCMP.setX(desiredCMPToPack.getX()); projectedCMP.setY(supportPolygon.getCentroid().getY()); projectCMPIntoSupportPolygonIfOutsideLocal(supportPolygon, desiredCMPToPack); desiredCMPToPack.changeFrame(returnFrame); capturePoint.changeFrame(returnFrame); }
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }
private void computeDesiredICPOffset() { pelvisPositionError.set(desiredPelvisPosition); tempPosition2d.setToZero(pelvisZUpFrame); tempPosition2d.changeFrame(worldFrame); pelvisPositionError.sub(tempPosition2d); pelvisPositionError.getFrameTuple2dIncludingFrame(tempError2d); tempError2d.scale(controlDT); pelvisPositionCumulatedError.add(tempError2d); double cumulativeErrorMagnitude = pelvisPositionCumulatedError.length(); if (cumulativeErrorMagnitude > maximumIntegralError.getDoubleValue()) { pelvisPositionCumulatedError.scale(maximumIntegralError.getDoubleValue() / cumulativeErrorMagnitude); } proportionalTerm.set(pelvisPositionError); proportionalTerm.scale(proportionalGain.getDoubleValue()); integralTerm.set(pelvisPositionCumulatedError); integralTerm.scale(integralGain.getDoubleValue()); desiredICPOffset.set(proportionalTerm); desiredICPOffset.add(integralTerm); }