ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 26 Aug 2014 23:22:41 -0500Controlling a quaternion with 6D controllerhttps://answers.ros.org/question/157626/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.Fri, 25 Apr 2014 13:39:12 -0500https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/Answer by kramer for <p>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.</p>
<p>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.</p>
<p>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."</p>
<p>Here is what I'm currently doing (I've removed everything not related to the quaternion):</p>
<pre><code>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 ...
}
</code></pre>
<p>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.</p>
https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?answer=191340#post-id-191340I 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 <i>think</i> I kept 'em consistent).
Let's say you want to rotate <code>left_hand</code> in the <code>map</code> frame:
<pre><code>// 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);
</code></pre>Tue, 26 Aug 2014 22:50:13 -0500https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?answer=191340#post-id-191340Comment by duffany1 for <p>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 <i>think</i> I kept 'em consistent).</p>
<p>Let's say you want to rotate <code>left_hand</code> in the <code>map</code> frame:</p>
<pre><code>// 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);
</code></pre>
https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?comment=191341#post-id-191341I 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).Tue, 26 Aug 2014 23:18:23 -0500https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?comment=191341#post-id-191341Answer by duffany1 for <p>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.</p>
<p>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.</p>
<p>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."</p>
<p>Here is what I'm currently doing (I've removed everything not related to the quaternion):</p>
<pre><code>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 ...
}
</code></pre>
<p>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.</p>
https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?answer=191342#post-id-191342I 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;Tue, 26 Aug 2014 23:22:41 -0500https://answers.ros.org/question/157626/controlling-a-quaternion-with-6d-controller/?answer=191342#post-id-191342