private static void computeTask(Vector3D objective, DenseMatrix64F taskJacobianToPack, DenseMatrix64F taskObjectiveToPack) { taskJacobianToPack.reshape(3, 3); CommonOps.setIdentity(taskJacobianToPack); taskObjectiveToPack.reshape(3, 1); objective.get(taskObjectiveToPack); }
Vector3D randomTuple = EuclidCoreRandomTools.nextVector3D(random); double[] array = new double[3]; randomTuple.get(array); FrameVector3D frameVector3D = new FrameVector3D(randomFrame, array); assertTrue(frameVector3D.getReferenceFrame() == randomFrame);
public static void shootTreeIntoSphere(Octree tree, double radius, double shootingDistance, double xCenter, double yCenter, double zCenter, int numberOfLidarBullets) { Random rand = new Random(-1985L); double[] centerAsArray = new double[] { xCenter, yCenter, zCenter }; double[] startPointAsArray = new double[3]; double[] endPointAsArray = new double[3]; double[] vectorAsArray = new double[3]; double radiusToShootFrom = shootingDistance; for (int k = 0; k < numberOfLidarBullets; k++) { Vector3D vector3d = RandomGeometry.nextVector3D(rand, 1.0); vector3d.get(vectorAsArray); for (int j = 0; j < 3; j++) { startPointAsArray[j] = vectorAsArray[j] * radiusToShootFrom + centerAsArray[j]; endPointAsArray[j] = vectorAsArray[j] * radius + centerAsArray[j]; } Point3D start = new Point3D(startPointAsArray); Point3D end = new Point3D(endPointAsArray); tree.putLidarAtGraduallyMoreAccurateResolution(start, end); } // tree.mergeIfPossible(); }
private DenseMatrix64F getSpatialErrorEndEffectorDesired() { transformEndEffectorToDesired.getTranslation(errorTranslationVector); transformEndEffectorToDesired.getRotation(errorQuat); errorAxisAngle.set(errorQuat); errorAngularVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorAngularVector.scale(errorAxisAngle.getAngle()); errorAngularVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); return spatialError; }
public static void main(String[] args) { RigidBodyTransform t1 = new RigidBodyTransform(); Vector3D rotationVector = new Vector3D(); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); t1.appendYawRotation(Math.PI / 8.0); t1.getRotation(rotationVector); rotationVector.get(rotationVectorMatrix); SelectionMatrix3D selectionMatrix3d = new SelectionMatrix3D(); selectionMatrix3d.selectZAxis(false); DenseMatrix64F selectionMatrix = new DenseMatrix64F(3, 3); selectionMatrix3d.getFullSelectionMatrixInFrame(null, selectionMatrix); DenseMatrix64F result = new DenseMatrix64F(3, 1); CommonOps.mult(selectionMatrix, rotationVectorMatrix, result); System.out.println(result); rotationVector.set(result); RotationMatrix rm = new RotationMatrix(rotationVector); System.out.println(rm); } }
/** * 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); } }
private void computeError(RigidBodyTransform desiredTransform) { jacobian.compute(); jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorRotationVector.scale(0.2); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); }
private void computeError(RigidBodyTransform desiredTransform) { /* * B is base E is end effector D is desired end effector * * H^D_E = H^D_B * H^B_E = (H^B_D)^-1 * H^B_E * * convert rotation matrix part to rotation vector */ jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, error); errorTranslationVector.get(3, error); errorScalar = NormOps.normP2(error); assert (exponentialCoordinatesOK()); }
robotConfigurationData.getForceSensorData().get(feetForceSensorIndexes.get(robotSide)).getAngularPart().get(0, arrayToPublish); robotConfigurationData.getForceSensorData().get(feetForceSensorIndexes.get(robotSide)).getLinearPart().get(3, arrayToPublish); footForceSensorWrenches.put(robotSide, arrayToPublish); footForceSensorPublishers.get(robotSide).publish(timeStamp, footForceSensorWrenches.get(robotSide)); robotConfigurationData.getForceSensorData().get(handForceSensorIndexes.get(robotSide)).getAngularPart().get(0, arrayToPublish); robotConfigurationData.getForceSensorData().get(handForceSensorIndexes.get(robotSide)).getLinearPart().get(3, arrayToPublish); wristForceSensorWrenches.put(robotSide, arrayToPublish); wristForceSensorPublishers.get(robotSide).publish(timeStamp, wristForceSensorWrenches.get(robotSide));
desiredChestOrientation.getRotationVector(rotationVector); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); rotationVector.get(rotationVectorMatrix);
direction.get(directionArray); origin.get(originArray);
desiredChestOrientation.getRotationVector(rotationVector); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); rotationVector.get(rotationVectorMatrix);
DenseMatrix64F denseMatrix = new DenseMatrix64F(7, 1); quaternion.get(denseMatrix); translation.get(4, denseMatrix); QuaternionBasedTransform transform = new QuaternionBasedTransform(denseMatrix); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS); double[] array = new double[7]; quaternion.get(array); translation.get(4, array); QuaternionBasedTransform transform = new QuaternionBasedTransform(array); EuclidCoreTestTools.assertQuaternionEquals(quaternion, transform.getQuaternion(), EPS);
CommonOps.extract(jacobian.getJacobianMatrix(), 0, 3, 0, 2, jacobianAngularPart, 0, 0); solver.setA(jacobianAngularPart); desiredAngularVelocity.get(angularVelocity); solver.solve(angularVelocity, expectedJointVelocities); Assert.assertEquals(expectedJointVelocities.get(0), achievedJointVelocities.get(0), 1.0e-10);
RotationVectorConversion.convertQuaternionToRotationVector(orientationError, rotationError); DenseMatrix64F rotationErrorQ = new DenseMatrix64F(3, 1); rotationError.get(rotationErrorQ);
RotationVectorConversion.convertQuaternionToRotationVector(orientationError, rotationError); DenseMatrix64F rotationErrorQ = new DenseMatrix64F(3, 1); rotationError.get(rotationErrorQ);