Ask Your Question
1

Applying rotations to coordinate frames using TF

asked 2013-10-06 10:36:57 -0500

obotic gravatar image

updated 2014-01-28 17:18:09 -0500

ngrennan gravatar image

Hello, I have two frames on my robot, /base_link and /frame_imu. The imu is positioned 90deg in the positive yaw direction relative to the robot center(or /base_link). So basically a roll in the imu's coordinate frame would correspond to a pitch in the robot's coordinate frame. From what I understand I should be able to use TF's transform_broadcaster to broadcast this transformation:

  tf::Transform transform;
  transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
  transform.setRotation( tf::createQuaternionFromRPY(0,0,M_PI/2) );
  tf_broadcast->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/base_link", "imu_frame"));

At first I thought if I just use TF's lookupTransform I could get the correct transformation between frames(my imu's frame_id is set to /imu_frame):

void imuCallback(const sensor_msgs::Imu::ConstPtr& msg){
tf::StampedTransform transform;

listener->waitForTransform("/base_link",
                                    msg->header.frame_id,
                                    msg->header.stamp,
                                    ros::Duration(3.0));

listener->lookupTransform("/base_link",
                                  msg->header.frame_id,
                                  msg->header.stamp,
                                  transform);

That just gives me the transformation (0,0,M_PI/2), then I could use TF's transformQuaternion to actually perform the rotation from the imu's frame to the robot's frame:

geometry_msgs::QuaternionStamped imu_quat = geometry_msgs::QuaternionStamped();
imu_quat.header = msg->header;
imu_quat.quaternion = msg->orientation;
geometry_msgs::QuaternionStamped q_robot;
listener->transformQuaternion("base_link",msg->header.stamp,imu_quat, imu_quat.header.frame_id, q_robot);
tf::Quaternion quatquat;
tf::quaternionMsgToTF(q_robot.quaternion,quatquat);
tf::Matrix3x3(quatquat).getEulerYPR(new_yaw,new_pitch,new_roll);

What happens is if my imu reads a (Yaw,Pitch,Roll) of say (80,10,120) I would expect my (new_roll,new_pitch,new_yaw) to be (80,90,120) but instead I get (170,10,120) so it just adds the yaw factor. Is trasnformQuaternion the right method for what I want to achieve? Any help to point me in the right direction would be greatly appreciated.

EDIT/UPDATE:

Well I don't think the twists really made a difference. I imagined that if my frame transformation setup is:

  tf::Transform transform;
  transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
  transform.setRotation( tf::createQuaternionFromRPY(0,0,M_PI/2) );
  tf_broadcast->sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/base_link", "imu_frame"));

And I have the twists set up like so:

        geometry_msgs::Twist robot_twist;
        listener.lookupTwist("/imu_frame","/base_link",ros::Time(0),ros::Duration(3.0),robot_twist);

Then if I perform a roll on my /imu_frame, I should see an angular velocity increase on the pitch(robot_twist.angular.y) of my /base_link , but I still see the angular velocity of roll(robot_twist.angular.x) increasing on /base_link . It may be that I am not doing the broadcast of frame transforms correctly

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2013-10-06 12:09:26 -0500

tfoote gravatar image

updated 2013-10-06 12:22:17 -0500

It looks like you put in a 90 degree yaw at the top. Note that you used the fixed axis RPY method.

That is the right method and the right output. With the euler angle conventions used here(ZYX) yaw is evaluated first, thus a transform of a yaw only will only effect the yaw terms. Your assertion that pitch becomes roll only happens if you don't change your reference frame after applying the transformation. But transformQuaternion puts you into base_link coordinate frame.

edit flag offensive delete link more

Comments

Sorry I meant to say "(new_roll,new_pitch,new_yaw) to be (80,100,120)" not (80,90,120)

obotic gravatar image obotic  ( 2013-10-06 12:15:11 -0500 )edit

Ok I think I understand what you are saying, but let's say my imu was mounted 40deg in the positive yaw relative to /base_link. Now if the imu sees a roll, the /base_link needs to interpret the imu's roll as a combination of pitch and yaw. I know the yaw is 40(I think) but how would I know the pitch

obotic gravatar image obotic  ( 2013-10-06 13:44:34 -0500 )edit

Ahh if you're trying to deal with these rotation rates you don't want to be expressing them as quaternions(positions) but as twists. tf has a basic API for twists, but you will need to understand the math and do it correctly yourself. PS kdl has a good library for doing operations with twists.

tfoote gravatar image tfoote  ( 2013-10-06 19:23:22 -0500 )edit

Thanks I think this may be the solution to our problem, I will test it out and post an update.

obotic gravatar image obotic  ( 2013-10-07 20:43:53 -0500 )edit
1

answered 2013-10-10 01:00:31 -0500

dornhege gravatar image

Besides the coordinate frame problems @tfoote mentioned, your approach is flawed.

You publish the 90 deg mounting as /base_link -> imu_frame, then get that in your IMU node, apply the IMU transform and republish the same. From the outside, i.e. another tf user, this looks to be two different transforms (with/without) IMU orientation applied that contradict each other as both are giving a transformation between the same frames.

You could either include the 90 degree mount in the imu node and apply that only ever sending out the full transformation over tf, or split this up in two transforms. It seems to me that base_link -> imu should never change as it's mounted fixed. Using two transforms also means, you'll never have to apply the transformation, tf will do that.

hector_slam has an example of how such a setup might look like. Check the base_stabilized vs. the base_link.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-10-06 10:36:57 -0500

Seen: 9,884 times

Last updated: Oct 10 '13