/** * Returns the value of the x-component of this vector. * * @return the x-component's value. */ @Override public double getX() { return vector.getX(); }
private Vector3DReadOnly convertToAngularAcceleration(QuaternionReadOnly q, Vector4DReadOnly qDot, Vector4DReadOnly qDDot) { // w = qDDot * q^-1 + qDot * qDot^-1 QuaternionTools.multiplyConjugateRight(qDDot, q, tempConvertVector4D); tempConvertVector3D.setX(tempConvertVector4D.getX()); tempConvertVector3D.setY(tempConvertVector4D.getY()); tempConvertVector3D.setZ(tempConvertVector4D.getZ()); QuaternionTools.multiplyConjugateRight(qDot, qDot, tempConvertVector4D); tempConvertVector3D.addX(tempConvertVector4D.getX()); tempConvertVector3D.addY(tempConvertVector4D.getY()); tempConvertVector3D.addZ(tempConvertVector4D.getZ()); tempConvertVector3D.scale(2.0); return tempConvertVector3D; }
private Vector3DReadOnly convertToAngularVelocity(QuaternionReadOnly q, Vector4DReadOnly qDot) {// w = qDot * q^-1 QuaternionTools.multiplyConjugateRight(qDot, q, tempConvertVector4D); tempConvertVector3D.setX(tempConvertVector4D.getX()); tempConvertVector3D.setY(tempConvertVector4D.getY()); tempConvertVector3D.setZ(tempConvertVector4D.getZ()); tempConvertVector3D.scale(2.0); return tempConvertVector3D; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
assertTrue(vector.getX() == 0.0); assertTrue(vector.getY() == 0.0); assertTrue(vector.getZ() == 0.0); double s = random.nextDouble(); Vector4D vector = new Vector4D(x, y, z, s); assertTrue(vector.getX() == x); assertTrue(vector.getY() == y); assertTrue(vector.getZ() == z); double[] vectorArray = {x, y, z, s}; Vector4D vector = new Vector4D(vectorArray); assertTrue(vector.getX() == x); assertTrue(vector.getY() == y); assertTrue(vector.getZ() == z);
/** * Computes the convex hull of the intersections of footPolygonInWorldFrame when snapped to planarRegion using * snapTransform * * @param planarRegion * @param footPolygon foot polygon in world frame * @param snapTransform * @return intersection polygon in region frame */ public static ConvexPolygon2D getConvexHullOfPolygonIntersections(PlanarRegion planarRegion, ConvexPolygon2D footPolygon, RigidBodyTransform snapTransform) { ArrayList<ConvexPolygon2D> intersections = new ArrayList<>(); ConvexPolygon2D footPolygonInPlaneFrame = new ConvexPolygon2D(); RigidBodyTransform inverseSnapTransform = new RigidBodyTransform(snapTransform); inverseSnapTransform.invert(); for (int i = 0; i < footPolygon.getNumberOfVertices(); i++) { Point2DReadOnly vertex = footPolygon.getVertex(i); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), 0.0, 1.0); snapTransform.transform(transformPoint); transformPoint.setZ(0.0); footPolygonInPlaneFrame.addVertex(transformPoint.getX() + 1e-10, transformPoint.getY() + 1e-10); } footPolygonInPlaneFrame.update(); planarRegion.getPolygonIntersectionsWhenProjectedVertically(footPolygonInPlaneFrame, intersections); return getConvexHull(intersections); }
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
private PlanarRegion createPlanarRegion(RigidBodyTransform regionToWorldFrameTransform, ConvexPolygon2D desiredFootholdInWorldXYPlane) { RigidBodyTransform worldToRegionTransform = new RigidBodyTransform(regionToWorldFrameTransform); worldToRegionTransform.invert(); ConvexPolygon2D planarRegionPolygon = new ConvexPolygon2D(); for (int i = 0; i < desiredFootholdInWorldXYPlane.getNumberOfVertices(); i++) { Point2DReadOnly vertex = desiredFootholdInWorldXYPlane.getVertex(i); double zHeight = getPlaneZGivenXY(regionToWorldFrameTransform, vertex.getX(), vertex.getY()); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), zHeight, 1.0); worldToRegionTransform.transform(transformPoint); planarRegionPolygon.addVertex(transformPoint.getX(), transformPoint.getY()); } planarRegionPolygon.update(); return new PlanarRegion(regionToWorldFrameTransform, planarRegionPolygon); }
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
double xError = Math.abs(correctedPelvis_x.getDoubleValue() - error.getX()); double yError = Math.abs(correctedPelvis_y.getDoubleValue() - error.getY()); double zError = Math.abs(correctedPelvis_z.getDoubleValue() - error.getZ());
Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); QuaternionTools.transform(quaternion, vector3D, vector3D); vectorExpected.set(vector3D);
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
@Test public void testTransformWithVector4D() throws Exception { Random random = new Random(5634L); AffineTransform transform = EuclidCoreRandomTools.nextAffineTransform(random); Vector4D vectorOriginal = EuclidCoreRandomTools.nextVector4D(random); Vector4D vectorExpected = new Vector4D(); Vector4D vectorActual = new Vector4D(); Vector3D vector3D = new Vector3D(vectorOriginal.getX(), vectorOriginal.getY(), vectorOriginal.getZ()); transform.transform(vector3D); vectorExpected.set(vector3D); vectorExpected.setS(vectorOriginal.getS()); vectorExpected.addX(vectorExpected.getS() * transform.getM03()); vectorExpected.addY(vectorExpected.getS() * transform.getM13()); vectorExpected.addZ(vectorExpected.getS() * transform.getM23()); transform.transform(vectorOriginal, vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); vectorActual.set(vectorOriginal); transform.transform(vectorActual); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); // Try with dense-matrix DenseMatrix64F transformDenseMatrix = new DenseMatrix64F(4, 4); transform.get(transformDenseMatrix); DenseMatrix64F vectorOriginalDenseMatrix = new DenseMatrix64F(4, 1); vectorOriginal.get(vectorOriginalDenseMatrix); DenseMatrix64F vectorTransformedDenseMatrix = new DenseMatrix64F(4, 1); CommonOps.mult(transformDenseMatrix, vectorOriginalDenseMatrix, vectorTransformedDenseMatrix); vectorExpected.set(vectorTransformedDenseMatrix); EuclidCoreTestTools.assertTuple4DEquals(vectorExpected, vectorActual, EPS); }
FrameVector4D frameVector4D = new FrameVector4D(randomFrame, randomTuple.getX(), randomTuple.getY(), randomTuple.getZ(), randomTuple.getS()); assertTrue(frameVector4D.getReferenceFrame() == randomFrame); EuclidCoreTestTools.assertTuple4DEquals(randomTuple, frameVector4D, EPSILON);
vectorExpected.setX(matrix.getM00() * vectorOriginal.getX()); vectorExpected.setY(matrix.getM11() * vectorOriginal.getY()); vectorExpected.setZ(matrix.getM22() * vectorOriginal.getZ());
tupleExpected.setX(pureQuaternion.getX()); tupleExpected.setY(pureQuaternion.getY()); tupleExpected.setZ(pureQuaternion.getZ());
tupleExpected.setX(pureQuaternion.getX()); tupleExpected.setY(pureQuaternion.getY()); tupleExpected.setZ(pureQuaternion.getZ());