Ask Your Question

Rotation between two frame similar to interactive markers

asked 2021-10-29 04:52:22 -0500

snice gravatar image

updated 2022-05-01 13:04:58 -0500

lucasw gravatar image

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

(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

# 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

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:
    initial_quaternion = \
                                    (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:

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)

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2021-10-29 07:50:19 -0500

snice gravatar image

updated 2021-11-03 10:02:05 -0500

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)


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.


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)
edit flag offensive delete link more


Glad you got it. That’s the part it wasn’t making sense to me why you were applying the extra rotation.

osilva gravatar image osilva  ( 2021-10-29 08:25:44 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2021-10-29 04:52:22 -0500

Seen: 48 times

Last updated: Nov 03 '21