public void setDesiredCapturePointState(YoFramePoint2d currentDesiredCapturePointPosition, YoFrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public void setDesiredCapturePointState(FramePoint2d currentDesiredCapturePointPosition, FrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public void compute(double time) { timeIntoStep.set(time); nominalTrajectoryGenerator.compute(time); nominalTrajectoryGenerator.getLinearData(nominalTrajectoryPosition, nominalTrajectoryVelocity, nominalTrajectoryAcceleration); xPolynomial.compute(time); yPolynomial.compute(time); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); desiredVelocity.setX(xPolynomial.getVelocity()); desiredVelocity.setY(yPolynomial.getVelocity()); desiredVelocity.setZ(nominalTrajectoryVelocity.getZ()); desiredAcceleration.setX(xPolynomial.getAcceleration()); desiredAcceleration.setY(yPolynomial.getAcceleration()); desiredAcceleration.setZ(nominalTrajectoryAcceleration.getZ()); }
translationPhase.setY(translationPhase.getY() + 2.0 * Math.PI * translationFreqHz[1] * deltaT); translationPhase.setZ(translationPhase.getZ() + 2.0 * Math.PI * translationFreqHz[2] * deltaT); rotationPhaseEuler.setY(rotationPhaseEuler.getY() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * deltaT); rotationPhaseEuler.setZ(rotationPhaseEuler.getZ() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[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.setY(rotationPhaseEuler.getY() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[1] * deltaT); rotationPhaseEuler.setZ(rotationPhaseEuler.getZ() + 2.0 * Math.PI * rotationFreqHzYawPitchRoll[0] * deltaT);
int index = 0; wrenchEquilibriumTorqueError.setX(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0)); wrenchEquilibriumTorqueError.setY(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0)); wrenchEquilibriumTorqueError.setZ(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0)); wrenchEquilibriumForceError.setX(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0)); wrenchEquilibriumForceError.setY(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0)); wrenchEquilibriumForceError.setZ(tempWrenchConstraint_LHS.get(index, 0) - tempWrenchConstraint_RHS.get(index++, 0));