private void computeJointAngleCorrection(DenseMatrix64F spatialError) { // inverseJacobianCalculator.solveUsingJacobianInverse(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseOne(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseTwo(spatialError); inverseJacobianCalculator.solveUsingDampedLeastSquares(spatialError, jacobian.getJacobianMatrix(), lambdaLeastSquares); jointAnglesCorrection.set(inverseJacobianCalculator.getJointspaceVelocity()); double correctionScale = RandomTools.generateRandomDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, jointAnglesCorrection); for (int i = 0; i < jointAnglesCorrection.getNumRows(); i++) { jointAnglesCorrection.set(i, 0, Math.min(maxStepSize, Math.max(jointAnglesCorrection.get(i, 0), -maxStepSize))); } }
private void computeJointAngleCorrection(DenseMatrix64F spatialError) { // inverseJacobianCalculator.solveUsingJacobianInverse(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseOne(spatialError); // inverseJacobianCalculator.solveUsingJacobianPseudoInverseTwo(spatialError); inverseJacobianCalculator.solveUsingDampedLeastSquares(spatialError, jacobian.getJacobianMatrix(), lambdaLeastSquares); jointAnglesCorrection.set(inverseJacobianCalculator.getJointspaceVelocity()); double correctionScale = RandomNumbers.nextDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, jointAnglesCorrection); for (int i = 0; i < jointAnglesCorrection.getNumRows(); i++) { jointAnglesCorrection.set(i, 0, Math.min(maxStepSize, Math.max(jointAnglesCorrection.get(i, 0), -maxStepSize))); } }