This commit is contained in:
Max Hunt 2020-02-19 11:16:27 +00:00
parent 31cce6dcf5
commit 4e434b5845
2 changed files with 19 additions and 1 deletions

View File

@ -5,7 +5,7 @@
<pose frame=''>0 0 0 0 -0 0</pose> <pose frame=''>0 0 0 0 -0 0</pose>
<inertial> <inertial>
<pose frame=''> 0 0 0.0 -0 0</pose> <pose frame=''> 0 0 0.0 -0 0</pose>
<mass>0.1</mass> <mass>0.001</mass>
<inertia> <inertia>
<ixx>0.000002</ixx> <ixx>0.000002</ixx>
<ixy>0</ixy> <ixy>0</ixy>

View File

@ -171,59 +171,77 @@ def H_Routine():
V_Routine() V_Routine()
left_pnp.send(ta.B_1_A) left_pnp.send(ta.B_1_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_1_P) left_pnp.send(ta.B_1_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_1_A) left_pnp.send(ta.B_1_A)
V_Routine() V_Routine()
left_pnp.send(ta.B_2_A) left_pnp.send(ta.B_2_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_2_P) left_pnp.send(ta.B_2_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_2_A) left_pnp.send(ta.B_2_A)
V_Routine() V_Routine()
left_pnp.send(ta.B_3_A) left_pnp.send(ta.B_3_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_3_P) left_pnp.send(ta.B_3_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_3_A) left_pnp.send(ta.B_3_A)
H_Routine() H_Routine()
left_pnp.send(ta.B_4_A) left_pnp.send(ta.B_4_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_4_P) left_pnp.send(ta.B_4_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_4_A) left_pnp.send(ta.B_4_A)
H_Routine() H_Routine()
left_pnp.send(ta.B_5_A) left_pnp.send(ta.B_5_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_5_P) left_pnp.send(ta.B_5_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_5_A) left_pnp.send(ta.B_5_A)
V_Routine() V_Routine()
left_pnp.send(ta.B_6_A) left_pnp.send(ta.B_6_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_6_P) left_pnp.send(ta.B_6_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_6_A) left_pnp.send(ta.B_6_A)
V_Routine() V_Routine()
left_pnp.send(ta.B_7_A) left_pnp.send(ta.B_7_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_7_P) left_pnp.send(ta.B_7_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_7_A) left_pnp.send(ta.B_7_A)
H_Routine() H_Routine()
left_pnp.send(ta.B_8_A) left_pnp.send(ta.B_8_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_8_P) left_pnp.send(ta.B_8_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_8_A) left_pnp.send(ta.B_8_A)
V_Routine() V_Routine()
left_pnp.send(ta.B_9_A) left_pnp.send(ta.B_9_A)
x = raw_input('Continue?: ')
left_pnp.send(ta.B_9_P) left_pnp.send(ta.B_9_P)
x = raw_input('Continue?: ')
left_pnp.gripper_open() left_pnp.gripper_open()
left_pnp.send(ta.B_9_A) left_pnp.send(ta.B_9_A)