ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# Rotation between two frame similar to interactive markers ### 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
eq_pose.pose.orientation.y = q_equilibrium
eq_pose.pose.orientation.z = q_equilibrium
eq_pose.pose.orientation.w = q_equilibrium

# 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
eq_pose.pose.orientation.y = initial_quaternion
eq_pose.pose.orientation.z = initial_quaternion
eq_pose.pose.orientation.w = initial_quaternion
eq_pose.pose.position.x = msg.O_T_EE
eq_pose.pose.position.y = msg.O_T_EE
eq_pose.pose.position.z = msg.O_T_EE

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

edit retag close merge delete

Sort by » oldest newest most voted

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)


to

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.

EDIT:

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)

more

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