机器人 四元数 与 欧拉角 的相互转换

机器人 四元数 与 欧拉角 的相互转换

flyfish

先看说明
Constructs and initializes the quaternion w+xi+yj+zk from its four coefficients w, x, y and z.
warning Note the order of the arguments: the real w coefficient first,
while internally the coefficients are stored in the following order:
x, y, z, w

从四个系数w x y z构造并初始化四元组w+xi+yj+zk
警告注意参数顺序:实数参数 w优先，
而内部的系数存储顺序如下:x, y, z, w
所以quaternion初始化时 w放在最前面

四元数转欧拉角

geometry_msgs::PoseStamped pose_stamped =
 move_group.getCurrentPose();

   double x = pose_stamped.pose.orientation.x;
   double y = pose_stamped.pose.orientation.y;
   double z = pose_stamped.pose.orientation.z;
   double w = pose_stamped.pose.orientation.w;
   tf::Quaternion q; //或者使用(w,x,y,z);
   tf::quaternionMsgToTF(pose_stamped.pose.orientation, q);

   double roll, pitch, yaw;
   tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

欧拉角转四元数

ros::Time current_time = ros::Time::now();
   geometry_msgs::PoseStamped pose_stamped_new;
   pose_stamped_new.pose.position.x = pose_stamped.pose.position.x;
   pose_stamped_new.pose.position.y = pose_stamped.pose.position.y;
   pose_stamped_new.pose.position.z = pose_stamped.pose.position.z;

   pose_stamped_new.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);

   pose_stamped_new.header.stamp = current_time;