Rotation between two frame similar to interactive markers - python-2.7

Question

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

Answer 1

Okay, 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)