Get link absolute orientation - IMU position

asked 2018-01-12 00:11:01 -0600

147 ●7 ●12 ●15

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);
setJointPosition(roll);

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!

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2018-01-12 06:33:36 -0600

PeteBlackerThe3rd

flag of United Kingdom of Great Britain and Northern Ireland

9529 ●30 ●143 ●135 https://www.linkedin.c...

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.

edit flag offensive delete link

Comments

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

rmck ( 2018-01-12 17:34:34 -0600 )edit

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

PeteBlackerThe3rd ( 2018-01-13 11:01:17 -0600 )edit

add a comment

answered 2018-01-18 18:27:59 -0600

rmck

147 ●7 ●12 ●15

I've marked Pete's answers as correct because it put me on the right path. My actual solution was a little different. First I created a transform listener in my main and passed this a tf2_buffer. Within my class as part of my callback method I used the buffer to look up the transform from second to final link to the base link. I then multiplied the inverse of this by my IMU output, giving me the IMU output in terms of roll only. e.g.

tf2::Quaternion all_joints_to_nozzle;      
geometry_msgs::TransformStamped transform_stamped;
transform_stamped = tf_buffer_.lookupTransform("base_link", "tilt_assembly", ros::Time(0));
tf2::convert(transform_stamped.transform.rotation, all_joints_to_nozzle);
tf2::Quaternion nozzle_offset;
nozzle_offset.setEuler(0.0, 0.07, 0.0);
//This results in an accurate position of the roll of the nozzle exclusively
transformed_quaternion = (all_joints_to_nozzle * nozzle_offset).inverse() * imu_quaternion; 
double roll, pitch, yaw;
tf2::Matrix3x3(transformed_quaternion).getEulerYPR(yaw, pitch, roll, 1);
set_position(index, roll);

As long as the IMU isn't powered on in the vertical position (X axis aligned to gravity), I can correctly and consistently measure the position of my end effector now.

edit flag offensive delete link

Comments

Glad you got this working, it can be tricky to get your head around all these frames and transforms at times!

PeteBlackerThe3rd ( 2018-01-19 04:13:36 -0600 )edit

add a comment

Get link absolute orientation - IMU position

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Get link absolute orientation - IMU position edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Get link absolute orientation - IMU position