From 11568a025d44eb30463756686d515e899a6a3e87 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Sat, 8 Feb 2020 17:52:43 +0000 Subject: [PATCH] Update --- main.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/main.py b/main.py index 5ef8aee..1484917 100755 --- a/main.py +++ b/main.py @@ -397,3 +397,14 @@ right_pnp = PickAndPlace('right', hover_distance) left_pnp.move_to_start(left_pnp.ik_request(left_pose)) right_pnp.move_to_start(right_pnp.ik_request(right_pose)) + +pose1 = Pose() +pose1.position = Point(x=bricks_start[0]['x'], y=bricks_start[0]['y'], z=bricks_start[0]['z']) +hqo = quaternion_from_euler(bricks_start[0]['roll'], bricks_start[0]['pitch'], bricks_start[0]['yaw']) +pose1.orientation.orientation = Quaternion(hqo[0], hqo[1], hqo[2], hqo[3]) + +right_pnp.move_to_start(right_pnp.ik_request(pose1)) + + + +