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.0Fri, 29 Oct 2021 08:25:44 -0500Rotation between two frame similar to interactive markershttps://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/
### What do I want to do?
I work with a Franka Emika Panda and use the "cartesian_impedance_example_controller" with its "equilibrium_pose" topic to move the panda arm.
I want to use a command to rotate the arm along its axes of the "panda_rightfinger" joint axes (axis of interactive marker seen in picture). The roation only happens around the axis and happens by pressing a specific button.
[![enter image description here][1]][1]
(Right finger frame with the interactive marker around it and panda_link0 frame on the left)
### How do I do it?
The rotation quaternion gets created by a function that uses following script:
axis = {
"roll": 0,
"pitch": 0,
"yaw": 0
}
def pyr_producer(self, gesture_msg):
global axis
axis[gesture_msg.cls] += 1 * 0.01
return list(axis.values())
def get_quaternion(self, gesture_msg):
roll, pitch, yaw = pyr_producer(gesture_msg)
q_rot = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
return Quaternion(*q_rot)
Afterwards, this rotation quaterion will be used by another script and gets published to the corresponding equilibrium_pose topic.
This part of the script calculates the rotation:
- `eq_pose`: the new pose that will be used for the topic
- `current_goal_pose`: the pose that contains the actual rotation
- `last_goal_pose`: the pose that contains the last rotation
eq_pose.pose.position = last_goal_pose.pose.position
eq_pose.pose.orientation = orientation_producer.get_quaternion(goal_pose.gesture)
# calculate the relative quaternion from the last pose to the new pose
# (see http://wiki.ros.org/tf2/Tutorials/Quaternions)
# add relative rotation quaternion to the new equilibrium orientation by multiplying
q_equilibrium = [eq_pose.pose.orientation.x, eq_pose.pose.orientation.y,
eq_pose.pose.orientation.z, eq_pose.pose.orientation.w]
q_2 = [current_goal_pose.pose.orientation.x, current_goal_pose.pose.orientation.y,
current_goal_pose.pose.orientation.z, current_goal_pose.pose.orientation.w]
# Negate w value for inverse
q_1_inv = [last_goal_pose.pose.orientation.x, last_goal_pose.pose.orientation.y,
last_goal_pose.pose.orientation.z, (-1)*last_goal_pose.pose.orientation.w]
q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)
q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
eq_pose.pose.orientation.x = q_equilibrium[0]
eq_pose.pose.orientation.y = q_equilibrium[1]
eq_pose.pose.orientation.z = q_equilibrium[2]
eq_pose.pose.orientation.w = q_equilibrium[3]
# update last pose
last_goal_pose = current_goal_pose
# Only publish poses when there is an interaction
eq_publisher.publish(eq_pose)
The eq_pose gets generated by this part:
def franka_state_callback(msg):
global eq_pose
global initial_eq_pose_found
# the initial pose has to be retrieved only once
if initial_eq_pose_found:
return
initial_quaternion = \
tf.transformations.quaternion_from_matrix(
np.transpose(np.reshape(msg.O_T_EE,
(4, 4))))
initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
eq_pose.pose.orientation.x = initial_quaternion[0]
eq_pose.pose.orientation.y = initial_quaternion[1]
eq_pose.pose.orientation.z = initial_quaternion[2]
eq_pose.pose.orientation.w = initial_quaternion[3]
eq_pose.pose.position.x = msg.O_T_EE[12]
eq_pose.pose.position.y = msg.O_T_EE[13]
eq_pose.pose.position.z = msg.O_T_EE[14]
initial_eq_pose_found = True
rospy.loginfo("Initial panda pose found: " + str(initial_eq_pose_found))
rospy.loginfo("Initial panda pose: " + str(eq_pose))
if __name__ == "__main__":
state_sub = rospy.Subscriber("/panda/franka_state_controller/franka_states", FrankaState, franka_state_callback)
while not initial_eq_pose_found:
rospy.sleep(1)
state_sub.unregister()
### What actually happens
The rotation itself works, but only happens around the "panda_link0" axis, which is the fixed position of the panda foot. The rotation should be the same like the one around the interactive marker in the [interactive marker example](https://frankaemika.github.io/docs/franka_ros.html#pick-place-example).
### Final Question
So I want to know, how to calculate the quaternions for this rotation?
I am quite new to robotics and hope my description was clear.
[1]: https://i.stack.imgur.com/48dJY.png
Fri, 29 Oct 2021 04:52:22 -0500https://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/Answer by snice for <div class="snippet"><h3>What do I want to do?</h3>
<p>I work with a Franka Emika Panda and use the "cartesian_impedance_example_controller" with its "equilibrium_pose" topic to move the panda arm.</p>
<p>I want to use a command to rotate the arm along its axes of the "panda_rightfinger" joint axes (axis of interactive marker seen in picture). The roation only happens around the axis and happens by pressing a specific button.</p>
<p><a href="https://i.stack.imgur.com/48dJY.png"><img src="https://i.stack.imgur.com/48dJY.png" alt="enter image description here"></a></p>
<p>(Right finger frame with the interactive marker around it and panda_link0 frame on the left)</p>
<h3>How do I do it?</h3>
<p>The rotation quaternion gets created by a function that uses following script:</p>
<pre><code>axis = {
"roll": 0,
"pitch": 0,
"yaw": 0
}
def pyr_producer(self, gesture_msg):
global axis
axis[gesture_msg.cls] += 1 * 0.01
return list(axis.values())
def get_quaternion(self, gesture_msg):
roll, pitch, yaw = pyr_producer(gesture_msg)
q_rot = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
return Quaternion(*q_rot)
</code></pre>
<p>Afterwards, this rotation quaterion will be used by another script and gets published to the corresponding equilibrium_pose topic.</p>
<p>This part of the script calculates the rotation:
- <code>eq_pose</code>: the new pose that will be used for the topic
- <code>current_goal_pose</code>: the pose that contains the actual rotation <br>
- <code>last_goal_pose</code>: the pose that contains the last rotation </p>
<pre><code>eq_pose.pose.position = last_goal_pose.pose.position
eq_pose.pose.orientation = orientation_producer.get_quaternion(goal_pose.gesture)
# calculate the relative quaternion from the last pose to the new pose
# (see http://wiki.ros.org/tf2/Tutorials/Quaternions)
# add relative rotation quaternion to the new equilibrium orientation by multiplying
q_equilibrium = [eq_pose.pose.orientation.x, eq_pose.pose.orientation.y,
eq_pose.pose.orientation.z, eq_pose.pose.orientation.w]
q_2 = [current_goal_pose.pose.orientation.x, current_goal_pose.pose.orientation.y,
current_goal_pose.pose.orientation.z, current_goal_pose.pose.orientation.w]
# Negate w value for inverse
q_1_inv = [last_goal_pose.pose.orientation.x, last_goal_pose.pose.orientation.y,
last_goal_pose.pose.orientation.z, (-1)*last_goal_pose.pose.orientation.w]
q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)
q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
eq_pose.pose.orientation.x = q_equilibrium[0]
eq_pose.pose.orientation.y = q_equilibrium[1]
eq_pose.pose.orientation.z = q_equilibrium[2]
eq_pose.pose.orientation.w = q_equilibrium[3]
# update last pose
last_goal_pose = current_goal_pose
# Only publish poses when there is an interaction
eq_publisher.publish(eq_pose)
</code></pre>
<p>The eq_pose gets generated by this part: </p>
<pre><code>def franka_state_callback(msg):
global eq_pose
global initial_eq_pose_found
# the initial pose has to be retrieved only once
if initial_eq_pose_found:
return
initial_quaternion = \
tf.transformations.quaternion_from_matrix(
np.transpose(np.reshape(msg.O_T_EE,
(4, 4))))
initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
eq_pose.pose.orientation.x = initial_quaternion[0]
eq_pose.pose.orientation.y = initial_quaternion[1]
eq_pose.pose.orientation.z = initial_quaternion[2]
eq_pose.pose.orientation.w = initial_quaternion[3]
eq_pose.pose.position.x = msg.O_T_EE[12]
eq_pose.pose.position.y = msg.O_T_EE[13]
eq_pose.pose.position.z = msg.O_T_EE[14]
initial_eq_pose_found = True
rospy.loginfo("Initial panda pose found: " + str(initial_eq_pose_found))
rospy.loginfo("Initial panda pose: " + str(eq_pose))
if __name__ == "__main__":
state_sub = rospy.Subscriber("/panda/franka_state_controller/franka_states", FrankaState, franka_state_callback)
while not initial_eq_pose_found:
rospy.sleep(1)
state_sub.unregister()
</code></pre>
<h3>What actually happens</h3>
<p>The rotation itself works, but only happens around the "panda_link0" axis, which is the fixed position of the panda foot. The rotation should be the same like ...<span class="expander"> <a>(more)</a></span></p></div> https://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/?answer=389628#post-id-389628Okay, I just found my mistake, as expected, it was very easy:
The multiplication of quaternions is not cummutative. With respect to that, I just had to change the calculation of the quaternion from
q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
to
q_equilibrium = tf.transformations.quaternion_multiply(q_equilibrium,q_relative)
If we change the order of the multiplication, we also change the frames which we want to rotate.
EDIT:
I also had to change from this:
q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)
to this
q_relative = tf.transformations.quaternion_multiply(q_1_inv, q_2)Fri, 29 Oct 2021 07:50:19 -0500https://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/?answer=389628#post-id-389628Comment by osilva for <p>Okay, I just found my mistake, as expected, it was very easy:</p>
<p>The multiplication of quaternions is not cummutative. With respect to that, I just had to change the calculation of the quaternion from </p>
<pre><code>q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
</code></pre>
<p>to</p>
<pre><code>q_equilibrium = tf.transformations.quaternion_multiply(q_equilibrium,q_relative)
</code></pre>
<p>If we change the order of the multiplication, we also change the frames which we want to rotate.</p>
<p>EDIT: </p>
<p>I also had to change from this:</p>
<pre><code>q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)
</code></pre>
<p>to this </p>
<pre><code>q_relative = tf.transformations.quaternion_multiply(q_1_inv, q_2)
</code></pre>
https://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/?comment=389631#post-id-389631Glad you got it. That’s the part it wasn’t making sense to me why you were applying the extra rotation.Fri, 29 Oct 2021 08:25:44 -0500https://answers.ros.org/question/389623/rotation-between-two-frame-similar-to-interactive-markers/?comment=389631#post-id-389631