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-11 12:48:51 +00:00
parent b8a356da50
commit d9f0ae5cba
1 changed files with 49 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
main.py
View File

@ -226,74 +226,74 @@ if __name__ == "__main__":
cleanup() cleanup()
# hover_distance = 0.1 # meters hover_distance = 0.1 # meters
# # Starting Pose for left arm # Starting Pose for left arm
# left_pose = Pose() left_pose = Pose()
# left_pose.position.x = 0.579679836383 left_pose.position.x = 0.579679836383
# left_pose.position.y = 0.283311769707 left_pose.position.y = 0.283311769707
# left_pose.position.z = 0.213676720426 left_pose.position.z = 0.213676720426
# left_pose.orientation.x = -0.0249590815779 left_pose.orientation.x = -0.0249590815779
# left_pose.orientation.y = 0.999649402929 left_pose.orientation.y = 0.999649402929
# left_pose.orientation.z = 0.00737916180073 left_pose.orientation.z = 0.00737916180073
# left_pose.orientation.w = 0.00486450832011 left_pose.orientation.w = 0.00486450832011
# # Starting Pose for right arm # Starting Pose for right arm
# right_pose = Pose() right_pose = Pose()
# right_pose.position.x = 0.579679836383 right_pose.position.x = 0.579679836383
# right_pose.position.y = -0.283311769707 right_pose.position.y = -0.283311769707
# right_pose.position.z = 0.213676720426 right_pose.position.z = 0.213676720426
# right_pose.orientation.x = -0.0249590815779 right_pose.orientation.x = -0.0249590815779
# right_pose.orientation.y = 0.999649402929 right_pose.orientation.y = 0.999649402929
# right_pose.orientation.z = -0.00737916180073 right_pose.orientation.z = -0.00737916180073
# right_pose.orientation.w = 0.00486450832011 right_pose.orientation.w = 0.00486450832011
# left_pnp = PickAndPlace('left', hover_distance) left_pnp = PickAndPlace('left', hover_distance)
# right_pnp = PickAndPlace('right', hover_distance) right_pnp = PickAndPlace('right', hover_distance)
# # Go to initial position # Go to initial position
# left_pnp.move_to_start(left_pnp.ik_request(left_pose)) left_pnp.move_to_start(left_pnp.ik_request(left_pose))
# right_pnp.move_to_start(right_pnp.ik_request(right_pose)) right_pnp.move_to_start(right_pnp.ik_request(right_pose))
load_objects() load_objects()
# print("loaded all objects, starting tf service thread") print("loaded all objects, starting tf service thread")
# tf_service(init=True) tf_service(init=True)
####################################HACKING BEGIN ###################################HACKING BEGIN
# left_pick = otc.tf_lookup('a3') left_pick = otc.tf_lookup('a3')
# left_pnp.pick(left_pick) left_pnp.pick(left_pick)
# left_place_pre = otc.tf_lookup('t1') left_place_pre = otc.tf_lookup('t1')
# xpos = built_bricks[0] xpos = built_bricks[0]
# zqtr = quaternion_from_euler(xpos['roll'], xpos['pitch'], xpos['yaw']) zqtr = quaternion_from_euler(xpos['roll'], xpos['pitch'], xpos['yaw'])
# left_place_pos = Pose() left_place_pos = Pose()
# left_place_pos.position.x = left_place_pre.position.x + xpos['x'] 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.y = left_place_pre.position.y + xpos['y']
# left_place_pos.position.z = left_place_pre.position.z + xpos['z'] 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.x = left_place_pre.orientation.x + zqtr[0]
# left_place_pos.orientation.y = left_place_pre.orientation.y + zqtr[1] 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.z = left_place_pre.orientation.z + zqtr[2]
# left_place_pos.orientation.w = left_place_pre.orientation.w + zqtr[3] left_place_pos.orientation.w = left_place_pre.orientation.w + zqtr[3]
# left_pnp.place(left_place_pos) left_pnp.place(left_place_pos)
####################################HACKING END ###################################HACKING END
# print('Done with task, enter x to kill tf service') print('Done with task, enter x to kill tf service')
# while running: while running:
# a = raw_input() a = raw_input()
# if a == 'x': if a == 'x':
# print("x has been caught, killing tf service") print("x has been caught, killing tf service")
# running = False running = False
# tf_service(init=False) tf_service(init=False)
print("Done, exiting") print("Done, exiting")