public NumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double lambdaLeastSquares, double tolerance, int maxIterations, double maxStepSize,
double minRandomSearchScalar, double maxRandomSearchScalar)
{
if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame())
throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()");
this.jacobian = jacobian;
numberOfConstraints = SpatialVector.SIZE;
numberOfDoF = jacobian.getNumberOfColumns();
inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false);
oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class);
if (oneDoFJoints.length != jacobian.getJointsInOrder().length)
throw new RuntimeException("Can currently only handle OneDoFJoints");
jointAnglesCorrection = new DenseMatrix64F(numberOfDoF, 1);
jointAnglesMinimumError = new DenseMatrix64F(numberOfDoF, 1);
this.lambdaLeastSquares = lambdaLeastSquares;
this.tolerance = tolerance;
this.maxIterations = maxIterations;
this.maxStepSize = maxStepSize;
this.minRandomSearchScalar = minRandomSearchScalar;
this.maxRandomSearchScalar = maxRandomSearchScalar;
}