@Override public double getQ_x() { return orientation.getX(); }
/** * Returns the value of the x-component of this quaternion. * * @return the x-component's value. */ @Override public double getX() { return quaternion.getX(); }
@Override protected double getX(Quaternion data) { return data.getX(); }
public double getOrientationQx() { return orientation.getX(); }
@Override public double getX() { return pose.getOrientation().getX(); }
@Override public boolean containsNaN() { if (Double.isNaN(orientation.getX()) || Double.isNaN(orientation.getY()) || Double.isNaN(orientation.getZ()) || Double.isNaN(orientation.getS())) return true; if (Double.isNaN(angularVelocity.getX()) || Double.isNaN(angularVelocity.getY()) || Double.isNaN(angularVelocity.getZ())) return true; return false; }
@Override protected void setW(Quaternion data, double w) { data.setUnsafe(data.getX(), data.getY(), data.getZ(), w); }
@Override protected void setZ(Quaternion data, double z) { data.setUnsafe(data.getX(), data.getY(), z, data.getS()); }
@Override protected void setY(Quaternion data, double y) { data.setUnsafe(data.getX(), y, data.getZ(), data.getS()); }
public void get(LongBuffer buffer) { buffer.put(Double.doubleToLongBits(rotation.getS())); buffer.put(Double.doubleToLongBits(rotation.getX())); buffer.put(Double.doubleToLongBits(rotation.getY())); buffer.put(Double.doubleToLongBits(rotation.getZ())); buffer.put(Double.doubleToLongBits(translation.getX())); buffer.put(Double.doubleToLongBits(translation.getY())); buffer.put(Double.doubleToLongBits(translation.getZ())); buffer.put(Double.doubleToLongBits(twist.getAngularPartX())); buffer.put(Double.doubleToLongBits(twist.getAngularPartY())); buffer.put(Double.doubleToLongBits(twist.getAngularPartZ())); buffer.put(Double.doubleToLongBits(twist.getLinearPartX())); buffer.put(Double.doubleToLongBits(twist.getLinearPartY())); buffer.put(Double.doubleToLongBits(twist.getLinearPartZ())); }
public MocapRigidBody(int id, Vector3D position, Quaternion orientation, ArrayList<MocapMarker> listOfAssociatedMarkers, boolean isTracked) { this.id = id; this.xPosition = (float) position.getX(); this.yPosition = (float) position.getY(); this.zPosition = (float) position.getZ(); this.qw = (float) orientation.getS(); this.qx = (float) orientation.getX(); this.qy = (float) orientation.getY(); this.qz = (float) orientation.getZ(); this.listOfAssociatedMarkers = listOfAssociatedMarkers; this.dataValid = isTracked; }
@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); } }
public void get(double[] array) { array[0] = rotation.getS(); array[1] = rotation.getX(); array[2] = rotation.getY(); array[3] = rotation.getZ(); array[4] = translation.getX(); array[5] = translation.getY(); array[6] = translation.getZ(); twist.get(7, array); }
public void queueQuaternion(Quaternion quaternion) { quaternions.reshape(quaternions.getNumRows() + 1, 4, true); int lastIndex = quaternions.getNumRows() - 1; quaternions.set(lastIndex, 0, quaternion.getX()); quaternions.set(lastIndex, 1, quaternion.getY()); quaternions.set(lastIndex, 2, quaternion.getZ()); quaternions.set(lastIndex, 3, quaternion.getS()); }
public static Point3D toPointInWorld(double xToTransform, double yToTransform, Point3D planeOrigin, Quaternion planeOrientation) { Point3D pointInWorld = new Point3D(); double qx = planeOrientation.getX(); double qy = planeOrientation.getY(); double qz = planeOrientation.getZ(); double qs = planeOrientation.getS(); // t = 2.0 * cross(q.xyz, v); // v' = v + q.s * t + cross(q.xyz, t); double x = xToTransform; double y = yToTransform; double z = 0.0; double crossX = 2.0 * (qy * z - qz * y); double crossY = 2.0 * (qz * x - qx * z); double crossZ = 2.0 * (qx * y - qy * x); double crossCrossX = qy * crossZ - qz * crossY; double crossCrossY = qz * crossX - qx * crossZ; double crossCrossZ = qx * crossY - qy * crossX; pointInWorld.setX(x + qs * crossX + crossCrossX); pointInWorld.setY(y + qs * crossY + crossCrossY); pointInWorld.setZ(z + qs * crossZ + crossCrossZ); pointInWorld.add(planeOrigin); return pointInWorld; }
private void setYawPitchRoll() { Quaternion q = new Quaternion(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). q.setYawPitchRoll(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue()); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.multiply(qprev); q_qs.set(q.getS()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
public void updateFullRobotModel(KinematicsToolboxOutputStatus solution) { if (jointsHashCode != solution.getJointNameHash()) throw new RuntimeException("Hashes are different."); for (int i = 0; i < oneDoFJoints.length; i++) { float q = solution.getDesiredJointAngles().get(i); OneDoFJointBasics joint = oneDoFJoints[i]; joint.setQ(q); } Vector3D translation = solution.getDesiredRootTranslation(); rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = solution.getDesiredRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); fullRobotModelToUseForConversion.updateFrames(); }
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); us.ihmc.euclid.tuple4D.Quaternion quat4d = new us.ihmc.euclid.tuple4D.Quaternion(); Vector3D vector3d = new Vector3D(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getS()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); us.ihmc.euclid.tuple4D.Quaternion quat4d = new us.ihmc.euclid.tuple4D.Quaternion(); Vector3D vector3d = new Vector3D(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getS()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
public ScsMocapRigidBody(int id, Vector3D position, Quaternion orientation, ArrayList<MocapMarker> listOfAssociatedMarkers, boolean isTracked) { registry = new YoVariableRegistry("rb_" + id); xPos = new YoDouble("xPos", registry); yPos = new YoDouble("yPos", registry); zPos = new YoDouble("zPos", registry); qx = new YoDouble("qx", registry); qy = new YoDouble("qy", registry); qz = new YoDouble("qz", registry); qw = new YoDouble("qw", registry); this.isTracked = new YoBoolean("", registry); xVel = new YoDouble("xVel", registry); yVel = new YoDouble("yVel", registry); zVel = new YoDouble("zVel", registry); this.id = id; xPos.set(position.getX()); yPos.set(position.getY()); zPos.set(position.getZ()); qx.set(orientation.getX()); qy.set(orientation.getY()); qz.set(orientation.getZ()); qw.set(orientation.getS()); this.listOfAssociatedMarkers = listOfAssociatedMarkers; this.isTracked.set(isTracked); }