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 00:02:53 +00:00
parent 9aafff307b
commit d75893c66a
2 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
bricks_etc.py
View File

@ -301,3 +301,6 @@ def getNodes():
nodes.append(node_name) nodes.append(node_name)
return nodes return nodes
def getTargets():
return target_positions

18
main.py
View File

@ -28,6 +28,7 @@ running = True
bricks = bet.getBuildable() bricks = bet.getBuildable()
table = bet.getTable() table = bet.getTable()
brick_targets = bet.getTargets()
class PickAndPlace(object): class PickAndPlace(object):
@ -255,9 +256,20 @@ if __name__ == "__main__":
left_pnp.pick(left_pick) left_pnp.pick(left_pick)
left_place_pre = otc.tf_lookup('t1') left_place_pre = otc.tf_lookup('t1')
print()
print(left_place_pre) xpos = brick_targets[0]
print() 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 ####################################HACKING END