From c8fc3ad0d34ce55fefc678c300fcf5f44b973353 Mon Sep 17 00:00:00 2001 From: Simon Gabl <simon.gabl@franka.de> Date: Thu, 25 Jan 2018 10:23:22 +0100 Subject: [PATCH] Revert interactive marker pose to O_T_EE. --- .../launch/scripts/interactive_marker.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/franka_example_controllers/launch/scripts/interactive_marker.py b/franka_example_controllers/launch/scripts/interactive_marker.py index 55d163f..379a7ca 100755 --- a/franka_example_controllers/launch/scripts/interactive_marker.py +++ b/franka_example_controllers/launch/scripts/interactive_marker.py @@ -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 -- GitLab