Controlling a quaternion with 6D controller

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

duffany1
173 ●10 ●12 ●18

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

add a comment

2 Answers

Sort by » oldest newest most voted

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

duffany1
173 ●10 ●12 ●18

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

add a comment

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

kramer
1470 ●3 ●14 ●23

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());
    return;
}
// 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());
    return;
}
// 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

Comments

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 ( 2014-08-26 23:18:23 -0500 )edit

add a comment

Controlling a quaternion with 6D controller

2 Answers

Comments

Question Tools

Stats

Related questions

Controlling a quaternion with 6D controller edit

2 Answers

Comments

Question Tools

Stats

Related questions

Controlling a quaternion with 6D controller