# Panda Franka Emika Quaternion Calculation

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

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]

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

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


Hi @snice. The one part I don't follow in your code, is what you intent to do with q_equilibrium? I see you are converting correctly from pose to quaternion q_relative. If you add relative rotation, why are you multiplying again?
Hi @osilva , sorry for the late answer. I just realized, today I posted a quite similar question. There, I used some updated code and explained my problem more detailled. But referring to your question, I calculate the relative change of the current rotation and use this change to apply it to the new pose q_equilibrium. In my understanding, for the equilibrium_pose topic to work, I only need to send relative changes from the last pose to the new one.