private void setUnitMomenta(CompositeRigidBodyInertia currentBodyInertia, GeometricJacobian motionSubspace) { nMomentaInUse = motionSubspace.getNumberOfColumns(); for (int i = 0; i < nMomentaInUse; i++) { tempTwist.set(motionSubspace.getAllUnitTwists().get(i)); tempTwist.changeFrame(currentBodyInertia.getExpressedInFrame()); unitMomenta[i].compute(currentBodyInertia, tempTwist); } }
public void set(GeometricJacobian geometricJacobian, FramePoint point) { if (geometricJacobian.getBaseFrame() != geometricJacobian.getJacobianFrame()) throw new RuntimeException("Jacobian must be expressed in base frame"); this.geometricJacobian = geometricJacobian; this.point.setIncludingFrame(point); this.point.changeFrame(geometricJacobian.getBaseFrame()); this.jacobianMatrix.reshape(3, geometricJacobian.getNumberOfColumns()); }
public void set(GeometricJacobian geometricJacobian, FramePoint3D point) { if (geometricJacobian.getBaseFrame() != geometricJacobian.getJacobianFrame()) throw new RuntimeException("Jacobian must be expressed in base frame"); this.geometricJacobian = geometricJacobian; this.point.setIncludingFrame(point); this.point.changeFrame(geometricJacobian.getBaseFrame()); this.jacobianMatrix.reshape(3, geometricJacobian.getNumberOfColumns()); }
private void setMassMatrixPart(int i, int j, GeometricJacobian motionSubspace) { int rowStart = massMatrixIndices[i]; int colStart = massMatrixIndices[j]; for (int m = 0; m < nMomentaInUse; m++) { Momentum unitMomentum = unitMomenta[m]; int massMatrixRow = rowStart + m; for (int n = 0; n < motionSubspace.getNumberOfColumns(); n++) { Twist unitRelativeTwist = motionSubspace.getAllUnitTwists().get(n); unitRelativeTwist.changeFrame(unitMomentum.getExpressedInFrame()); double entry = unitMomentum.computeKineticCoEnergy(unitRelativeTwist); int massMatrixCol = colStart + n; setMassMatrixSymmetrically(massMatrixRow, massMatrixCol, entry); } } }
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; }
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 = SpatialMotionVector.SIZE; numberOfDoF = jacobian.getNumberOfColumns(); inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false); oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.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; }
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, 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; this.solver = LinearSolverFactory.leastSquares(SpatialMotionVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = ScrewTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, 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; this.solver = LinearSolverFactory.leastSquares(SpatialVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = MultiBodySystemTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
/** * 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); } }
rootJacobian.compute(); DenseMatrix64F jointVelocitiesMatrix = new DenseMatrix64F(rootJacobian.getNumberOfColumns(), 1); MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); jacobian.compute(); jointVelocitiesMatrix.reshape(jacobian.getNumberOfColumns(), 1); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix);
rootJacobian.compute(); DenseMatrix64F jointVelocitiesMatrix = new DenseMatrix64F(rootJacobian.getNumberOfColumns(), 1); MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); jacobian.compute(); jointVelocitiesMatrix.reshape(jacobian.getNumberOfColumns(), 1); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix);
continue; int chainNumberOfDoFs = chainJacobian.getNumberOfColumns(); tempCompactJacobian.reshape(chainNumberOfDoFs, chainNumberOfDoFs); CommonOps.setIdentity(tempCompactJacobian);
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); } }
pointJacobian2.compute(); DenseMatrix64F assembledJacobian = new DenseMatrix64F(SpatialVector.SIZE, geometricJacobian.getNumberOfColumns()); CommonOps.insert(pointJacobian1.getJacobianMatrix(), assembledJacobian, 0, 0); CommonOps.insert(pointJacobian2.getJacobianMatrix(), assembledJacobian, 3, 0);
tempTaskJacobian.reshape(taskSize, jacobian.getNumberOfColumns()); CommonOps.mult(selectionMatrix, jacobian.getJacobianMatrix(), tempTaskJacobian); boolean success = jointIndexHandler.compactBlockToFullBlock(jacobian.getJointsInOrder(), tempTaskJacobian, motionQPInputToPack.taskJacobian);
tempTaskJacobian.reshape(taskSize, jacobian.getNumberOfColumns()); CommonOps.mult(selectionMatrix, jacobian.getJacobianMatrix(), tempTaskJacobian);