Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

Source

Hope someone can help me to find out !