Rotation between two frame similar to interactive markers

edit

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