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