From d75893c66a087522f0a13b131d49cd0b0273252f Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Tue, 11 Feb 2020 00:02:53 +0000 Subject: [PATCH] Update --- bricks_etc.py | 3 +++ main.py | 18 +++++++++++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/bricks_etc.py b/bricks_etc.py index 0f6df4f..12f6b74 100644 --- a/bricks_etc.py +++ b/bricks_etc.py @@ -301,3 +301,6 @@ def getNodes(): nodes.append(node_name) return nodes +def getTargets(): + return target_positions + diff --git a/main.py b/main.py index 892feef..1afd3bf 100755 --- a/main.py +++ b/main.py @@ -28,6 +28,7 @@ running = True bricks = bet.getBuildable() table = bet.getTable() +brick_targets = bet.getTargets() class PickAndPlace(object): @@ -255,9 +256,20 @@ if __name__ == "__main__": left_pnp.pick(left_pick) left_place_pre = otc.tf_lookup('t1') - print() - print(left_place_pre) - print() + + xpos = brick_targets[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_pnp.place(left_place_pos) ####################################HACKING END