Panda Franka Emika Quaternion Calculation

edit

Hi,

What do I want to do?

Real time teleoperation of a panda franka emika.

Problem

During my search, I found the interactive_marker demo with the cartesian_impedance_example_controller of the tutorial from the franka emika website.

I now want to move each rotation of the gripper independently, like moving the "circles" in the interactive_marker example.

I somehow cannot manage to make the rotation look like the ones by moving the interactive marker. Does someone have any tips in achieving this?

My approach looks like this:

My first take was to use the rotations around the axes (pitch, roll, yaw) and give them as input for the quaternion function:

import numpy as np
import tf

# 90 degree rotation around one axis
pitch = np.radians(90)
roll = np.radians(0)
yaw = np.radians(0)

quaternion = tf.transformations.quaternion_from_euler(pitch, yaw, roll)

The transformation of this quaternion and forwarding to the equilibrium_pose topic happens exemplary with this functions:

def calculate_quaternion(eq_pose):
    # calculate the relative quaternion from the last pose to the new pose 
    # (see http://wiki.ros.org/tf2/Tutorials/Quaternions)
    q_2 = [current_pose.pose.orientation.x, current_pose.pose.orientation.y,
          current_pose.pose.orientation.z, current_pose.pose.orientation.w]
    # Negate w value for inverse
    q_1_inv = [last_pose.pose.orientation.x, last_pose.pose.orientation.y,
              last_pose.pose.orientation.z, (-1)*last_pose.pose.orientation.w]

    q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)

    # 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]    

    if not lock_orientation:
        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]


def publisher_callback(msg, link_name):
    global initial_pose_found
    global last_pose
    global current_pose
    global eq_pose

    if not initial_pose_found or not initial_eq_pose_found:
        return

    # If the time difference of two consecutive messages is bigger than 1/2 second, skip publishing
    # and wait for the next.
    time_between_messages = current_pose.header.stamp - last_pose.header.stamp
    if (time_between_messages.to_sec() >= 0.5):
        initial_pose_found = False
        return

    # calculate position deltas
    d_position = Point()
    d_position.x = diff(current_pose.pose.position.x, last_pose.pose.position.x)
    d_position.y = diff(current_pose.pose.position.y, last_pose.pose.position.y)
    d_position.z = diff(current_pose.pose.position.z, last_pose.pose.position.z)

    eq_pose.header.frame_id = link_name
    eq_pose.header.stamp = rospy.get_rostime()

    # add deltas to equilibrium pose
    if not lock_position:
        eq_pose.pose.position.x = d_position.x
        eq_pose.pose.position.y = d_position.y
        eq_pose.pose.position.z = d_position.z

    eq_pose = calculate_quaternion(eq_pose)

    # update last pose
    last_pose = current_pose

    eq_publisher.publish(eq_pose)

if __name__ == "__main__":
    eq_publisher = rospy.Publisher("/panda/cartesian_impedance_example_controller/equilibrium_pose", PoseStamped, queue_size=10)

    rospy.Timer(rospy.Duration(publishing_rate),lambda msg: publisher_callback(msg, link_name))