ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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 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[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
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[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:
        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 ... (more)

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

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)

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

Comments

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

Question Tools

2 followers

Stats

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

Seen: 182 times

Last updated: Nov 03 '21