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-19 11:08:41 +00:00
parent 8133fb5d6f
commit acba8487e0
1 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
new_main.py
View File

@ -86,31 +86,31 @@ def etq(roll, pitch, yaw):
return [qx, qy, qz, qw] return [qx, qy, qz, qw]
def spawn_v_brick(): def spawn_v_brick():
brick_pose = Pose() brick_pose = Pose()
brick_pose.position.x = 0.475 brick_pose.position.x = 0.475
brick_pose.position.y = 0.759 brick_pose.position.y = 0.759
brick_pose.position.z = 0.818 brick_pose.position.z = 0.818
QUATS = etq(0, 1.57, 1.57) QUATS = etq(0, 1.57, 1.57)
brick_pose.orientation.x = QUATS[0] brick_pose.orientation.x = QUATS[0]
brick_pose.orientation.y = QUATS[1] brick_pose.orientation.y = QUATS[1]
brick_pose.orientation.z = QUATS[2] brick_pose.orientation.z = QUATS[2]
brick_pose.orientation.w = QUATS[3] brick_pose.orientation.w = QUATS[3]
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)
def spawn_h_brick(): def spawn_h_brick():
brick_pose = Pose() 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
brick_pose.orientation.x = 0 brick_pose.orientation.x = 0
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)
def spawn_tables(): def spawn_tables():
table1 = Pose() table1 = Pose()
@ -146,28 +146,28 @@ left_pnp = PickAndPlace('left', hover_distance)
left_pnp.gripper_open() left_pnp.gripper_open()
def V_Routine(): def V_Routine():
spawn_v_brick() spawn_v_brick()
left_pnp.send(ta.V_approach) left_pnp.send(ta.V_approach)
x = raw_input('Ready?') x = raw_input('Ready?')
if x == 'n': if x == 'n':
exit(0) exit(0)
left_pnp.send(ta.V_pickup) left_pnp.send(ta.V_pickup)
left_pnp.gripper_close() left_pnp.gripper_close()
left_pnp.send(ta.V_approach) left_pnp.send(ta.V_approach)
def H_Routine(): def H_Routine():
spawn_h_brick() spawn_h_brick()
left_pnp.send(ta.H_approach) left_pnp.send(ta.H_approach)
x = raw_input('Ready?') x = raw_input('Ready?')
if x == 'n': if x == 'n':
exit(0) exit(0)
left_pnp.send(ta.H_pickup) left_pnp.send(ta.H_pickup)
left_pnp.gripper_close() left_pnp.gripper_close()
left_pnp.send(ta.H_approach) left_pnp.send(ta.H_approach)