From 5b92b95217ec9b39b8d84f9b8fb23e3390099aa9 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Thu, 13 Feb 2020 15:24:21 +0000 Subject: [PATCH] Update --- object_tf_client.py | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/object_tf_client.py b/object_tf_client.py index ec3da8a..6026fa9 100755 --- a/object_tf_client.py +++ b/object_tf_client.py @@ -4,6 +4,7 @@ import tf import time from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) from math import radians +from numpy import matmul def tf_lookup(object_name): do_try = True @@ -25,15 +26,9 @@ def tf_lookup(object_name): print('@@@@@@@@@@@@@@@@') print(object_angles) print('@@@@@@@@@@@@@@@@') - # xangle = object_angles[0]# + radians(360) - # xangle1 = object_angles[1]# + radians(90) - # xangle2 = object_angles[2]# + radians(-106) - # print("Angles:") - # print(xangle) - # print(xangle1) - # print(xangle2) - # print() - target_quat = tf.transformations.quaternion_from_matrix(object_angles) + Re = euler_matrix(radians(180), radians(0), radians(-270), 'rxyz') + updated_angles = matmul(Re, object_angles) + target_quat = tf.transformations.quaternion_from_matrix(updated_angles) target_pose = Pose() target_pose.position.x = trans[0] target_pose.position.y = trans[1]