From 3c8e03e8f771ca39bc3aae6017a411c217c13ba7 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Tue, 11 Feb 2020 12:50:33 +0000 Subject: [PATCH] Update --- main.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/main.py b/main.py index 9c2939e..8f98c8a 100755 --- a/main.py +++ b/main.py @@ -266,19 +266,19 @@ if __name__ == "__main__": left_pick = otc.tf_lookup('a3') left_pnp.pick(left_pick) - left_place_pre = otc.tf_lookup('t1') + left_place_pos = otc.tf_lookup('ea2') - xpos = built_bricks[0] - zqtr = quaternion_from_euler(xpos['roll'], xpos['pitch'], xpos['yaw']) + # xpos = built_bricks[0] + # zqtr = quaternion_from_euler(xpos['roll'], xpos['pitch'], xpos['yaw']) - left_place_pos = Pose() - left_place_pos.position.x = left_place_pre.position.x + xpos['x'] - left_place_pos.position.y = left_place_pre.position.y + xpos['y'] - left_place_pos.position.z = left_place_pre.position.z + xpos['z'] - left_place_pos.orientation.x = left_place_pre.orientation.x + zqtr[0] - left_place_pos.orientation.y = left_place_pre.orientation.y + zqtr[1] - left_place_pos.orientation.z = left_place_pre.orientation.z + zqtr[2] - left_place_pos.orientation.w = left_place_pre.orientation.w + zqtr[3] + # left_place_pos = Pose() + # left_place_pos.position.x = left_place_pre.position.x + xpos['x'] + # left_place_pos.position.y = left_place_pre.position.y + xpos['y'] + # left_place_pos.position.z = left_place_pre.position.z + xpos['z'] + # left_place_pos.orientation.x = left_place_pre.orientation.x + zqtr[0] + # left_place_pos.orientation.y = left_place_pre.orientation.y + zqtr[1] + # left_place_pos.orientation.z = left_place_pre.orientation.z + zqtr[2] + # left_place_pos.orientation.w = left_place_pre.orientation.w + zqtr[3] left_pnp.place(left_place_pos)