public void clipMaxLength(double maxLength) { if (maxLength < 1e-7) { this.set(0.0, 0.0); return; } double length = this.length(); if (length < maxLength) return; scale(maxLength / length); }
protected FramePoint2d offsetFootstepFromPath(RobotSide currentFootstepSide, FramePoint2d footstepPosition2d, double footHeading, double offsetAmount) { double sideWaysHeading = footHeading + Math.PI / 2.0; FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, Math.cos(sideWaysHeading), Math.sin(sideWaysHeading)); offsetVector.scale(currentFootstepSide.negateIfRightSide(offsetAmount)); FramePoint2d footstepPosition = new FramePoint2d(footstepPosition2d); footstepPosition.changeFrame(WORLD_FRAME); footstepPosition.add(offsetVector); return footstepPosition; }
private void updateDesiredVelocityVector() { desiredVelocityControlModule.getDesiredVelocity(desiredVelocity); desiredVelocity.set(desiredVelocityDirection); desiredVelocity.scale(desiredVelocityMagnitude.getDoubleValue()); desiredVelocityControlModule.setDesiredVelocity(desiredVelocity); }
public static void computeCapturePointVelocity(FrameVector2d capturePointVelocityToPack, FrameVector2d centerOfMassVelocityInWorld, FrameVector2d centerOfMassAccelerationInWorld, double omega0) { centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassAccelerationInWorld.checkReferenceFrameMatch(worldFrame); capturePointVelocityToPack.setToZero(worldFrame); capturePointVelocityToPack.set(centerOfMassAccelerationInWorld); capturePointVelocityToPack.scale(1.0 / omega0); capturePointVelocityToPack.add(centerOfMassVelocityInWorld); }
shiftingVector.scale(distanceToMoveAwayFromLine);
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 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; }
private void rateLimitCMP(FramePoint2d cmp, FramePoint2d cmpPreviousValue, FramePoint2d perfectCMP, FramePoint2d previousPerfectCMP) { if (feedbackPartMaxRate.getDoubleValue() < 1.0e-3) return; cmpError.setToZero(cmp.getReferenceFrame()); cmpError.sub(cmp, perfectCMP); cmpErrorPreviousValue.setToZero(cmp.getReferenceFrame()); cmpErrorPreviousValue.sub(cmpPreviousValue, previousPerfectCMP); cmpErrorDifference.sub(cmpError, cmpErrorPreviousValue); double errorDifferenceMagnitude = cmpErrorDifference.length(); double errorDifferenceMax = controlDT * feedbackPartMaxRate.getDoubleValue(); if (errorDifferenceMagnitude > errorDifferenceMax) cmpErrorDifference.scale(errorDifferenceMax / errorDifferenceMagnitude); cmpError.add(cmpErrorPreviousValue, cmpErrorDifference); cmp.add(perfectCMP, cmpError); }
vector.sub(intersections[j], intersections[i]); vector.normalize(); vector.scale(distanceToBeInside); pointToMove.set(intersections[i]); pointToMove.add(vector);
desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.set(desiredVelocity.getX(), desiredVelocity.getY()); desiredOffsetInPredictedHeadingFrame.scale(stepDuration); desiredOffsetInPredictedHeadingFrame.scale(stepDuration); if (stepLength > maxStepLength.getDoubleValue()) desiredOffsetFromSupport.scale(maxStepLength.getDoubleValue() / stepLength);
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); }
/** * 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); }
referenceICPVelocityToPack.scale(currentStateProjectionMultiplier.getVelocityMultiplier());
icpOffsetForFreezing.scale(frozenOffsetDecayAlpha.getDoubleValue()); return;
comXYAcceleration.scale(omega0); // MathTools.square(omega0.getDoubleValue()) * (com.getX() - copX);