max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-13 15:24:21 +00:00
parent 2c2c78e822
commit 5b92b95217
1 changed files with 4 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
object_tf_client.py
View File

@ -4,6 +4,7 @@ import tf
import time import time
from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion) from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
from math import radians from math import radians
from numpy import matmul
def tf_lookup(object_name): def tf_lookup(object_name):
do_try = True do_try = True
@ -25,15 +26,9 @@ def tf_lookup(object_name):
print('@@@@@@@@@@@@@@@@') print('@@@@@@@@@@@@@@@@')
print(object_angles) print(object_angles)
print('@@@@@@@@@@@@@@@@') print('@@@@@@@@@@@@@@@@')
# xangle = object_angles[0]# + radians(360) Re = euler_matrix(radians(180), radians(0), radians(-270), 'rxyz')
# xangle1 = object_angles[1]# + radians(90) updated_angles = matmul(Re, object_angles)
# xangle2 = object_angles[2]# + radians(-106) target_quat = tf.transformations.quaternion_from_matrix(updated_angles)
# print("Angles:")
# print(xangle)
# print(xangle1)
# print(xangle2)
# print()
target_quat = tf.transformations.quaternion_from_matrix(object_angles)
target_pose = Pose() target_pose = Pose()
target_pose.position.x = trans[0] target_pose.position.x = trans[0]
target_pose.position.y = trans[1] target_pose.position.y = trans[1]