ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# 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.

edit retag close merge delete

Sort by ยป oldest newest most voted

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;

more

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

more