How to get the Quaternion from Matrix with python

Question

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;
}

Source

Hope someone can help me to find out !

---

Originally posted by A_YIng on ROS Answers with karma: 37 on 2019-05-24

Post score: 0

---

Original comments

Comment by kosmastsk on 2019-06-06:
Check this. Is that what you want?

Answer 1

You can find the API you're looking for in here:

http://docs.ros.org/melodic/api/tf/html/python/transformations.html

The function you need is called:

tf.transformations.quaternion_from_matrix(matrix)

---

Originally posted by dasanche with karma: 83 on 2019-06-08

This answer was ACCEPTED on the original site

Post score: 2