@Override public void foundClosestPointOnSimplex(SimplexPolytope simplex, Point3D closestPointToOrigin) { System.out.println("\nFound closest point on Simplex: " + closestPointToOrigin); System.out.println("Distance to origin is " + new Vector3D(closestPointToOrigin).length()); }
public static void assertControlErrorIsLow(SimulationConstructionSet scs, RigidBodyBasics chest, double errorTolerance) { Vector3D error = findControlErrorRotationVector(scs, chest); boolean isErrorLow = error.length() <= errorTolerance; assertTrue("Error: " + error, isErrorLow); }
private double findAccelerationGreatestMagnitude() { FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0); double mag = scsFloatingJoint.getAngularAccelerationInBody().length(); Vector3D linearAcceleration = new Vector3D(); scsFloatingJoint.getLinearAccelerationInWorld(linearAcceleration); mag = Math.max(linearAcceleration.length(), mag); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) mag = Math.max(mag, Math.abs(joint.getQDD())); return mag; } }
public void accel2quaternions(Vector3D a, double heading) { double g = a.length(); double roll = -Math.atan2(a.getY(), -a.getZ()); if (!Double.isNaN(roll)) { torso_roll.update(roll); // roll } double pitch = -Math.asin(a.getX() / -g); if (!Double.isNaN(pitch)) { torso_pitch.update(pitch); // pitch } if (!Double.isNaN(heading)) { torso_yaw.update(heading); // yaw } }
private Vector3D exponentialCutForceModel(ExternalForcePoint forcePoint) { tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0) { tangentionalVelocity.setX(0.0); tangentVector.setX(0.0); tangentVector.normalize(); tangentVector.scale(-1.0); tangentVector.scale(90.0 * (Math.exp(1.0 *tangentionalVelocity.length()) - 1.0)); efpHandControlFrameVelocity.set(tangentionalVelocity.length()); return tangentVector; } else { return tangentVector; } } }
/** * Creates a calculator for the camera rotation. * @param up indicates which way is up. * @param forward indicates which is forward. * @throws RuntimeException if {@code up} and {@code forward} are not orthogonal. */ public CameraRotationCalculator(Vector3D up, Vector3D forward) { Vector3D left = new Vector3D(); left.cross(up, forward); if (!MathTools.epsilonEquals(left.length(), 1.0, Epsilons.ONE_HUNDRED_THOUSANDTH)) throw new RuntimeException("The vectors up and forward must be orthogonal. Received: up = " + up + ", forward = " + forward); this.up.set(up); this.forward.set(forward); down.setAndNegate(up); computeOffset(); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testNextOrthogonalVector3D() { Random random = new Random(4876L); for (int i = 0; i < 100; i++) { double length = RandomNumbers.nextDouble(random, 0.1, 100.0); Vector3D vector = RandomGeometry.nextVector3D(random, length); Vector3D orthoVector = RandomGeometry.nextOrthogonalVector3D(random, vector, true); assertEquals(0.0, vector.dot(orthoVector), 1.0e-12); assertEquals(1.0, orthoVector.length(), 1.0e-12); } } }
private void addCylinderLinkGraphics(Vector3D linkStartPointOffsetFromParentJoint, double linkThickness, Vector3D linkVectorInWorld, AppearanceDefinition color, double trimBothEndsByThisMuch) { linkVectorCopy.set(linkVectorInWorld); Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); linkGraphics.translate(linkStartPointOffsetFromParentJoint); linkVectorCopy.sub(linkStartPointOffsetFromParentJoint); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVectorCopy)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVectorCopy.length() - trimBothEndsByThisMuch, linkThickness, color); }
private void addCylinderLinkGraphics(Vector3D linkStartPointOffsetFromParentJoint, double linkThickness, Joint jointWhereLinkEnds, AppearanceDefinition color, double trimBothEndsByThisMuch) { jointWhereLinkEnds.getOffset(linkVectorCopy); Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); linkGraphics.translate(linkStartPointOffsetFromParentJoint); linkVectorCopy.sub(linkStartPointOffsetFromParentJoint); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVectorCopy)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVectorCopy.length() - trimBothEndsByThisMuch, linkThickness, color); }
private Graphics3DObject createJointAxisGeometry(double length, double radius, AppearanceDefinition color, Vector3D cylinderZAxisInWorld, Vector3D parentJointOffsetFromCoM) { Graphics3DObject ret = new Graphics3DObject(); ret.identity(); ret.translate(parentJointOffsetFromCoM); AxisAngle cylinderRotationFromZup; if (parentJointOffsetFromCoM.length() > 0.0) cylinderRotationFromZup = EuclidGeometryTools.axisAngleFromZUpToVector3D(parentJointOffsetFromCoM); else cylinderRotationFromZup = EuclidGeometryTools.axisAngleFromZUpToVector3D(cylinderZAxisInWorld); ret.rotate(cylinderRotationFromZup); ret.translate(0.0, 0.0, -length / 2.0); ret.addCylinder(length, radius, color); return ret; }
public void addCylinderLinkGraphics(Point3D cylinderStart, Point3D cylinderEnd, double linkThickness, AppearanceDefinition color, double trimBothEndsByThisMuch) { Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); Vector3D linkVector = new Vector3D(); linkVector.sub(cylinderEnd, cylinderStart); linkGraphics.translate(cylinderStart); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVector)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVector.length() - trimBothEndsByThisMuch, linkThickness, color); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRotatePoseAboutCollinearAxisAndCheckTranslation() { Random random = new Random(1179L); double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0)); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); FramePose3D framePose = new FramePose3D(worldFrame); framePose.setPosition(1.0, 0.0, 1.0); framePose.setOrientation(RandomGeometry.nextQuaternion(random)); GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, angleToRotate, framePose); Point3D actualPosePositionAfterRotation = new Point3D(framePose.getPosition()); Point3D desiredPosition = new Point3D(Math.cos(angleToRotate), Math.sin(angleToRotate), 1.0); Vector3D positionError = new Vector3D(); positionError.sub(desiredPosition, actualPosePositionAfterRotation); assertTrue("Reference Frame shoud not have changed. Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: " + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame); assertEquals("FramePose Position after rotation is wrong. Desired position: " + desiredPosition + ", actual position: " + actualPosePositionAfterRotation, 0.0, positionError.length(), 1e-3); }
public static RigidBodyTransform opencvTRtoRigidBodyTransform(Mat rvec, Mat tvec) { Vector3D translation = new Vector3D(tvec.get(0, 0)[0], tvec.get(1, 0)[0], tvec.get(2, 0)[0]); Vector3D axis = new Vector3D(rvec.get(0, 0)[0], rvec.get(1, 0)[0], rvec.get(2, 0)[0]); double angle = axis.length(); axis.normalize(); AxisAngle rotation = new AxisAngle(axis, angle); RigidBodyTransform transform = new RigidBodyTransform(rotation, translation); return transform; }
@Test public void testPointDistance() { Random random = new Random(41133L); Pose3D firstPose, secondPose; Point3D point = new Point3D(); Vector3D translation; double length; for (int i = 0; i < ITERATIONS; i++) { firstPose = EuclidGeometryRandomTools.nextPose3D(random); secondPose = new Pose3D(firstPose); translation = EuclidCoreRandomTools.nextVector3D(random); length = translation.length(); secondPose.appendTranslation(translation); assertEquals(length, firstPose.getPositionDistance(secondPose), EPSILON); point.set(firstPose.getPosition()); point.add(translation); assertEquals(length, firstPose.getPositionDistance(point), EPSILON); } }
private Graphics3DObject createCylinderGeometry(double length, double radius, AppearanceDefinition color, Vector3D cylinderZAxisInWorld, Vector3D parentJointOffsetFromCoM) { Graphics3DObject ret = new Graphics3DObject(); ret.addSphere(1.5*radius, YoAppearance.BlackMetalMaterial()); ret.identity(); ret.translate(parentJointOffsetFromCoM); AxisAngle cylinderRotationFromZup; if (parentJointOffsetFromCoM.length() > 0.0) cylinderRotationFromZup = EuclidGeometryTools.axisAngleFromZUpToVector3D(parentJointOffsetFromCoM); else cylinderRotationFromZup = EuclidGeometryTools.axisAngleFromZUpToVector3D(cylinderZAxisInWorld); ret.rotate(cylinderRotationFromZup); ret.translate(0.0, 0.0, -length / 2.0); ret.addCylinder(length, radius, color); return ret; }
@Test public void testSignedDistanceFromPoint3DToPlane3D() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Vector3D parallelToPlane = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal, true); Point3D secondPointOnPlane = new Point3D(); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); double expectedDistance = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); Point3D point = new Point3D(); point.scaleAdd(expectedDistance / planeNormal.length(), planeNormal, secondPointOnPlane); double actualDistance = EuclidGeometryTools.signedDistanceFromPoint3DToPlane3D(point, pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); } }
@Test public void testNormalizeDirection() throws Exception { Random random = new Random(3242L); for (int i = 0; i < ITERATIONS; i++) { Line3D line = new Line3D(); Point3D point = EuclidCoreRandomTools.nextPoint3D(random); Vector3D direction = EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0); line.set(point, direction); assertFalse(direction.epsilonEquals(line.getDirection(), 1.0e-3)); assertTrue(direction.dot(line.getDirection()) > 0.0); assertEquals(direction.length(), direction.dot(line.getDirection()), EPSILON); assertEquals(1.0, line.getDirection().length(), EPSILON); } }
@Test public void testDistanceFromPoint3DToPlane3D() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Vector3D parallelToPlane = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal, true); Point3D secondPointOnPlane = new Point3D(); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); double expectedDistance = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); Point3D point = new Point3D(); double scalar = expectedDistance / planeNormal.length(); if (random.nextBoolean()) scalar = -scalar; point.scaleAdd(scalar, planeNormal, secondPointOnPlane); double actualDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); actualDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point.getX(), point.getY(), point.getZ(), pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); } }
private Quaternion computeRotatedRotationVector(RigidBodyTransform transform1, Quaternion quaternion1, Quaternion quaternion2) { AxisAngle axisAngle1 = new AxisAngle(); axisAngle1.set(quaternion1); AxisAngle axisAngle2 = new AxisAngle(); axisAngle2.set(quaternion2); Vector3D axisAngleVector2 = axisAngleToVector(axisAngle2); Vector3D rotatedAxisAngleVector2 = new Vector3D(axisAngleVector2); transform1.transform(rotatedAxisAngleVector2); Vector3D rotatedAxisAngleVector2Normalized = new Vector3D(rotatedAxisAngleVector2); rotatedAxisAngleVector2Normalized.normalize(); double rotatedAxisAngleVector2Magnitude = rotatedAxisAngleVector2.length(); AxisAngle rotatedAxisAngle2 = new AxisAngle(rotatedAxisAngleVector2Normalized, rotatedAxisAngleVector2Magnitude); Quaternion rotatedAxisAngle2Quaternion = new Quaternion(); rotatedAxisAngle2Quaternion.set(rotatedAxisAngle2); return rotatedAxisAngle2Quaternion; }
private static double computeScalarInertiaAroundJointAxis(Link link, PinJoint pinJoint) { Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); Vector3D jointAxis = new Vector3D(); pinJoint.getJointAxis(jointAxis); Vector3D temp1 = new Vector3D(jointAxis); momentOfInertia.transform(temp1); double scalarInertiaAboutCoM = jointAxis.dot(temp1); // jointAxis^T * momentOfInertia * jointAxis double mass = link.getMass(); Vector3D offsetToCoM = new Vector3D(link.getComOffset()); Vector3D offset = new Vector3D(); pinJoint.getOffset(offset); offsetToCoM.sub(offset); // c - p Vector3D temp3 = new Vector3D(jointAxis); temp3.scale(offsetToCoM.dot(jointAxis)); // ((c - p) . a) * a Vector3D comToJointAxis = new Vector3D(); comToJointAxis.sub(offsetToCoM, temp3); // (c - p) - ((c - p) . a) * a double distanceToJointAxis = comToJointAxis.length(); double scalarInertiaAboutJointAxis = scalarInertiaAboutCoM + mass * distanceToJointAxis * distanceToJointAxis; return scalarInertiaAboutJointAxis; }