public double getAngularPartX() { return angularPart.getX(); }
public double getLinearPartX() { return linearPart.getX(); }
private void initializePolynomials() { xPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getX(), initialVelocity.getX(), finalPosition.getX(), finalVelocity.getX()); yPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getY(), initialVelocity.getY(), finalPosition.getY(), finalVelocity.getY()); zPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getZ(), initialVelocity.getZ(), finalPosition.getZ(), finalVelocity.getZ()); }
groundContactPointsSlipper.setDoSlip(true); translationPhase.setX(translationPhase.getX() + 2.0 * Math.PI * translationFreqHz[0] * deltaT); translationPhase.setY(translationPhase.getY() + 2.0 * Math.PI * translationFreqHz[1] * deltaT); translationPhase.setZ(translationPhase.getZ() + 2.0 * Math.PI * translationFreqHz[2] * deltaT); rotationPhaseEuler.setX(rotationPhaseEuler.getX() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[2] * deltaT); rotationPhaseEuler.setY(rotationPhaseEuler.getY() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * deltaT); rotationPhaseEuler.setZ(rotationPhaseEuler.getZ() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * deltaT); nextTranslationToSlip.setX(nextTranslationToSlip.getX() * (2.0 * Math.PI * translationFreqHz[0] * Math.sin(translationPhase.getX()) * deltaT)); nextTranslationToSlip.setY(nextTranslationToSlip.getY() * (2.0 * Math.PI * translationFreqHz[1] * Math.sin(translationPhase.getY()) * deltaT)); nextTranslationToSlip.setZ(nextTranslationToSlip.getZ() * (2.0 * Math.PI * translationFreqHz[2] * Math.sin(translationPhase.getZ()) * deltaT)); nextRotationToSlipEulerAngles.setX(nextRotationToSlipEulerAngles.getX() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[2] * Math.sin(rotationPhaseEuler.getX()) * deltaT)); nextRotationToSlipEulerAngles.setY(nextRotationToSlipEulerAngles.getY() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * Math.sin(rotationPhaseEuler.getY()) * deltaT)); nextRotationToSlipEulerAngles.setZ(nextRotationToSlipEulerAngles.getZ() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * Math.sin(rotationPhaseEuler.getZ()) * deltaT));
groundContactPointsSlipper.setDoSlip(true); translationPhase.setX(translationPhase.getX() + 2.0 * Math.PI * translationFreqHz[0] * deltaT); translationPhase.setY(translationPhase.getY() + 2.0 * Math.PI * translationFreqHz[1] * deltaT); translationPhase.setZ(translationPhase.getZ() + 2.0 * Math.PI * translationFreqHz[2] * deltaT); rotationPhaseEuler.setX(rotationPhaseEuler.getX() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[2] * deltaT); rotationPhaseEuler.setY(rotationPhaseEuler.getY() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * deltaT); rotationPhaseEuler.setZ(rotationPhaseEuler.getZ() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * deltaT); nextTranslationToSlip.setX(nextTranslationToSlip.getX() * (2.0 * Math.PI * translationFreqHz[0] * Math.sin(translationPhase.getX()) * deltaT)); nextTranslationToSlip.setY(nextTranslationToSlip.getY() * (2.0 * Math.PI * translationFreqHz[1] * Math.sin(translationPhase.getY()) * deltaT)); nextTranslationToSlip.setZ(nextTranslationToSlip.getZ() * (2.0 * Math.PI * translationFreqHz[2] * Math.sin(translationPhase.getZ()) * deltaT)); nextRotationToSlipEulerAngles.setX(nextRotationToSlipEulerAngles.getX() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[2] * Math.sin(rotationPhaseEuler.getX()) * deltaT)); nextRotationToSlipEulerAngles.setY(nextRotationToSlipEulerAngles.getY() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * Math.sin(rotationPhaseEuler.getY()) * deltaT)); nextRotationToSlipEulerAngles.setZ(nextRotationToSlipEulerAngles.getZ() * (2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * Math.sin(rotationPhaseEuler.getZ()) * deltaT));
public void set(YoFramePoint base, YoFrameVector vector) { this.baseX.set(base.getX()); this.baseY.set(base.getY()); this.baseZ.set(base.getZ()); this.x.set(vector.getX()); this.y.set(vector.getY()); this.z.set(vector.getZ()); }
private void computeIntegralTerm() { if (gains.getMaximumIntegralError() < 1e-5) { integralTerm.setToZero(bodyFrame); return; } double errorIntegratedX = positionError.getX() * dt; double errorIntegratedY = positionError.getY() * dt; double errorIntegratedZ = positionError.getZ() * dt; positionErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ); double errorMagnitude = positionErrorCumulated.length(); if (errorMagnitude > gains.getMaximumIntegralError()) { positionErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude); } positionErrorCumulated.getFrameTuple(integralTerm); integralGainMatrix.transform(integralTerm.getVector()); }
private void computeIntegralTerm(ReferenceFrame baseFrame) { if (gains.getMaximumIntegralError() < 1e-5) { integralTerm.setToZero(baseFrame); return; } double errorIntegratedX = positionError.getX() * dt; double errorIntegratedY = positionError.getY() * dt; double errorIntegratedZ = positionError.getZ() * dt; positionErrorCumulated.add(errorIntegratedX, errorIntegratedY, errorIntegratedZ); double errorMagnitude = positionErrorCumulated.length(); if (errorMagnitude > gains.getMaximumIntegralError()) { positionErrorCumulated.scale(gains.getMaximumIntegralError() / errorMagnitude); } positionErrorCumulated.getFrameTupleIncludingFrame(integralTerm); integralTerm.changeFrame(baseFrame); integralGainMatrix.transform(integralTerm.getVector()); }
@Override public void initialize() { currentTime.set(0.0); double tIntermediate = trajectoryTime.getDoubleValue() / 2.0; xPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getX(), finalPosition.getX(), finalVelocity.getX()); yPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getY(), finalPosition.getY(), finalVelocity.getY()); zPolynomial.setCubicWithIntermediatePositionAndFinalVelocityConstraint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), initialPosition.getZ(), intermediateZPosition.getDoubleValue(), finalPosition.getZ(), finalVelocity.getZ()); currentPosition.set(initialPosition); currentAcceleration.setToZero(); }
linearMomentumRateWeight.set(defaultLinearMomentumRateWeight); momentumRateCommand.setWeights(0.0, 0.0, 0.0, linearMomentumRateWeight.getX(), linearMomentumRateWeight.getY(), linearMomentumRateWeight.getZ());
measuredTorqueAtControlFrame.set(measuredTorque.getVector()); errorForce.setIncludingFrame(controlFrame, desiredForce.getX(), desiredForce.getY(), desiredForce.getZ()); errorForce.sub(measuredForce); errorTorque.setIncludingFrame(controlFrame, desiredTorque.getX(), desiredTorque.getY(), desiredTorque.getZ()); errorTorque.sub(measuredTorque);