m1.setZero(); if (nCol > 0) { if (nRow > 0){
private void computeCenterOfMassVelocityBlock() { tempMatrix.setZero(); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(centerOfMassVelocityPort)); }
if (_vecteur.x != 0) { alpha = -Math.atan(_vecteur.x / _vecteur.y); rot_.setZero(); rot_.rotZ(alpha); } else { alpha = Math.atan(_vecteur.z / _vecteur.y); rot_.setZero(); rot_.rotX(alpha); rot_.transform(lines[5]); beta = -_vecteur.angle(new Vector3d(lines[1].x, lines[1].y, lines[1].z)); rot_.setZero(); rot_.rotY(beta); rot_.transform(lines[1]); lines[5] = new Point3d(_vecteur.length() - _chapeau, 0, -_chapeau); alpha = Math.atan(_vecteur.y / _vecteur.x); rot_.setZero(); rot_.rotZ(alpha); rot_.transform(lines[1]); rot_.transform(lines[5]); beta = -_vecteur.angle(new Vector3d(lines[1].x, lines[1].y, lines[1].z)); rot_.setZero(); rot_.rotY(beta); rot_.transform(lines[1]); lines[5] = new Point3d(0, -_chapeau, _vecteur.length() - _chapeau); alpha = -Math.atan(_vecteur.y / _vecteur.z);
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)); }
public void assembleMeasurementJacobian(Matrix3d ret, Matrix3d jPhi, Matrix3d jOmega, Matrix3d jV, Matrix3d jOmegad, Matrix3d jVd, Matrix3d jP) ret.setZero();
this.tempMatrix3d.setZero(); this.tempSpatialForceVector = new SpatialForceVector(centerOfMassFrame);
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); }