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:15:40 +00:00
parent a64d19e7af
commit e3f750ec08
1 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
object_tf_client.py
View File

@ -21,16 +21,19 @@ def tf_lookup(object_name):
print('@@@@@@@@@@@@@@@@')
print(rot)
print('@@@@@@@@@@@@@@@@')
object_angles = tf.transformations.euler_from_quaternion([rot[2] , rot[0], rot[1], rot[2]], axes='sxyz')
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_euler(xangle, xangle1, xangle2)
object_angles = tf.transformations.matrix_from_quaternion([rot[0] , rot[1], rot[2], rot[3]])
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)
target_pose = Pose()
target_pose.position.x = trans[0]
target_pose.position.y = trans[1]