add conversion

aicp_core/include/aicp_utils/common.hpp

@@ -49,3 +49,4 @@ Eigen::VectorXf get_random_gaussian_variable(float mean, float std_deviation, in
 
 void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& yaw);
 
+Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw);

aicp_core/src/utils/common.cpp

@@ -76,3 +76,30 @@ void quat_to_euler(Eigen::Quaterniond q, double& roll, double& pitch, double& ya
   pitch = asin(2*(q0*q2-q3*q1));
   yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
 }
+
+
+Eigen::Quaterniond euler_to_quat(double roll, double pitch, double yaw) {
+  
+  // This conversion function introduces a NaN in Eigen Rotations when:
+  // roll == pi , pitch,yaw =0    ... or other combinations.
+  // cos(pi) ~=0 but not exactly 0 
+  if ( ((roll==M_PI) && (pitch ==0)) && (yaw ==0)){
+    return  Eigen::Quaterniond(0,1,0,0);
+  }else if( ((pitch==M_PI) && (roll ==0)) && (yaw ==0)){
+    return  Eigen::Quaterniond(0,0,1,0);
+  }else if( ((yaw==M_PI) && (roll ==0)) && (pitch ==0)){
+    return  Eigen::Quaterniond(0,0,0,1);
+  }
+  
+  double sy = sin(yaw*0.5);
+  double cy = cos(yaw*0.5);
+  double sp = sin(pitch*0.5);
+  double cp = cos(pitch*0.5);
+  double sr = sin(roll*0.5);
+  double cr = cos(roll*0.5);
+  double w = cr*cp*cy + sr*sp*sy;
+  double x = sr*cp*cy - cr*sp*sy;
+  double y = cr*sp*cy + sr*cp*sy;
+  double z = cr*cp*sy - sr*sp*cy;
+  return Eigen::Quaterniond(w,x,y,z);
+}