机器人 四元数 与 欧拉角 的相互转换
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;