private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
public void compute(double time) { tangentialPlane.update(); changeFrame(tangentialPlane, false); this.currentTime.set(time); double tIntermediate = leaveTime.getDoubleValue(); xyPolynomial.compute(MathTools.clipToMinMax(time, tIntermediate, trajectoryTime.getDoubleValue())); boolean shouldBeZero = currentTime.getDoubleValue() >= tIntermediate || currentTime.getDoubleValue() < 0.0; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); zPolynomial.compute(MathTools.clipToMinMax(time, 0.0, trajectoryTime.getDoubleValue())); shouldBeZero = isDone() || currentTime.getDoubleValue() < 0.0; alphaDot = shouldBeZero ? 0.0 : zPolynomial.getVelocity(); alphaDDot = shouldBeZero ? 0.0 : zPolynomial.getAcceleration(); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }
@Override public void compute(double time) { currentTime.set(time); double t1 = leaveTime.getDoubleValue(); double t2 = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue(); double tf = trajectoryTime.getDoubleValue(); xyPolynomial.compute(MathTools.clipToMinMax(time, t1, t2)); currentDistortionPose.interpolate(initialDistortionPose, finalDistortionPose, xyPolynomial.getPosition()); distortedPlanePose.setAndMatchFrame(currentDistortionPose); distortedPlane.update(); changeFrame(distortedPlane, false); boolean shouldBeZero = currentTime.getDoubleValue() >= t2 || currentTime.getDoubleValue() < t1; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); zPolynomial.compute(MathTools.clipToMinMax(time, 0.0, tf)); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }
currentPosition.interpolate(initialPosition, finalPosition, quinticParameterPolynomial.getPosition()); currentVelocity.subAndScale(alphaVel, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaAcc, finalPosition, initialPosition);
@Override public void compute(double time) { tangentialPlane.update(); changeFrame(tangentialPlane, false); currentTime.set(time); double tIntermediate = trajectoryTime.getDoubleValue() - approachTime.getDoubleValue(); time = MathTools.clipToMinMax(time, 0.0, tIntermediate); xyPolynomial.compute(MathTools.clipToMinMax(time, 0.0, tIntermediate)); boolean shouldBeZero = currentTime.getDoubleValue() >= tIntermediate || currentTime.getDoubleValue() < 0.0; double alphaDot = shouldBeZero ? 0.0 : xyPolynomial.getVelocity(); double alphaDDot = shouldBeZero ? 0.0 : xyPolynomial.getAcceleration(); currentPosition.interpolate(initialPosition, finalPosition, xyPolynomial.getPosition()); currentVelocity.subAndScale(alphaDot, finalPosition, initialPosition); currentAcceleration.subAndScale(alphaDDot, finalPosition, initialPosition); time = MathTools.clipToMinMax(currentTime.getDoubleValue(), 0.0, trajectoryTime.getDoubleValue()); zPolynomial.compute(time); shouldBeZero = isDone() || currentTime.getDoubleValue() < 0.0; alphaDot = shouldBeZero ? 0.0 : zPolynomial.getVelocity(); alphaDDot = shouldBeZero ? 0.0 : zPolynomial.getAcceleration(); currentPosition.setZ(zPolynomial.getPosition()); currentVelocity.setZ(zPolynomial.getVelocity()); currentAcceleration.setZ(zPolynomial.getAcceleration()); changeFrame(currentTrajectoryFrame, false); }