private DenseMatrix64F getUpdatedJacobianMatrix() { jacobian.compute(); return jacobian.getJacobianMatrix(); }
private DenseMatrix64F getUpdatedJacobianMatrix() { jacobian.compute(); return jacobian.getJacobianMatrix(); }
public double computeDeterminant() { jacobian.compute(); return inverseJacobianSolver.computeDeterminant(jacobian.getJacobianMatrix()); }
public void computeSelectionMatrix(GeometricJacobian jacobian, DenseMatrix64F selectionMatrix) { DenseMatrix64F jacobianMatrix = jacobian.getJacobianMatrix(); if (selectionMatrixSolver.modifiesA()) { throw new RuntimeException("Selection matrix solver changes A"); } if (!selectionMatrixSolver.setA(jacobianMatrix)) throw new IllegalArgumentException("Invert failed, maybe a bug?"); selectionMatrixSolver.invert(selectionMatrix); } }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); geometricJacobian.changeFrame(geometricJacobianFrame); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
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))); } }
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))); } }
/** * assumes that the GeometricJacobian.compute() method has already been called */ public void compute() { translation.set(point); int angularPartStartRow = 0; int linearPartStartRow = SpatialVector.SIZE / 2; for (int i = 0; i < geometricJacobian.getNumberOfColumns(); i++) { DenseMatrix64F geometricJacobianMatrix = geometricJacobian.getJacobianMatrix(); // angular part: tempVector.set(angularPartStartRow, i, geometricJacobianMatrix); tempJacobianColumn.cross(tempVector, translation); tempVector.set(linearPartStartRow, i, geometricJacobianMatrix); // linear part tempJacobianColumn.add(tempVector); tempJacobianColumn.get(angularPartStartRow, i, jacobianMatrix); } }
/** * assumes that the GeometricJacobian.compute() method has already been called */ public void compute() { point.get(translation); int angularPartStartRow = 0; int linearPartStartRow = SpatialMotionVector.SIZE / 2; for (int i = 0; i < geometricJacobian.getNumberOfColumns(); i++) { DenseMatrix64F geometricJacobianMatrix = geometricJacobian.getJacobianMatrix(); // angular part: MatrixTools.denseMatrixToVector3d(geometricJacobianMatrix, tempVector, angularPartStartRow, i); tempJacobianColumn.cross(tempVector, translation); MatrixTools.denseMatrixToVector3d(geometricJacobianMatrix, tempVector, linearPartStartRow, i); // linear part tempJacobianColumn.add(tempVector); MatrixTools.setDenseMatrixFromTuple3d(jacobianMatrix, tempJacobianColumn, angularPartStartRow, i); } }
private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (InverseDynamicsJoint joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.set(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } }
@Override public void update() { for(int i = 0; i < legOneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = legOneDoFJoints.get(i); jointTorques.set(i, 0, oneDoFJoint.getTauMeasured()); } footJacobian.compute(); DenseMatrix64F jacobianMatrix = footJacobian.getJacobianMatrix(); CommonOps.mult(selectionMatrix, jacobianMatrix, linearPartOfJacobian); UnrolledInverseFromMinor.inv3(linearPartOfJacobian, linearJacobianInverse, 1.0); CommonOps.multTransA(linearJacobianInverse, jointTorques, footLinearForce); footForce.setToZero(footJacobian.getJacobianFrame()); footForce.set(footLinearForce.get(0), footLinearForce.get(1), footLinearForce.get(2)); footForce.changeFrame(ReferenceFrame.getWorldFrame()); measuredZForce.set(footForce.getZ() * -1.0); isInContact.set(measuredZForce.getDoubleValue() > zForceThreshold.getDoubleValue()); } }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); if (!jacobianSolver.setA(jacobian.getJacobianMatrix())) throw new RuntimeException("jacobian cannot be solved"); jacobianSolver.solve(biasedSpatialAcceleration, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomTools.generateRandomDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clipToMinMax(correction.get(i, 0), -maxStepSize, maxStepSize)); } }
public void compute() { jacobian.compute(); CommonOps.extract(jacobian.getJacobianMatrix(), 0, 3, 0, 3, jacobianAngularPart64F, 0, 0); if (Math.abs(CommonOps.det(jacobianAngularPart64F)) < 1e-5) return; CommonOps.invert(jacobianAngularPart64F, inverseAngularJacobian64F); chestAngularVelocity.setToZero(chestIMU.getMeasurementFrame()); chestIMU.getAngularVelocityMeasurement(chestAngularVelocity.getVector()); chestAngularVelocity.changeFrame(jacobian.getJacobianFrame()); pelvisAngularVelocity.setToZero(pelvisIMU.getMeasurementFrame()); pelvisIMU.getAngularVelocityMeasurement(pelvisAngularVelocity.getVector()); pelvisAngularVelocity.changeFrame(jacobian.getJacobianFrame()); chestAngularVelocity.sub(pelvisAngularVelocity); omega.setData(chestAngularVelocity.toArray()); CommonOps.mult(inverseAngularJacobian64F, omega, qd_estimated); for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; double qd_sensorMap = sensorMap.getJointVelocityProcessedOutput(joint); double qd_IMU = qd_estimated.get(i, 0); double qd_fused = (1.0 - alpha.getDoubleValue()) * qd_sensorMap + alpha.getDoubleValue() * qd_IMU; jointVelocitiesFromIMUOnly.get(joint).set(qd_IMU); jointVelocities.get(joint).update(qd_fused); } }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomNumbers.nextDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clamp(correction.get(i, 0), -maxStepSize, maxStepSize)); } }
jacobian.compute(); DenseMatrix64F jacobianMatrix = jacobian.getJacobianMatrix(); DenseMatrix64F transposeJacobianMatrix = new DenseMatrix64F(jacobianMatrix.numCols, Wrench.SIZE); CommonOps.transpose(jacobianMatrix, transposeJacobianMatrix);
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); jacobianSolver.setJacobian(jacobian.getJacobianMatrix()); jacobianSolver.solve(jointAccelerations, biasedSpatialAcceleration); CommonOps.scale(sign, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
public void compute(FrameVector desiredAngularAcceleration) { int vectorSize = SpatialMotionVector.SIZE / 2; DenseMatrix64F biasedAccelerationMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F jacobianDerivativeTermMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F angularJacobian = new DenseMatrix64F(SpatialMotionVector.SIZE / 2, jacobian.getNumberOfColumns()); DenseMatrix64F jointAccelerations = new DenseMatrix64F(angularJacobian.getNumCols(), 1); desiredAngularAcceleration.changeFrame(jacobian.getJacobianFrame()); desiredAngularAcceleration.getInMatrixColumn(biasedAccelerationMatrix, 0); CommonOps.scale(sign, biasedAccelerationMatrix); jacobian.compute(); SpatialAccelerationVector jacobianDerivativeTerm = new SpatialAccelerationVector(); jointAccelerationCalculator.computeJacobianDerivativeTerm(jacobianDerivativeTerm); MatrixTools.setDenseMatrixFromTuple3d(jacobianDerivativeTermMatrix, jacobianDerivativeTerm.getAngularPartCopy(), 0, 0); CommonOps.subtractEquals(biasedAccelerationMatrix, jacobianDerivativeTermMatrix); SubmatrixOps.setSubMatrix(jacobian.getJacobianMatrix(), angularJacobian, 0, 0, 0, 0, angularJacobian.getNumRows(), angularJacobian.getNumCols()); CommonOps.solve(angularJacobian, biasedAccelerationMatrix, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
jacobianMatrix.set(jacobian.getJacobianMatrix());