/** * Transforms the given orthonormal basis coordinates into crystal coordinates. * See Giacovazzo eq 2.20 (or any linear algebra manual) * @param v */ public void transfToCrystal(Tuple3d v) { getMTranspose().transform(v); }
/** * Transforms the given crystal basis coordinates into orthonormal coordinates. * e.g. transfToOrthonormal(new Point3d(1,1,1)) returns the orthonormal coordinates of the * vertex of the unit cell. * See Giacovazzo section 2.E, eq. 2.E.1 (or any linear algebra manual) * @param v */ public void transfToOrthonormal(Tuple3d v) { // see Giacovazzo section 2.E, eq. 2.E.1 getMTransposeInv().transform(v); }
Vector3d vector = ... //a vector to be transformed double x=2, y=0.12; Matrix3d rotMat = new Matrix3d(1,0,0, 0,1,0, 0,0,1); //identity matrix rotMat.rotX(x); //rotation on X axis rotMat.transform(vector); rotMat.rotY(y); // rotation on Y axis rotMat.transform(vector); // the vector should now have both x and y rotation // transformations applied System.out.println("Rotated vector :\n" + vector);
private void processAngularVelocity() { // use orientationOffset^T instead of rotationMatrix, because the angular velocity is measured in IMU body! // omega^B = R^B_IMUBody * omega^IMUBody rawIMUSensors.getAngularVelocity(angularVelocity, imuIndex); orientationOffsetTranspose.transform(angularVelocity); processedSensors.setAngularVelocityInBody(angularVelocity, imuIndex); }
/** * <p> * Transform a point into the "viewport" defined by the localizer that we are an instance of. * </p> * * @param point * a 3D point to be transformed * @return a new, transformed point */ protected Point3d transformPointFromSourceSpaceIntoLocalizerSpace(Point3d point) { Point3d newPoint = new Point3d(point); // do not overwrite the supplied point newPoint.sub(localizerTLHC); // move everything to origin of the target localizer rotateIntoLocalizerSpace.transform(newPoint); return newPoint; }
private void computeBasisVector(int basisVectorIndex, Matrix3d normalContactVectorRotationMatrix, FrameVector basisVectorToPack) { double angle = basisVectorIndex * basisVectorAngleIncrement; double mu = yoPlaneContactState.getCoefficientOfFriction(); // Compute the linear part considering a normal contact vector pointing z-up basisVectorToPack.setIncludingFrame(planeFrame, Math.cos(angle) * mu, Math.sin(angle) * mu, 1.0); // Transforming the result to consider the actual normal contact vector normalContactVectorRotationMatrix.transform(basisVectorToPack.getVector()); basisVectorToPack.normalize(); }
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ public static Point3d[] getPolygonVertices(int n, double radius, Point3d center) { Point3d[] polygon = new Point3d[n]; Matrix3d m = new Matrix3d(); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, radius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[i].add(center); } return polygon; }
/** * Rotates a vector around an axis. * *@param vector vector to be rotated around axis *@param axis axis of rotation *@param angle angle to vector rotate around *@return rotated vector *author: egonw */ public static Vector3d rotate(Vector3d vector, Vector3d axis, double angle) { Matrix3d rotate = new Matrix3d(); rotate.set(new AxisAngle4d(axis.x, axis.y, axis.z, angle)); Vector3d result = new Vector3d(); rotate.transform(vector, result); return result; }
private void computeProportionalTerm(FramePoint desiredPosition, ReferenceFrame baseFrame) { desiredPosition.changeFrame(frameAtBodyFixedPoint); positionError.setAndMatchFrame(desiredPosition); positionError.getFrameTupleIncludingFrame(proportionalTerm); proportionalGainMatrix.transform(proportionalTerm.getVector()); proportionalTerm.changeFrame(baseFrame); }
private void integrateError() { double yawNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_yaw.getDoubleValue() + noiseBias_yaw.getDoubleValue(); double pitchNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_pitch.getDoubleValue() + noiseBias_pitch.getDoubleValue(); double rollNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_roll.getDoubleValue() + noiseBias_roll.getDoubleValue(); RotationTools.convertYawPitchRollToMatrix(yawNoise, pitchNoise, rollNoise, rotationNoise); rotationError.mul(rotationNoise); double xNoise = (random.nextDouble() - 0.5) * noiseScalar_x.getDoubleValue() + noiseBias_x.getDoubleValue(); double yNoise = (random.nextDouble() - 0.5) * noiseScalar_y.getDoubleValue() + noiseBias_y.getDoubleValue(); double zNoise = (random.nextDouble() - 0.5) * noiseScalar_z.getDoubleValue() + noiseBias_z.getDoubleValue(); translationNoise.set(xNoise, yNoise, zNoise); pelvisPose.getRotation(pelvisRotation); pelvisRotation.mul(rotationError); pelvisRotation.transform(translationNoise); translationError.add(translationNoise); } }
private void computeProportionalTerm(FramePoint desiredPosition) { desiredPosition.changeFrame(bodyFrame); positionError.set(desiredPosition); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); double errorMagnitude = positionError.length(); positionError.getFrameTuple(proportionalTerm); if (errorMagnitude > maximumError) { proportionalTerm.scale(maximumError / errorMagnitude); } proportionalGainMatrix.transform(proportionalTerm.getVector()); }
private void processAcceleration() { // Compute acceleration in world frame, subtracting off gravity: Vector3d acceleration = new Vector3d(); rawIMUSensors.getAcceleration(acceleration, imuIndex); acceleration.sub(accelerationOffset); // use rotationMatrixBeforeOffset instead of rotationMatrix, because the accelerations are measured in IMU body! // a(M') = R(M'S') * a(S') rotationMatrixBeforeOffset.transform(acceleration); acceleration.setZ(acceleration.getZ() - localGravityZ); FrameVector frameAcceleration = new FrameVector(ReferenceFrame.getWorldFrame(), acceleration); processedSensors.setAcceleration(frameAcceleration, imuIndex); }
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ @Override public Point3d[] getVertices() { Point3d[] polygon = new Point3d[2*n]; Matrix3d m = new Matrix3d(); Point3d center = new Point3d(0, 0, height/2); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, circumscribedRadius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[n+i] = new Point3d(polygon[i]); polygon[i].sub(center); polygon[n+i].add(center); } return polygon; };
private void computeDerivativeTerm(FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) { desiredAngularVelocity.changeFrame(bodyFrame); currentAngularVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
private void computeDerivativeTerm(FrameVector desiredVelocity, FrameVector currentVelocity) { desiredVelocity.changeFrame(bodyFrame); currentVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); tempMatrix.set(derivativeGainMatrix); tangentialDampingCalculator.compute(positionError, tempMatrix); tempMatrix.transform(derivativeTerm.getVector()); }
public static double computeMinZPointWithRespectToSoleInWorldFrame(Matrix3d footToWorldRotation, ContactablePlaneBody contactableBody) { List<FramePoint> footPoints = contactableBody.getContactPointsCopy(); double minZ = Double.POSITIVE_INFINITY; FramePoint tempFramePoint = new FramePoint(ReferenceFrame.getWorldFrame()); Vector3d tempVector = new Vector3d(); for (FramePoint footPoint : footPoints) { tempFramePoint.setIncludingFrame(footPoint); tempFramePoint.changeFrame(contactableBody.getSoleFrame()); tempVector.set(tempFramePoint.getPoint()); footToWorldRotation.transform(tempVector); if (tempVector.getZ() < minZ) minZ = tempVector.getZ(); } return minZ; }
private void computeDerivativeTerm(FrameVector desiredVelocity, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); endEffectorTwist.changeFrame(baseFrame); bodyFixedPoint.setToZero(frameAtBodyFixedPoint); bodyFixedPoint.changeFrame(baseFrame); endEffectorTwist.getLinearVelocityOfPointFixedInBodyFrame(currentVelocity, bodyFixedPoint); desiredVelocity.changeFrame(baseFrame); currentVelocity.changeFrame(baseFrame); derivativeTerm.setToZero(baseFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); velocityError.setAndMatchFrame(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
public static double computeMinZPointWithRespectToAnkleInWorldFrame(Matrix3d footToWorldRotation, ContactablePlaneBody contactableBody) { List<FramePoint> footPoints = contactableBody.getContactPointsCopy(); double minZ = Double.POSITIVE_INFINITY; FramePoint tempFramePoint = new FramePoint(ReferenceFrame.getWorldFrame()); Vector3d tempVector = new Vector3d(); for (FramePoint footPoint : footPoints) { tempFramePoint.setIncludingFrame(footPoint); tempFramePoint.changeFrame(contactableBody.getFrameAfterParentJoint()); tempVector.set(tempFramePoint.getPoint()); footToWorldRotation.transform(tempVector); if (tempVector.getZ() < minZ) minZ = tempVector.getZ(); } return minZ; }
private void computeProportionalTerm(FrameOrientation desiredOrientation) { desiredOrientation.changeFrame(bodyFrame); desiredOrientation.getQuaternion(errorQuaternion); errorAngleAxis.set(errorQuaternion); errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle())); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); if (errorAngleAxis.getAngle() > maximumError) { errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError); } proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ()); proportionalTerm.scale(errorAngleAxis.getAngle()); rotationErrorInBody.set(proportionalTerm); proportionalGainMatrix.transform(proportionalTerm.getVector()); }
public static void integrate(Matrix3d rotationToPack, Tuple3d positionToPack, double dt, Twist twist) { twist.changeFrame(twist.getBodyFrame()); Vector3d dPosition = new Vector3d(); twist.getLinearPart(dPosition); // velocity in body frame rotationToPack.transform(dPosition); // velocity in base frame dPosition.scale(dt); // translation in base frame positionToPack.add(dPosition); Vector3d axis = new Vector3d(); twist.getAngularPart(axis); axis.scale(dt); double angle = axis.length(); if (angle > 0.0) axis.normalize(); else axis.set(1.0, 0.0, 0.0); AxisAngle4d axisAngle = new AxisAngle4d(axis, angle); Matrix3d dRotation = new Matrix3d(); dRotation.set(axisAngle); rotationToPack.mul(dRotation); }