How to use

getZ

method

in

us.ihmc.euclid.tuple4D.Quaternion

@Override
public double getQ_z()
{
 return orientation.getZ();
}

/**
* Returns the value of the z-component of this quaternion.
*
* @return the z-component's value.
*/
@Override
public double getZ()
{
 return quaternion.getZ();
}

@Override
protected double getZ(Quaternion data)
{
 return data.getZ();
}

public double getOrientationQz()
{
 return orientation.getZ();
}

@Override
public double getZ()
{
  return pose.getOrientation().getZ();
}

@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 setY(Quaternion data, double y)
{
 data.setUnsafe(data.getX(), y, data.getZ(), data.getS());
}

@Override
protected void setX(Quaternion data, double x)
{
 data.setUnsafe(x, data.getY(), 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);
}