How to get the Quaternion from Matrix with python
Hello,
I found below code which is using for get the Quaternion values from Matrix. Then, I would like to do the same work with it, but it's C++ programming language. I can't find any similar python API to it.
geometry_msgs::msg::Pose grasp_to_pose(gpd::GraspConfig & grasp)
{
geometry_msgs::msg::Pose pose;
tf2::Matrix3x3 orientation(
grasp.approach.x, grasp.binormal.x, grasp.axis.x,
grasp.approach.y, grasp.binormal.y, grasp.axis.y,
grasp.approach.z, grasp.binormal.z, grasp.axis.z);
tf2::Quaternion orientation_quat;
orientation.getRotation(orientation_quat);
orientation_quat.normalize();
pose.orientation.x = orientation_quat.x();
pose.orientation.y = orientation_quat.y();
pose.orientation.z = orientation_quat.z();
pose.orientation.w = orientation_quat.w();
pose.position = grasp.top;
return pose;
}
Hope someone can help me to find out !
Check this. Is that what you want?