@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { jointTwist.getMatrix(matrix, rowStart); }
/** * Computes the Jacobian. */ public void compute() { int column = 0; for (int i = 0; i < unitTwistList.size(); i++) { Twist twist = unitTwistList.get(i); tempTwist.set(twist); tempTwist.changeFrame(jacobianFrame); tempTwist.getMatrix(tempMatrix, 0); CommonOps.extract(tempMatrix, 0, tempMatrix.getNumRows(), 0, tempMatrix.getNumCols(), jacobian, 0, column++); } }
tempTwist.getMatrix(tempTwistMatrix, 0); tempJacobianPart.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, tempTwistMatrix, tempJacobianPart);
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 Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(errorAxisAngle.getAngle()); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
public Twist computeDesiredTwist(FrameOrientation desiredOrientation, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFrameOrientation.setIncludingFrame(desiredOrientation); errorFrameOrientation.changeFrame(controlFrame); errorFrameOrientation.getAxisAngle(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, new Vector3d(), errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
tempTwist.changeFrame(rigidBodies[i].getInertia().getExpressedInFrame()); tempTwist.getMatrix(tempSpatialMotionMatrix, 0); CommonOps.mult(denseAdjTimesI[i], tempSpatialMotionMatrix, tempMatrix); tempSpatialForceVector.set(centerOfMassFrame, tempMatrix); tempTwist.changeFrame(jointList[j].getSuccessor().getInertia().getExpressedInFrame()); tempTwist.getMatrix(tempSpatialMotionMatrix, 0); CommonOps.mult(denseAdjTimesI[j], tempSpatialMotionMatrix, tempMatrix); tempSpatialForceVector.set(centerOfMassFrame,tempMatrix);
spatialVelocity.getMatrix(tempTaskObjective, 0); CommonOps.mult(selectionMatrix, tempTaskObjective, motionQPInputToPack.taskObjective);