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; }