How to use

set

method

in

us.ihmc.euclid.tuple4D.Quaternion

  public void packOrientation(Quaternion orientation)
  {
   // FIXME used to be that before switching to EuclidCore
//      orientation.setW((this.orientation[0]));
//      orientation.setW((this.orientation[1]));
//      orientation.setW((this.orientation[2]));
//      orientation.setW((this.orientation[3]));
   orientation.set(this.orientation);
  }

@Override
protected void copy(Quaternion src, Quaternion dest)
{
 dest.set(src);
}

@Override
public void update()
{
 if (unfilteredQuaternion == null)
 {
   throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null "
      + "quaternion variable to call update(), otherwise use update(Quat4d)");
 }
 qMeasured.set(unfilteredQuaternion);
 update(qMeasured);
}

@Override
public void set(SO3Waypoint other)
{
 orientation.set(other.orientation);
 angularVelocity.set(other.angularVelocity);
}

public void queueAxisAngle(AxisAngle axisAngle)
{
 tempQuaternion.set(axisAngle);
 queueQuaternion(tempQuaternion);
}

public void queueMatrix(RotationMatrix rotationMatrix)
{
 tempQuaternion.set(rotationMatrix);
 queueQuaternion(tempQuaternion);
}

public static Quaternion nextQuaternion(Random random, double minMaxAngleRange)
{
 AxisAngle orientation = nextAxisAngle(random, minMaxAngleRange);
 Quaternion quat = new Quaternion();
 quat.set(orientation);
 return quat;
}

private PelvisOrientationTrajectoryMessage createPelvisOrientationTrajectoryMessage(Vector3D rotationAxis, double rotationAngle)
{
 AxisAngle desiredPelvisAxisAngle = new AxisAngle(rotationAxis, rotationAngle);
 Quaternion desiredPelvisQuat = new Quaternion();
 desiredPelvisQuat.set(desiredPelvisAxisAngle);
 PelvisOrientationTrajectoryMessage pelvisOrientationTrajectoryMessage = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(3.0,
                                                                    desiredPelvisQuat);
 return pelvisOrientationTrajectoryMessage;
}

public static VehiclePosePacket createVehiclePosePacket(Point3D position, Quaternion orientation)
{
 VehiclePosePacket message = new VehiclePosePacket();
 message.getPosition().set(position);
 message.getOrientation().set(orientation);
 return message;
}

public double getBodyEstimateYaw()
{
 bodyOrientation.set(bodyCurrentOrientationQx.getDoubleValue(), bodyCurrentOrientationQy.getDoubleValue(), bodyCurrentOrientationQz.getDoubleValue(),
           bodyCurrentOrientationQs.getDoubleValue());
 return bodyOrientation.getYaw();
}

public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, QuaternionReadOnly centerOrientation)
{
 WallPosePacket message = new WallPosePacket();
 message.setCuttingRadius(cuttingRadius);
 message.getCenterPosition().set(centerPosition);
 message.getCenterOrientation().set(centerOrientation);
 return message;
}

public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, RotationMatrixReadOnly rotationMatrix)
{
 WallPosePacket message = new WallPosePacket();
 message.setCuttingRadius(cuttingRadius);
 message.getCenterPosition().set(centerPosition);
 message.getCenterOrientation().set(new Quaternion(rotationMatrix));
 return message;
}

private void lookDown()
{
  AxisAngle orientationAxisAngle = new AxisAngle(0.0, 1.0, 0.0, Math.PI / 2.0);
  Quaternion headOrientation = new Quaternion();
  headOrientation.set(orientationAxisAngle);
  HeadTrajectoryMessage headTrajectoryMessage = HumanoidMessageTools.createHeadTrajectoryMessage(1.0, headOrientation, ReferenceFrame.getWorldFrame(),
                                                 referenceFrames.getChestFrame());
  headTrajectoryMessage.setDestination(PacketDestination.CONTROLLER.ordinal());
  headTrajectoryPublisher.publish(headTrajectoryMessage);
}

private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message)
{
 FootstepDataMessage footstepData = new FootstepDataMessage();
 footstepData.getLocation().set(stepLocation);
 footstepData.getOrientation().set(orient);
 footstepData.setRobotSide(robotSide.toByte());
 message.getFootstepDataList().add().set(footstepData);
}

private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message)
{
 FootstepDataMessage footstepData = new FootstepDataMessage();
 footstepData.getLocation().set(stepLocation);
 footstepData.getOrientation().set(orient);
 footstepData.setRobotSide(robotSide.toByte());
 message.getFootstepDataList().add().set(footstepData);
}

  public void getPose(RigidBodyTransform pose)
  {
   quaternion.set(qx.getDoubleValue(), qy.getDoubleValue(), qz.getDoubleValue(), qw.getDoubleValue());
   pose.setRotation(quaternion);
   pose.setTranslation(xPos.getDoubleValue(), yPos.getDoubleValue(), zPos.getDoubleValue());
  }
}

public static IMUPacket nextIMUPacket(Random random)
{
 IMUPacket next = new IMUPacket();
 next.getLinearAcceleration().set(EuclidCoreRandomTools.nextVector3D32(random));
 next.getOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random));
 next.getAngularVelocity().set(EuclidCoreRandomTools.nextVector3D32(random));
 return next;
}

public void computeChestTrajectoryMessage()
{
 checkIfDataHasBeenSet();
 ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame();
 Quaternion desiredQuaternion = new Quaternion();
 FrameQuaternion desiredOrientation = new FrameQuaternion(chestFrame);
 desiredOrientation.changeFrame(worldFrame);
 desiredQuaternion.set(desiredOrientation);
 ReferenceFrame pelvisZUpFrame = referenceFrames.getPelvisZUpFrame();
 ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredQuaternion, worldFrame, pelvisZUpFrame);
 output.getChestTrajectoryMessage().set(chestTrajectoryMessage);
}

private void addFootstep(Point3D stepLocation, RobotSide robotSide, FootstepDataListMessage message)
{
 FootstepDataMessage footstepData = new FootstepDataMessage();
 footstepData.getLocation().set(stepLocation);
 footstepData.getOrientation().set(new Quaternion(0.0, 0.0, 0.0, 1.0));
 footstepData.setRobotSide(robotSide.toByte());
 message.getFootstepDataList().add().set(footstepData);
}

public static VideoPacket createVideoPacket(VideoSource videoSource, long timeStamp, byte[] data, Point3DReadOnly position, QuaternionReadOnly orientation,
                      IntrinsicParameters intrinsicParameters)
{
 VideoPacket message = new VideoPacket();
 message.setVideoSource(videoSource.toByte());
 message.setTimestamp(timeStamp);
 message.getData().add(data);
 message.getPosition().set(position);
 message.getOrientation().set(orientation);
 message.getIntrinsicParameters().set(toIntrinsicParametersMessage(intrinsicParameters));
 return message;
}