public double getNormPositionError(FramePoint desiredPosition) { tempPoint.setIncludingFrame(desiredPosition); tempPoint.changeFrame(localControlFrame); tempPositionError.setIncludingFrame(tempPoint); DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix(); tempSpatialError.reshape(SpatialMotionVector.SIZE, 1); tempSubspaceError.reshape(selectionMatrix.getNumRows(), 1); MatrixTools.insertFrameTupleIntoEJMLVector(tempPositionError, tempSpatialError, 3); CommonOps.mult(selectionMatrix, tempSpatialError, tempSubspaceError); return NormOps.normP2(tempSubspaceError); }
private void computeDesiredSpatialVelocityToSolveFor(DenseMatrix64F spatialDesiredVelocityToPack, DenseMatrix64F spatialVelocityFromError, Twist desiredControlFrameTwist) { DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix(); // Clip to maximum velocity clipSpatialVector(spatialVelocityFromError, selectionMatrix, maximumTaskspaceAngularVelocityMagnitude.getDoubleValue(), maximumTaskspaceLinearVelocityMagnitude.getDoubleValue()); // Update YoVariables for the velocity getAngularAndLinearPartsFromSpatialVector(yoAngularVelocityFromError, yoLinearVelocityFromError, spatialVelocityFromError); filteredAngularVelocityFromError.update(); filteredLinearVelocityFromError.update(); setSpatialVectorFromAngularAndLinearParts(spatialVelocityFromError, filteredAngularVelocityFromError, filteredLinearVelocityFromError); desiredControlFrameTwist.getMatrix(spatialDesiredVelocityToPack, 0); CommonOps.add(spatialVelocityFromError, spatialDesiredVelocityToPack, spatialDesiredVelocityToPack); }
public double getNormRotationError(FrameOrientation desiredOrientation) { tempOrientation.setIncludingFrame(desiredOrientation); tempOrientation.changeFrame(localControlFrame); tempOrientation.getAxisAngle(errorAxisAngle); errorRotationVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.scale(errorAxisAngle.getAngle()); DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix(); tempSpatialError.reshape(SpatialMotionVector.SIZE, 1); tempSubspaceError.reshape(selectionMatrix.getNumRows(), 1); MatrixTools.insertTuple3dIntoEJMLVector(errorRotationVector, tempSpatialError, 0); CommonOps.mult(selectionMatrix, tempSpatialError, tempSubspaceError); return NormOps.normP2(tempSubspaceError); }