public static void transformColumnVectorIntoFrameVector2d(DenseMatrix64F matrix, FrameVector2d frameVector) { frameVector.setX(matrix.get(0, 0)); frameVector.setY(matrix.get(1, 0)); }
public static void transformRowVectorIntoFrameVector2d(DenseMatrix64F matrix, FrameVector2d frameVector) { frameVector.setX(matrix.get(0, 0)); frameVector.setY(matrix.get(0, 1)); }
public void getCMPFeedbackDifference(FrameVector2d cmpFeedbackDifferenceToPack) { cmpFeedbackDifferenceToPack.setToZero(worldFrame); cmpFeedbackDifferenceToPack.setX(feedbackDeltaSolution.get(0, 0)); cmpFeedbackDifferenceToPack.setY(feedbackDeltaSolution.get(1, 0)); }
private void getTransformedWeights(FrameVector2d weightsToPack, double forwardWeight, double lateralWeight) { RigidBodyTransform transform = contactableFeet.get(supportSide.getEnumValue()).getSoleFrame().getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotation.transpose(); transformTranspose.setRotation(rotationTranspose); weightsMatrix.setZero(); weightsMatrix.setElement(0, 0, forwardWeight); weightsMatrix.setElement(1, 1, lateralWeight); weightsMatrixTransformed.set(rotation); weightsMatrixTransformed.mul(weightsMatrix); weightsMatrixTransformed.mul(rotationTranspose); weightsToPack.setToZero(worldFrame); weightsToPack.setX(weightsMatrixTransformed.getElement(0, 0)); weightsToPack.setY(weightsMatrixTransformed.getElement(1, 1)); }
private void getTransformedFeedbackGains(FrameVector2d feedbackGainsToPack) { double epsilonZeroICPVelocity = 1e-5; if (desiredICPVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) { icpVelocityDirectionFrame.setXAxis(desiredICPVelocity); RigidBodyTransform transform = icpVelocityDirectionFrame.getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotationTranspose.transpose(); transformTranspose.setRotation(rotationTranspose); gainsMatrix.setZero(); gainsMatrix.setElement(0, 0, 1.0 + feedbackParallelGain.getDoubleValue()); gainsMatrix.setElement(1, 1, 1.0 + feedbackOrthogonalGain.getDoubleValue()); gainsMatrixTransformed.set(rotation); gainsMatrixTransformed.mul(gainsMatrix); gainsMatrixTransformed.mul(rotationTranspose); feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.setX(gainsMatrixTransformed.getElement(0, 0)); feedbackGainsToPack.setY(gainsMatrixTransformed.getElement(1, 1)); } else { feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.set(feedbackOrthogonalGain.getDoubleValue(), feedbackOrthogonalGain.getDoubleValue()); } yoFeedbackGains.set(feedbackGainsToPack); }
tempControl.setX(tempControl.getX() * captureKpParallelToMotion.getDoubleValue()); tempControl.setY(tempControl.getY() * captureKpOrthogonalToMotion.getDoubleValue()); tempControl.changeFrame(desiredCMP.getReferenceFrame());