From 47b908f9acea408f829e8d7bf5673dfc73f66066 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Thu, 13 Feb 2020 00:01:25 +0000 Subject: [PATCH] Update --- main.py | 30 ------------------------------ object_tf_client.py | 6 ++++-- 2 files changed, 4 insertions(+), 32 deletions(-) diff --git a/main.py b/main.py index 3ba038b..83e3b02 100755 --- a/main.py +++ b/main.py @@ -265,41 +265,11 @@ if __name__ == "__main__": left_pick = otc.tf_lookup('a1') - left_pick_angles = euler_from_quaternion([left_pick.orientation.x , left_pick.orientation.y , left_pick.orientation.z, left_pick.orientation.w]) - xangle = left_pick_angles[0] + radians(-90) - xangle1 = left_pick_angles[1] + radians(-90) - xangle2 = left_pick_angles[2] + radians(-180) - left_pick_angles = (xangle, left_pick_angles[1], left_pick_angles[2]) - target_quat = quaternion_from_euler(left_pick_angles[0], left_pick_angles[1], left_pick_angles[2]) - left_pick_proc = Pose() - left_pick_proc.position.x = left_pick.position.x - 0.05 - left_pick_proc.position.y = left_pick.position.y - left_pick_proc.position.z = left_pick.position.z - left_pick_proc.orientation.x = target_quat[0] - left_pick_proc.orientation.y = target_quat[1] - left_pick_proc.orientation.z = target_quat[2] - left_pick_proc.orientation.w = target_quat[3] - left_pnp.pick(left_pick_proc) left_place = otc.tf_lookup('f2') - left_place_angles = euler_from_quaternion([left_place.orientation.x , left_place.orientation.y , left_place.orientation.z, left_place.orientation.w]) - xangle = left_place_angles[0]+ radians(180) - xangle1 = left_place_angles[1] - xangle2 = left_place_angles[2] - left_place_angles = (xangle, left_place_angles[1], left_place_angles[2]) - target_quat = quaternion_from_euler(left_place_angles[0], left_place_angles[1], left_pick_angles[2]) - left_place_pos = Pose() - left_place_pos.position.x = left_place.position.x - left_place_pos.position.y = left_place.position.y - left_place_pos.position.z = left_place.position.z - left_place_pos.orientation.x = target_quat[0] - left_place_pos.orientation.y = target_quat[1] - left_place_pos.orientation.z = target_quat[2] - left_place_pos.orientation.w = target_quat[3] - left_pnp.place(left_place_pos) diff --git a/object_tf_client.py b/object_tf_client.py index d09995e..c32e1f4 100755 --- a/object_tf_client.py +++ b/object_tf_client.py @@ -19,8 +19,10 @@ def tf_lookup(object_name): continue object_angles = tf.transformations.euler_from_quaternion([rot[0] , rot[1] , rot[2], rot[3]]) - xangle = object_angles[0] + radians(90) - object_angles = (xangle, object_angles[1], object_angles[2]) + xangle = object_angles[0] + xangle1 = object_angles[1] + xangle2 = object_angles[2] + object_angles = (xangle, xangle1, xangle2) target_quat = tf.transformations.quaternion_from_euler(object_angles[0], object_angles[1], object_angles[2]) target_pose = Pose() target_pose.position.x = trans[0]