public void setYawPitchRoll(double yaw, double pitch, double roll) {
Quat4d q = new Quat4d();
RotationTools.convertYawPitchRollToQuaternion(yaw, pitch, roll, q);
RotationTools.checkQuaternionNormalized(q);
q_qs.set(q.getW());
q_qx.set(q.getX());
q_qy.set(q.getY());
q_qz.set(q.getZ());
}
public void setQuaternion(Quat4d q) {
q_qs.set(q.getW());
q_qx.set(q.getX());
q_qy.set(q.getY());
q_qz.set(q.getZ());
}
private static void packQuat4dToGeometry_msgsQuaternion(Quat4d quat, Quaternion orientation) {
orientation.setW(quat.getW());
orientation.setX(quat.getX());
orientation.setY(quat.getY());
orientation.setZ(quat.getZ());
}