Quaternion wrong orientation

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:

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:

edit retag close merge delete

Sort by ยป oldest newest most voted

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.

more

1
( 2021-03-02 09:12:21 -0600 )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)

( 2021-03-04 01:46:07 -0600 )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?

( 2021-03-04 02:00:06 -0600 )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 :)

( 2021-03-04 02:29:25 -0600 )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

( 2021-03-04 03:02:37 -0600 )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.

( 2021-03-04 03:05:06 -0600 )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.

( 2021-03-04 09:24:11 -0600 )edit