# Controlling a quaternion with 6D controller

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.

Originally posted by duffany1 on ROS Answers with karma: 173 on 2014-04-25

Post score: 0

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 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());
return;
}
// apply your 6d rotation to target (hand) orientation
tf::Quaternion q_rot = qs_6d * st_hand.getRotation();
tf::Stamped qs_rot(q_rot, ros::Time(0), "/left_hand");

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


Originally posted by kramer with karma: 1470 on 2014-08-26

This answer was NOT ACCEPTED on the original site

Post score: 1

Comment by duffany1 on 2014-08-26:
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).

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


to

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


Originally posted by duffany1 with karma: 173 on 2014-08-26

This answer was ACCEPTED on the original site

Post score: 0