Skip to content
Snippets Groups Projects
Commit c8fc3ad0 authored by Simon Gabl's avatar Simon Gabl
Browse files

Revert interactive marker pose to O_T_EE.

parent e6b011d3
No related branches found
No related tags found
No related merge requests found
......@@ -27,16 +27,16 @@ def publisherCallback(msg, arm_id):
def franka_state_callback(msg):
initial_quaternion = \
tf.transformations.quaternion_from_matrix(
np.transpose(np.reshape(msg.O_T_EE_d,
np.transpose(np.reshape(msg.O_T_EE,
(4, 4))))
initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
marker_pose.pose.orientation.x = initial_quaternion[0]
marker_pose.pose.orientation.y = initial_quaternion[1]
marker_pose.pose.orientation.z = initial_quaternion[2]
marker_pose.pose.orientation.w = initial_quaternion[3]
marker_pose.pose.position.x = msg.O_T_EE_d[12]
marker_pose.pose.position.y = msg.O_T_EE_d[13]
marker_pose.pose.position.z = msg.O_T_EE_d[14]
marker_pose.pose.position.x = msg.O_T_EE[12]
marker_pose.pose.position.y = msg.O_T_EE[13]
marker_pose.pose.position.z = msg.O_T_EE[14]
global initial_pose_found
initial_pose_found = True
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment