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-15 01:29:54 +00:00
parent 9651db5c63
commit 25dc210510
1 changed files with 29 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
new_main.py
View File

@ -160,7 +160,7 @@ def cleanup():
for obj in brick_ids: for obj in brick_ids:
delete_model(obj) delete_model(obj)
def spawn_brick(vertical=True): def spawn_v_brick():
brick_pose = Pose() brick_pose = Pose()
if vertical: if vertical:
brick_pose.position.x = 0.485 brick_pose.position.x = 0.485
@ -170,7 +170,12 @@ def spawn_brick(vertical=True):
brick_pose.orientation.y = 0.5 brick_pose.orientation.y = 0.5
brick_pose.orientation.z = 0.5 brick_pose.orientation.z = 0.5
brick_pose.orientation.w = 0.5 brick_pose.orientation.w = 0.5
else: brick_reference_frame = 'world'
brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
def spawn_h_brick():
brick_pose = Pose()
brick_pose.position.x = 0.4664 brick_pose.position.x = 0.4664
brick_pose.position.y = 0.8069 brick_pose.position.y = 0.8069
brick_pose.position.z = 0.7533 brick_pose.position.z = 0.7533
@ -178,7 +183,6 @@ def spawn_brick(vertical=True):
brick_pose.orientation.y = 0 brick_pose.orientation.y = 0
brick_pose.orientation.z = 0.707 brick_pose.orientation.z = 0.707
brick_pose.orientation.w = 0.707 brick_pose.orientation.w = 0.707
brick_reference_frame = 'world' brick_reference_frame = 'world'
brick_id = brick_ids.pop() brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame) spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
@ -194,30 +198,30 @@ hover_distance = 0.2
left_pnp = PickAndPlace('left', hover_distance) left_pnp = PickAndPlace('left', hover_distance)
spawn_brick(vertical=True) spawn_v_brick()
left_pnp.pick(brickstuff[0]['pose']) left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[2]['pose']) left_pnp.place(brickstuff[2]['pose'])
spawn_brick(vertical=True) spawn_v_brick()
left_pnp.pick(brickstuff[0]['pose']) left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[3]['pose']) left_pnp.place(brickstuff[3]['pose'])
spawn_brick(vertical=True) spawn_v_brick()
left_pnp.pick(brickstuff[0]['pose']) left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[4]['pose']) left_pnp.place(brickstuff[4]['pose'])
spawn_brick(vertical=False) spawn_h_brick()
left_pnp.pick(brickstuff[1]['pose']) left_pnp.pick(brickstuff[1]['pose'])
left_pnp.place(brickstuff[5]['pose']) left_pnp.place(brickstuff[5]['pose'])
# spawn_brick(vertical=False) # spawn_h_brick()
# left_pnp.pick(brickstuff[1]['pose']) # left_pnp.pick(brickstuff[1]['pose'])
# left_pnp.place(brickstuff[6]['pose']) # left_pnp.place(brickstuff[6]['pose'])
# spawn_brick(vertical=True) # spawn_v_brick()
# left_pnp.pick(brickstuff[0]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[7]['pose']) # left_pnp.place(brickstuff[7]['pose'])
# spawn_brick(vertical=True) # spawn_v_brick()
# left_pnp.pick(brickstuff[0]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[8]['pose']) # left_pnp.place(brickstuff[8]['pose'])
# spawn_brick(vertical=False) # spawn_h_brick()
# left_pnp.pick(brickstuff[1]['pose']) # left_pnp.pick(brickstuff[1]['pose'])
# left_pnp.place(brickstuff[9]['pose']) # left_pnp.place(brickstuff[9]['pose'])
# spawn_brick(vertical=True) # spawn_v_brick()
# left_pnp.pick(brickstuff[0]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[10]['pose']) # left_pnp.place(brickstuff[10]['pose'])