public void setRotation(Quat4d jointRotation) { this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(jointRotation); }
public void queueAxisAngle(AxisAngle4d axisAngle) { tempQuaternion.set(axisAngle); queueQuaternion(tempQuaternion); }
public void setOrientation(Quat4d orientation) { if (this.orientation == null) this.orientation = new Quat4d(); this.orientation.set(orientation); }
public void setOrientation(Quat4d orientation) { if (this.orientation == null) this.orientation = new Quat4d(); this.orientation.set(orientation); }
public void resetBodyFixedPoint() { controlFrameOriginInEndEffectorFrame.set(0.0, 0.0, 0.0); controlFrameOrientationInEndEffectorFrame.set(0.0, 0.0, 0.0, 1.0); }
public void setPose(Point3d position, Quat4d orientation) { this.adjustedPosition.set(position); this.adjustedOrientation.set(orientation); }
public void setOrientation(Quat4d orientation) { this.orientation.set(orientation); RotationTools.checkQuaternionNormalized(this.orientation); }
public void set(SO3TrajectoryPointMessage other) { time = other.time; if (other.orientation != null) orientation.set(other.orientation); else orientation.set(0.0, 0.0, 0.0, 1.0); if (other.angularVelocity != null) angularVelocity.set(other.angularVelocity); else angularVelocity.set(0.0, 0.0, 0.0); }
public void setRotation(Quat4d jointRotation) { this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(jointRotation); } public void setRotation(double x, double y, double z, double w)
public void setRotation(double x, double y, double z, double w) { this.jointRotation.set(x, y, z, w); this.afterJointFrame.setRotation(this.jointRotation); }
@Override public void set(TransformableQuat4d other) { super.set(other); if (!containsNaN()) normalize(); }
public void setOrientation(Matrix3d matrix3d) { super.set(matrix3d); if (!containsNaN()) normalize(); }
public void setOrientation(AxisAngle4d axisAngle4d) { super.set(axisAngle4d); if (!containsNaN()) normalize(); }
/** * This computes: resultToPack = q^power. * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result. */ public void pow(Quat4d q, double power, Quat4d resultToPack) { axisAngleForPow.set(q); axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); resultToPack.set(axisAngleForPow); }
public static void extractQuat4dFromEJMLVector(Quat4d quaternion, DenseMatrix64F matrix, int rowStart) { int index = rowStart; double x = matrix.get(index++, 0); double y = matrix.get(index++, 0); double z = matrix.get(index++, 0); double w = matrix.get(index++, 0); quaternion.set(x, y, z, w); }
public void computeQDDotByFiniteDifferenceCentral(Quat4d qPrevious, Quat4d q, Quat4d qNext, double dt, Quat4d qDDotToPack) { qDDotToPack.set(qNext); qDDotToPack.sub(q); qDDotToPack.sub(q); qDDotToPack.add(qPrevious); qDDotToPack.scale(1.0 / MathTools.square(dt)); }
public void interpolate(double alpha, Quat4d q0, Quat4d q1, Quat4d qInterpolatedToPack, boolean preventExtraSpin) { tempQ1ForInterpolation.set(q1); if (preventExtraSpin && dot(q0, tempQ1ForInterpolation) < 0.0) { tempQ1ForInterpolation.negate(); } computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); pow(qInterpolatedToPack, alpha, qInterpolatedToPack); qInterpolatedToPack.mul(q0, qInterpolatedToPack); }
private void corruptOrientation(Quat4d orientation) { Vector3d axis = RandomTools.generateRandomVector(random); double angle = RandomTools.generateRandomDouble(random, -maxRotationCorruption, maxRotationCorruption); AxisAngle4d axisAngle4d = new AxisAngle4d(); axisAngle4d.set(axis, angle); Quat4d corruption = new Quat4d(); corruption.set(axisAngle4d); orientation.mul(corruption); }
/** * This computes: resultToPack = log(q) * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). */ public void log(Quat4d q, Quat4d resultToPack) { axisAngleForLog.set(q); resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); resultToPack.scale(axisAngleForLog.getAngle()); }
private void pitchHeadToFindFiducial() { headPitchToFindFucdicial.mul(-1.0); AxisAngle4d axisAngleOrientation = new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), headPitchToFindFucdicial.getDoubleValue()); Quat4d headOrientation = new Quat4d(); headOrientation.set(axisAngleOrientation); sendHeadTrajectoryMessage(1.0, headOrientation); }