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 void setRoll(double roll) {
pose[PoseIndex.ROLL.ordinal()] = roll;
}
public double getRoll() {
return pose[PoseIndex.ROLL.ordinal()];
}