Rosanswers logo

Can anyone advise how to calculate the quaternion orientation of a link? I'm following a somewhat unconventional approach and using an IMU that outputs it's absolute quaternion to measure the position of my end effector link. I can see the absolute orientation of each link if I look at the TF tree in RVIZ.

I have encoders on my other joints which gives me an accurate pose up to the end effector. To remove the effects of these other joints on the IMU reading I have tried to multiply the IMU output by the inverse of the orientation quaternions to that point. I have pulled these quaternions from the rotation component of the TF broadcast. I then set my end effector position using the roll component of the transformed quaternion e.g.

tf2::convert(global_transforms_.transforms[0].transform.rotation, extension_pitch_component);
tf2::convert(global_transforms_.transforms[1].transform.rotation, slew_yaw_component);
tf2::convert(global_transforms_.transforms[7].transform.rotation, orbital_roll_component);
tf2::convert(global_transforms_.transforms[8].transform.rotation, tilt_pitch_component);    
tf2::Quaternion output;
output = tilt_pitch_component * (orbital_roll_component * (extension_pitch_component * slew_yaw_component));
tf2::Quaternion publication;
publication = output.inverse() * imu_reading;
double roll, pitch, yaw;
tf2::Matrix3x3(publication).getEulerYPR(yaw, pitch, roll, 1);

The problem is that the variable output doesn't seem to match the TF absolute position of the link before the end effector. If I look at the TF output in RVIZ and publish a pose in the same position, there's a discrepancy between the two. I had a look at the source for the robot state publisher and it looks like it's multiplying the pose matrices of the joints from TF to get the absolute quaternion for each. Is that right?

My end effector can rotate 180 degrees and getting it's orientation right is fairly critical to my application. Unfortunately I can't change to an encoder at this point so I'm stuck with my IMU. Because the other joints can twist and roll I need to remove their effects on the IMU and measure only it's relative change in orientation. Any help appreciated!

Originally posted by rmck on ROS Answers with karma: 147 on 2018-01-12

Post score: 0


1 Answer 1


Rosanswers logo

If you've calibrated your IMU correctly then it will be producing orientation estimates in a global (truly) frame, up/down east/west north/south. If you create a static transform from your robots base_link to the orientation of this global frame. You can then produce a TF of the IMU's orientation in this new global frame. This will give you a frame in your TF tree which is the orientation of the IMU connected to the TF tree for your robot.

Once this is setup and working you can query the TF system to find the transform from the link before your end effector to the IMU frame you've just created, if you take just the orientation part of the resulting transform it will describe the relative orientation of your IMU (and therefore your end effector) in the frame of the preceding joint.

Hope this makes sense.

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2018-01-12

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by rmck on 2018-01-12:
I think I follow. With the static transform, would that take into account the links along the arm? e.g. base_link -> slew_link -> extension_link -> roll_link -> tilt_link -> imu_position then something like:

listener.lookupTransform("/base_link", "/imu_position",ros::Time::now(), transform);

Comment by PeteBlackerThe3rd on 2018-01-13:
Not quite, the static transform is converting the orientation of the IMU into the base_link. base_link to tilt_link is already provided by the robot so then you have to do

listener.lookupTransform("/base_link", "/imu_position",ros::Time::now(), transform);

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.