Ask Your Question

Controlling a quaternion with 6D controller

asked 2014-04-25 13:39:12 -0500

duffany1 gravatar image

updated 2014-04-27 07:10:41 -0500

To control a robotic arm, I am computing a geometry_msgs::Pose to send to the setFromIK method in ROS control. The Pose has a position and a quaternion. What I am trying to do is control the pose in realtime using a spacenav controller, which has 3 positional and 3 rotational axes.

I am able to set the x, y, and z of the position part of the pose just fine. I am also able to set the quaternion just fine as well, but my current problem is that controlling the quaternion is extremely unintuitive, since rotation is not commutative. i.e., if I rotate 180 degrees about the x-axis, then the control of the rotation about the z-axis gets inverted.

Is there any way that I can control the rotation from a sort of "global reference frame," if that makes any sense? I would like it so that if I twist the joystick clockwise about z, the pose will rotate accordingly, instead of "sometimes rotating clockwise, sometimes rotating counterclockwise."

Here is what I'm currently doing (I've removed everything not related to the quaternion):

while (true) {
  // ... Calculate position ...
  double roll  =  joy_axes[3];
  double pitch =  joy_axes[4];
  double yaw   = -joy_axes[5];
  current_quaternion = last_quaternion * tf::createQuaternionFromRPY(roll, pitch, yaw);
  tf::quaternionTFToMsg(current_quaternion, current_pose.orientation);
  // ... Compute inverse kinematics; convert pose to PoseStamped, publish resulting pose if a solution was found ...

By the way, I am converting the pose to a PoseStamped, with frame_id of "base_link." This may or may not have anything to do with how the quaternion is controlled.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2014-08-26 23:22:41 -0500

duffany1 gravatar image

I wound up solving this problem. The fix was simple, though I'm not too familiar with the mathematical reasoning behind why this solution works, other than the non-commutativity of matrix multiplication. I needed to change

current_quaternion = last_quaternion * tf::createQuaternionFromRPY(roll, pitch, yaw);


current_quaternion = tf::createQuaternionFromRPY(roll, pitch, yaw) * last_quaternion;
edit flag offensive delete link more

answered 2014-08-26 22:50:13 -0500

kramer gravatar image

I don't know if you've resolved this or not, but I just came across the question. You just need to do some TF manipulation with the appropriate frames. Here's one way to do it; I'm leaving the following in long form for clarity...sorry about the variable names, but I think they're distinguishable enough (and I think I kept 'em consistent).

Let's say you want to rotate left_hand in the map frame:

// set up, note that roll, pitch, and yaw are doubles you captured
// you may need to manipulate the values to make sense in radians
tf::TransformListener tf_;
tf::Quaternion q_6d = tf::createQuaternionFromRPY(roll, pitch, yaw);
tf::Stamped<tf::quaternion> qs_6d(q_6d, ros::Time(0), "/left_hand");

// get the transform from source (map) to target (hand) frame
tf::StampedTransform st_hand;
try {
    tf->lookupTransform("/left_hand", "/map", ros::Time(0), st_hand);
} catch (tf::TransformException &e) {
    ROS_ERROR("shift orientation frame: [%s]", e.what());
// apply your 6d rotation to target (hand) orientation
tf::Quaternion q_rot = qs_6d * st_hand.getRotation();
tf::Stamped<tf::quaternion> qs_rot(q_rot, ros::Time(0), "/left_hand");

// put rotated target (hand) quaternion back in source (map) frame
tf::Stamped<tf::quaternion> sq_map;
try {
    tf->transformQuaternion("/map", qs_rot, sq_map);
} catch (tf::TransformException &e) {
    ROS_ERROR("shift orientation quat: [%s]", e.what());
// convert to quaternion stamped message, can put that in pose...
geometry_msgs::QuaternionStamped qs_msg;
tf::quaternionStampedTFToMsg(sq_map, qs_msg);
edit flag offensive delete link more


I actually did wind up solving this problem, and the solution was much simpler than I thought it would be (in my case, at least). I'll add it as an answer for future reference (something I should have done months ago).

duffany1 gravatar image duffany1  ( 2014-08-26 23:18:23 -0500 )edit

Your Answer

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

Add Answer

Question Tools


Asked: 2014-04-25 13:39:12 -0500

Seen: 837 times

Last updated: Aug 26 '14