Quaternion wrong orientation

asked 2021-03-02 07:28:46 -0500

crnewton
71 ●4 ●16 ●15

updated 2021-03-04 02:36:25 -0500

I have a binpick application with robot arm and a fixed camera. The camera does it's vision stuff and sends the robot pickpose as a quaternion to the robot, This is done using a static tf between the robot_base and camera_base.

The position(x,y,z) is correct, and the angles is correct, I only want the robot positioned from above:

image description

I don't have a lot of experience with TF, and it looks like my problem is there. Another solution is to 'invert' the quaternion.

Anyone who can point me in the right direction?

Solution:

The answer of @fvd solved my problem, I used rotate_pose_by_rpy(..):

pose_goal = rotate_pose_by_rpy(pose_goal, pi, 0, 0)

This results in the following pose: image description

edit retag flag offensive close merge delete

add a comment

answered 2021-03-02 09:02:31 -0500

fvd
2180 ●15 ●98 ●56

I'm not sure what exactly you need, but I guess this snippet to rotate geometry_msg poses from this World Robot Summit project might help:

// RPY rotations are applied in the frame of the pose.
void rotatePoseByRPY(const double roll, const double pitch, const double yaw, geometry_msgs::Pose& inpose)
{
  tf::Quaternion q;
  tf::Quaternion qrotate = tf::createQuaternionFromRPY(roll, pitch, yaw);

  tf::quaternionMsgToTF(inpose.orientation, q);

  q = q * qrotate;

  tf::quaternionTFToMsg(q, inpose.orientation);
}

In Python:

import tf.transformations
import geometry_msgs.msg

def rotate_pose_by_rpy(in_pose, roll, pitch, yaw):
  """
  Apply an RPY rotation to a pose in its parent coordinate system.
  """
  try:
    if in_pose.header: # = in_pose is a PoseStamped instead of a Pose.
      in_pose.pose = rotate_pose_by_rpy(in_pose.pose, roll, pitch, yaw)
      return in_pose
  except:
    pass
  q_in = [in_pose.orientation.x, in_pose.orientation.y, in_pose.orientation.z, in_pose.orientation.w]
  q_rot = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
  q_rotated = tf.transformations.quaternion_multiply(q_in, q_rot)

  rotated_pose = copy.deepcopy(in_pose)
  rotated_pose.orientation = geometry_msgs.msg.Quaternion(*q_rotated)
  return rotated_pose

There are other helpful functions to work with quaternions in tf and tf2, but they're not very easy to find. Feel free to add references or examples to the tutorials where you believe they might be helpful.

edit flag offensive delete link

Comments

wiki/tf2/Tutorials/Quaternions.

gvdhoorn ( 2021-03-02 09:12:21 -0500 )edit

Thank you. I'll look into quaternion rotation. But shouldn't this be fixed with a 'correct' transform between robot and camera? (your suggestions include conversions (rpy/euler) and to me it looks like that's not what I want)

crnewton ( 2021-03-04 01:46:07 -0500 )edit

Well, maybe? It sounded like your robot moves its tip to the correct position, but at an incorrect orientation, so that you want to change the orientation of the goal pose. If the transform between robot and camera was incorrect, the position would most likely be wrong as well.

If you have doubts, maybe you could update your question with some more information, e.g.: Where is the camera? How do you define the goal pose?

I might have misread your question. You know you can set the orientation of the goal pose from RPY values, right?

fvd ( 2021-03-04 02:00:06 -0500 )edit

Sound logical. "You know you can set the orientation of the goal pose from RPY values, right?" this opened my eyes, it works now. I'm still confused why my TF doesn't take care of this. thank you for your help :)

crnewton ( 2021-03-04 02:29:25 -0500 )edit

You can add a TF display to your Rviz window and compare the orientation of your end effector frame and goal pose to investigate. Cheers

fvd ( 2021-03-04 03:02:37 -0500 )edit

I'm still confused why my TF doesn't take care of this.

What would you expect TF would do?

It's not really an active entity in your application. It's essentially a distributed graph with query and search functionality on-top.

gvdhoorn ( 2021-03-04 03:05:06 -0500 )edit

I'm using fiducial markers to let the camera know my robot pose. I was thinking that this + the TF would be enough information to calculate my robot pose, and it does, but the pitch was inverted. I will use TF display in RVIZ to get a better understanding.

crnewton ( 2021-03-04 09:24:11 -0500 )edit

add a comment

Quaternion wrong orientation

1 Answer

Comments

Question Tools

Stats

Related questions

Quaternion wrong orientation edit

1 Answer

Comments

Question Tools

Stats

Related questions

Quaternion wrong orientation