public Pose(double x, double y, double z, double roll, double pitch, double yaw) {
wmes = null;
poseID = null;
pose = new double[6];
pose[PoseIndex.X.ordinal()] = x;
pose[PoseIndex.Y.ordinal()] = y;
pose[PoseIndex.Z.ordinal()] = z;
pose[PoseIndex.ROLL.ordinal()] = roll;
pose[PoseIndex.PITCH.ordinal()] = pitch;
pose[PoseIndex.YAW.ordinal()] = yaw;
}
public double getYaw() {
return pose[PoseIndex.YAW.ordinal()];
}
public void setYaw(double yaw) {
pose[PoseIndex.YAW.ordinal()] = yaw;
}
