public Vector3D getCurrentTorque() { Vector3D contactTorque = new Vector3D(); contactTorque.set(this.contactTorque); contactTorque.negate(); return contactTorque; }
public Vector3D getCurrentForce() { Vector3D contactForce = new Vector3D(); contactForce.set(this.contactForce); contactForce.negate(); return contactForce; }
public static void flipNormalOfOutliers(PlanarRegionSegmentationNodeData region) { Vector3D regionNormal = region.getNormal(); int numberOfNormalsFlipped = (int) region.nodeParallelStream().filter(node -> isNodeNormalFlipped(node, regionNormal)).count(); int numberOfNormalsNotFlipped = region.getNumberOfNodes() - numberOfNormalsFlipped; // The majority of the nodes are flipped => flip the region normal if (numberOfNormalsFlipped > numberOfNormalsNotFlipped) regionNormal.negate(); // Flip the nodes that upside down region.nodeParallelStream().filter(node -> isNodeNormalFlipped(node, regionNormal)).forEach(NormalOcTreeNode::negateNormal); }
public static void computeQuaternionFromYawAndZNormal(double yaw, Vector3DReadOnly zNormal, QuaternionBasics quaternionToPack) { double Cx = 1.0; double Cy = Math.tan(yaw); if (Math.abs(zNormal.getZ()) < 1e-9) { quaternionToPack.setToNaN(); return; } double Cz = -1.0 * (zNormal.getX() + Cy * zNormal.getY()) / zNormal.getZ(); double CT = Math.sqrt(Cx * Cx + Cy * Cy + Cz * Cz); if (CT < 1e-9) throw new RuntimeException("Error calculating Quaternion"); Vector3D xAxis = new Vector3D(Cx / CT, Cy / CT, Cz / CT); if (xAxis.getX() * Math.cos(yaw) + xAxis.getY() * Math.sin(yaw) < 0.0) { xAxis.negate(); } Vector3D yAxis = new Vector3D(); Vector3DReadOnly zAxis = zNormal; yAxis.cross(zAxis, xAxis); RotationMatrix rotationMatrix = rotationMatrixForQuaternionFromYawAndZNormal.get(); rotationMatrix.setColumns(xAxis, yAxis, zAxis); quaternionToPack.set(rotationMatrix); }
public void addSpinnerHandle(double angleOnSteeringWheelingInDegrees, double percentOfSteeringWheelRadius, double handleLength, double handleRadius, double distanceFromWheel) { RigidBodyTransform transform = new RigidBodyTransform(); double angleOnSteeringWheel = Math.toRadians(angleOnSteeringWheelingInDegrees); double distanceFromCenter = percentOfSteeringWheelRadius * steeringWheelRadius; double xHandle = distanceFromCenter * Math.cos(angleOnSteeringWheel); double yHandle = distanceFromCenter * Math.sin(angleOnSteeringWheel); Vector3D translation = new Vector3D(xHandle, yHandle, distanceFromWheel); transform.setTranslation(translation); FrameCylinder3d spinnerHandleCylinder = new FrameCylinder3d(steeringWheelFrame, transform, handleLength, handleRadius); spokesCylinders.add(spinnerHandleCylinder); steeringWheelLinkGraphics.translate(translation); steeringWheelLinkGraphics.addCylinder(handleLength, handleRadius, YoAppearance.IndianRed()); translation.negate(); steeringWheelLinkGraphics.translate(translation); steeringWheelLink.setLinkGraphics(steeringWheelLinkGraphics); spinnerHandleCenter = new FramePoint3D(steeringWheelFrame, xHandle, yHandle, handleLength / 2.0); }
private void updateNormalAndOriginOnly(NormalOcTreeNode node) { node.getNormal(temporaryVector); // TODO Review and possibly improve dealing with normal flips. if (getNumberOfNodes() >= 1 && temporaryVector.dot(normal) < 0.0) temporaryVector.negate(); normal.update(temporaryVector); point.update(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConversionQDotToAngularVelocityBackAndForth() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); double length = RandomNumbers.nextDouble(random, 0.0, 10.0); Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularVelocity.negate(); Vector3D actualAngularVelocity = new Vector3D(); Vector4D qDot = new Vector4D(); quaternionCalculus.computeQDot(q, expectedAngularVelocity, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(q, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, EPSILON)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAxisAngleEpsilonEqualsIgnoreFlippedAxes() { for (int i = 0; i < 100; i++) { double randomAngle = AngleTools.generateRandomAngle(random); Vector3D randomAxisA = RandomGeometry.nextVector3D(random, 1.0); Vector3D randomAxisA_flipped = new Vector3D(randomAxisA); randomAxisA_flipped.negate(); AxisAngle axisAngleA = new AxisAngle(randomAxisA, randomAngle); AxisAngle axisAngleB = new AxisAngle(randomAxisA_flipped, -randomAngle); assertTrue(axisAngleA + "\n should equal:\n" + axisAngleB + "!", RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngleA, axisAngleB, EPSILON)); } for (int i = 0; i < 100; i++) { double randomAngle = AngleTools.generateRandomAngle(random); Vector3D randomAxisA = RandomGeometry.nextVector3D(random, 1.0); Vector3D randomAxisA_flipped = new Vector3D(randomAxisA); randomAxisA_flipped.negate(); AxisAngle axisAngleA = new AxisAngle(randomAxisA, randomAngle); AxisAngle axisAngleB = new AxisAngle(randomAxisA_flipped, randomAngle); assertTrue(axisAngleA + "\n should *NOT* equal:\n" + axisAngleB + "!", !RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngleA, axisAngleB, EPSILON)); } }
public void recomputeNormalAndOrigin() { pca.clear(); nodes.stream().forEach(node -> pca.addPoint(node.getHitLocationX(), node.getHitLocationY(), node.getHitLocationZ())); pca.compute(); Point3D mean = new Point3D(); pca.getMean(mean); point.clear(); point.update(mean, getNumberOfNodes()); Vector3D thirdVector = new Vector3D(); pca.getThirdVector(thirdVector); pca.getStandardDeviation(standardDeviationPrincipalValues); if (thirdVector.dot(normal) < 0.0) thirdVector.negate(); normal.clear(); normal.update(thirdVector, getNumberOfNodes()); }
Vector3D angularVelocity = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) angularVelocity.negate(); Vector3D expectedAngularAcceleration = RandomGeometry.nextVector3D(random, length); if (random.nextBoolean()) expectedAngularAcceleration.negate(); Vector3D actualAngularAcceleration = new Vector3D(); Vector4D qDot = new Vector4D();
@Test public void testGetCenterPoint() throws Exception { Random random = new Random(24324L); for (int i = 0; i < ITERATIONS; i++) { Point3D center = EuclidCoreRandomTools.nextPoint3D(random, 10.0); Vector3D halfSize = EuclidCoreRandomTools.nextVector3D(random, 0.0, 10.0); BoundingBox3D boundingBox3D = new BoundingBox3D(); boundingBox3D.set(center, halfSize); Point3D actualCenter = new Point3D(); boundingBox3D.getCenterPoint(actualCenter); EuclidCoreTestTools.assertTuple3DEquals(center, actualCenter, EPSILON); Vector3D centerToMin = new Vector3D(); centerToMin.sub(boundingBox3D.getMinPoint(), center); Vector3D centerToMax = new Vector3D(); centerToMax.sub(boundingBox3D.getMaxPoint(), center); centerToMax.negate(); EuclidCoreTestTools.assertTuple3DEquals(centerToMax, centerToMin, EPSILON); } }
/** * Valkyrie is able to complete only half rotating motion. */ public static Pose3D computeClosingValveTrajectory(double time, double trajectoryTime, double radius, boolean closingDirectionCW, double closingAngle, Point3D valveCenterPosition, Vector3D valveNormalVector) { Vector3D xAxisHandFrame = new Vector3D(valveNormalVector); Vector3D yAxisHandFrame = new Vector3D(); Vector3D zAxisHandFrame = new Vector3D(0, 0, 1); xAxisHandFrame.negate(); xAxisHandFrame.normalize(); yAxisHandFrame.cross(zAxisHandFrame, xAxisHandFrame); RotationMatrix handFrame = new RotationMatrix(); handFrame.setColumns(xAxisHandFrame, yAxisHandFrame, zAxisHandFrame); handFrame.appendRollRotation(closingDirectionCW ? -0.5 * Math.PI : 0.5 * Math.PI); RigidBodyTransform handControl = new RigidBodyTransform(handFrame, valveCenterPosition); double phase = time / trajectoryTime; handControl.appendRollRotation(closingDirectionCW ? phase * closingAngle : -phase * closingAngle); handControl.appendTranslation(0, -radius, 0); return new Pose3D(handControl); }
@Test public void testNormal3DFromThreePoint3Ds() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Vector3D expectedPlaneNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); Point3D firstPointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); firstPointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Point3D secondPointOnPlane = new Point3D(); Point3D thirdPointOnPlane = new Point3D(); Vector3D secondOrthogonalToNormal = EuclidCoreRandomTools.nextOrthogonalVector3D(random, expectedPlaneNormal, true); Vector3D thirdOrthogonalToNormal = EuclidCoreRandomTools.nextOrthogonalVector3D(random, expectedPlaneNormal, true); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0), secondOrthogonalToNormal, firstPointOnPlane); thirdPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0), thirdOrthogonalToNormal, firstPointOnPlane); Vector3D actualPlaneNormal = EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); if (expectedPlaneNormal.dot(actualPlaneNormal) < 0.0) actualPlaneNormal.negate(); EuclidCoreTestTools.assertTuple3DEquals(expectedPlaneNormal, actualPlaneNormal, EPSILON); assertNull(EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, firstPointOnPlane)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransformFromA1toA2Simple() { Vector3D vectorA1 = new Vector3D(-1.0, -2.0, -3.0); RigidBodyTransform transformFromWorldToA1 = new RigidBodyTransform(); transformFromWorldToA1.setTranslation(vectorA1); Vector3D vectorA2 = new Vector3D(vectorA1); vectorA2.negate(); RigidBodyTransform transformFromWorldToA2 = new RigidBodyTransform(); transformFromWorldToA2.setTranslation(vectorA2); ReferenceFrame a1 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a1", ReferenceFrame.getWorldFrame(), transformFromWorldToA1); ReferenceFrame a2 = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("a2", ReferenceFrame.getWorldFrame(), transformFromWorldToA2); RigidBodyTransform transformA2toA1 = TransformTools.getTransformFromA2toA1(transformFromWorldToA1, transformFromWorldToA2); Point3D a2Origin = new Point3D(); transformA2toA1.transform(a2Origin); // System.out.println("a2Origin after transform" + a2Origin); FramePoint3D a2OriginFramePoint = new FramePoint3D(a2); a2OriginFramePoint.changeFrame(a1); // System.out.println("a2OriginFramePoint = " + a2OriginFramePoint); a2Origin.epsilonEquals(a2OriginFramePoint, 1e-9); }
Vector3D estimatedBias = new Vector3D(); estimatedBias.set(mahonyFilter.getIntegralTerm()); estimatedBias.negate(); EuclidCoreTestTools.assertVector3DGeometricallyEquals(inputAngularVelocity, estimatedBias, 1.0e-4);
private void read() { imuState.getIMUAccelerationVector(accel); imuState.getIMUMagnetoVector(mag); mag.setX((mag.getX() - magBias.getX()) * magScale.getX()); mag.setY((mag.getY() - magBias.getY()) * magScale.getY()); mag.setZ((mag.getZ() - magBias.getZ()) * magScale.getZ()); accel.negate(); accelAndGyroToZUpMatrix.transform(accel); magToAccellMatrix.transform(mag); accelAndGyroToZUpMatrix.transform(mag); torsoMagX.set(mag.getX()); torsoMagY.set(mag.getY()); torsoMagZ.set(mag.getZ()); torsoAccelX.set(accel.getX()); torsoAccelY.set(accel.getY()); torsoAccelZ.set(accel.getZ()); pYawMagnet.set(-Math.atan2(mag.getY(), mag.getX())); }
direction.negate(); secondLine = new Line3D(firstLine.getPoint(), direction);
translationFromKnobToDoorPlane.negate(); knobCenterControl.prependTranslation(translationFromKnobToDoorPlane);
lineDirection.negate();