public boolean solveUsingJacobianInverse(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); boolean success = linearAlgebraSolver.setA(subspaceJacobianMatrix); if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, jointspaceVelocity); return success; }
public boolean solveUsingJacobianInverse(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); boolean success = linearAlgebraSolver.setA(subspaceJacobianMatrix); if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseTwo(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseOne(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J^T J computeJacobianTransposedTimesJacobian(jacobianTransposedTimesJacobianMatrix, subspaceJacobianMatrix); // J^T*deltaX jacobianTimesSpatialVelocity.reshape(numberOfDoF, 1); CommonOps.mult(jacobianMatrixTransposed, subspaceSpatialVelocity, jacobianTimesSpatialVelocity); boolean success = linearAlgebraSolver.setA(jacobianTransposedTimesJacobianMatrix); // Solve J^T*J delta q = J^T * deltaX if (success) linearAlgebraSolver.solve(jacobianTimesSpatialVelocity, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseTwo(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingJacobianPseudoInverseOne(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J^T J computeJacobianTransposedTimesJacobian(jacobianTransposedTimesJacobianMatrix, subspaceJacobianMatrix); // J^T*deltaX jacobianTimesSpatialVelocity.reshape(numberOfDoF, 1); CommonOps.mult(jacobianMatrixTransposed, subspaceSpatialVelocity, jacobianTimesSpatialVelocity); boolean success = linearAlgebraSolver.setA(jacobianTransposedTimesJacobianMatrix); // Solve J^T*J delta q = J^T * deltaX if (success) linearAlgebraSolver.solve(jacobianTimesSpatialVelocity, jointspaceVelocity); return success; }
public boolean solveUsingNullspaceMethod(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix, DenseMatrix64F privilegedJointVelocities) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); intermediateSubspaceSpatialVelocity.reshape(numberOfConstraints, 1); intermediateSubspaceSpatialVelocity.set(subspaceSpatialVelocity); // xDot - J qDot0 CommonOps.multAdd(-1.0, subspaceJacobianMatrix, privilegedJointVelocities, intermediateSubspaceSpatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T xDot = f if (success) linearAlgebraSolver.solve(intermediateSubspaceSpatialVelocity, intermediateResultInTaskspace); // qDot = J^T f + qDot0 jointspaceVelocity.set(privilegedJointVelocities); CommonOps.multAdd(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingNullspaceMethod(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix, DenseMatrix64F privilegedJointVelocities) { computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); intermediateSubspaceSpatialVelocity.reshape(numberOfConstraints, 1); intermediateSubspaceSpatialVelocity.set(subspaceSpatialVelocity); // xDot - J qDot0 CommonOps.multAdd(-1.0, subspaceJacobianMatrix, privilegedJointVelocities, intermediateSubspaceSpatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedMatrix); // Solve J*J^T xDot = f if (success) linearAlgebraSolver.solve(intermediateSubspaceSpatialVelocity, intermediateResultInTaskspace); // qDot = J^T f + qDot0 jointspaceVelocity.set(privilegedJointVelocities); CommonOps.multAdd(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingDampedLeastSquares(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix, double lambdaLeastSquares) { // J computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); lamdaSquaredMatrix.reshape(numberOfConstraints, numberOfConstraints); lamdaSquaredMatrix.zero(); for (int i = 0; i < numberOfConstraints; i++) lamdaSquaredMatrix.set(i, i, lambdaLeastSquares); jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix.reshape(numberOfConstraints, numberOfConstraints); jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix.set(jacobianTimesJacobianTransposedMatrix); CommonOps.add(jacobianTimesJacobianTransposedMatrix, lamdaSquaredMatrix, jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }
public boolean solveUsingDampedLeastSquares(DenseMatrix64F spatialVelocity, DenseMatrix64F jacobianMatrix, double lambdaLeastSquares) { // J computeSubspaceJacobian(subspaceJacobianMatrix, jacobianMatrix); computeSubspaceSpatialVelocity(subspaceSpatialVelocity, spatialVelocity); // J^T computeJacobianTransposed(jacobianMatrixTransposed, subspaceJacobianMatrix); // J J^T computeJacobianTimesJacobianTransposed(jacobianTimesJacobianTransposedMatrix, subspaceJacobianMatrix); intermediateResultInTaskspace.reshape(numberOfConstraints, 1); lamdaSquaredMatrix.reshape(numberOfConstraints, numberOfConstraints); lamdaSquaredMatrix.zero(); for (int i = 0; i < numberOfConstraints; i++) lamdaSquaredMatrix.set(i, i, lambdaLeastSquares); jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix.reshape(numberOfConstraints, numberOfConstraints); jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix.set(jacobianTimesJacobianTransposedMatrix); CommonOps.add(jacobianTimesJacobianTransposedMatrix, lamdaSquaredMatrix, jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix); boolean success = linearAlgebraSolver.setA(jacobianTimesJacobianTransposedPlusLamdaSquaredMatrix); // Solve J*J^T deltaX = f if (success) linearAlgebraSolver.solve(subspaceSpatialVelocity, intermediateResultInTaskspace); CommonOps.mult(jacobianMatrixTransposed, intermediateResultInTaskspace, jointspaceVelocity); return success; }