public void setICPError(FrameVector2d icpError) { this.icpError.setIncludingFrame(icpError); icpErrorUpToDate = true; }
public void setDesiredCapturePointVelocity(FrameVector2d desiredCapturePointVelocity) { this.desiredCapturePointVelocity.setIncludingFrame(desiredCapturePointVelocity); }
public void getPerpendicularBisector(FrameVector2d perpendicularBisectorToPack, double bisectorLengthDesired) { lineSegment.getPerpendicularBisector(tempVector2d, bisectorLengthDesired); perpendicularBisectorToPack.setIncludingFrame(referenceFrame, tempVector2d); }
@Override public void getDesiredHeading(FrameVector2d desiredHeadingToPack, double timeFromNow) { double heading = predictHeading(timeFromNow); desiredHeadingToPack.setIncludingFrame(ReferenceFrame.getWorldFrame(), Math.cos(heading), Math.sin(heading)); }
@Override public void getDesiredHeading(FrameVector2d desiredHeadingToPack, double timeFromNow) { double heading = predictDesiredHeading(timeFromNow); desiredHeadingToPack.setIncludingFrame(ReferenceFrame.getWorldFrame(), Math.cos(heading), Math.sin(heading)); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
public void getICPError(FrameVector2d icpErrorToPack) { momentumBasedController.getCapturePoint(capturePoint2d); yoDesiredCapturePoint.getFrameTuple2dIncludingFrame(desiredCapturePoint2d); icpErrorToPack.setIncludingFrame(desiredCapturePoint2d); icpErrorToPack.sub(capturePoint2d); }
public void getDesiredCapturePointPositionAndVelocity(FramePoint2d desiredCapturePointPositionToPack, FrameVector2d desiredCapturePointVelocityToPack, FramePoint2d currentCapturePointPosition, double time) { super.getDesiredCapturePointPositionAndVelocity(desiredCapturePointPositionToPack, desiredCapturePointVelocityToPack, getTimeWithDelay(time)); if (doTimeFreezing.getBooleanValue()) { tmpCapturePointPosition.setIncludingFrame(desiredCapturePointPositionToPack); tmpCapturePointVelocity.setIncludingFrame(desiredCapturePointVelocityToPack); doTimeFreezeIfNeeded(currentCapturePointPosition, time); } previousTime.set(time); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public void yawAboutPoint(FramePoint2d pointToYawAbout, FramePoint2d pointToPack, double yaw) { if (temporaryPointForYawing == null) temporaryPointForYawing = new FrameVector2d(this); else temporaryPointForYawing.setIncludingFrame(this); temporaryPointForYawing.sub(pointToYawAbout); temporaryTransformToDesiredFrame.setIdentity(); temporaryTransformToDesiredFrame.setRotationYawAndZeroTranslation(yaw); temporaryPointForYawing.applyTransform(temporaryTransformToDesiredFrame); pointToPack.setIncludingFrame(pointToYawAbout); pointToPack.add(temporaryPointForYawing); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
tempICPOffset.setIncludingFrame(supportFrame, desiredICPOffset.getX(), desiredICPOffset.getY()); safeSupportPolygonToConstrainICPOffset.orthogonalProjection(desiredICPToModify); icpOffsetForFreezing.setIncludingFrame(desiredICPToModify); icpOffsetForFreezing.sub(originalICPToModify);
desiredHeadingControlModule.getDesiredHeading(desiredHeading, timeFromNow); toLeftOfDesiredHeading.setIncludingFrame(desiredHeading.getReferenceFrame(), -desiredHeading.getY(), desiredHeading.getX()); desiredVelocity.changeFrame(desiredHeading.getReferenceFrame()); velocityMagnitudeInHeading.set(desiredVelocity.dot(desiredHeading)); desiredOffsetInPredictedHeadingFrame.setIncludingFrame(desiredVelocity); desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.scale(stepDuration);
/** * This function projects the footstep midpoint in the capture region. * Might be a bit conservative it should be sufficient to slightly overlap the capture region * and the touch-down polygon. */ private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion) { projection.setIncludingFrame(projectionPoint); projection.changeFrame(footstep.getParentFrame()); // move the position of the footstep to the capture region centroid nextStep2d.setIncludingFrame(captureRegion.getCentroid()); nextStep2d.changeFrame(footstep.getParentFrame()); // move the position as far away from the projectionPoint as possible direction.setIncludingFrame(nextStep2d); direction.sub(projection); direction.normalize(); direction.scale(10.0); nextStep2d.add(direction); nextStep2d.changeFrame(captureRegion.getReferenceFrame()); captureRegion.orthogonalProjection(nextStep2d); nextStep2d.changeFrame(footstep.getParentFrame()); footstep.setPositionChangeOnlyXY(nextStep2d); calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon); }
comXYVelocity.setIncludingFrame(comVelocity.getReferenceFrame(), comVelocity.getX(), comVelocity.getY()); if (desiredICPVelocity.containsNaN()) comXYAcceleration.setIncludingFrame(desiredICPVelocity);
rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(finalDesiredICPLocation, capturePoint); finalDesiredICPToICPDirection.setIncludingFrame(capturePoint); finalDesiredICPToICPDirection.sub(finalDesiredICPLocation); rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(capturePoint, finalDesiredICPToICPDirection);
exitCMPRayDirection2d.setIncludingFrame(soleFrame, 1.0, 0.0); rayThroughExitCMP.setToNaN(soleFrame); frameConvexPolygonWithRayIntersector2d = new FrameConvexPolygonWithLineIntersector2d();
tempControl.setIncludingFrame(desiredCapturePointVelocity); tempControl.scale(1.0 / omega0);
lineDirection.setIncludingFrame(soleFrame, tempVector3d.getX(), tempVector3d.getY());