Update
This commit is contained in:
Max Hunt
parent
1744c6f355
commit
d70c4401f4
63
brick/ststic-b.sdf
Normal file
63
brick/ststic-b.sdf
Normal file
@ -0,0 +1,63 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version='1.6'>
|
||||
<model name='Brick'>
|
||||
<static>true</static>
|
||||
<link name='Brick'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''> 0 0 0.0 -0 0</pose>
|
||||
<mass>0.001</mass>
|
||||
<inertia>
|
||||
<ixx>0.000002</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.000002</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='Brick_collision'>
|
||||
<pose frame=''>0 0 0 1.5708 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.192 0.062 0.086</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>10000000</mu>
|
||||
<mu2>10000000</mu2>
|
||||
<fdir1>0 0 1</fdir1>
|
||||
<slip1>1000</slip1>
|
||||
<slip2>1000</slip2>
|
||||
</ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>1000000000.000000</kp>
|
||||
<kd>100000000.00000</kd>
|
||||
<max_vel>20.000000</max_vel>
|
||||
<min_depth>0.0001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='Brick_visual'>
|
||||
<pose frame=''>0 0 0 1.5708 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://Brick/meshes/Brick_1.STL</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.698 0.25 0.25 1</ambient>
|
||||
<diffuse>0.698 0.25 0.25 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</sdf>
|
||||
192
new_main.py
192
new_main.py
@ -24,7 +24,7 @@ brickstuff = tps.brick_directions_notf
|
||||
|
||||
global sumulation
|
||||
|
||||
simulation = True
|
||||
simulation = False
|
||||
debug = True
|
||||
|
||||
class PickAndPlace(object):
|
||||
@ -111,6 +111,7 @@ if simulation:
|
||||
|
||||
with open ("brick/model.sdf", "r") as brick_file:brick_sdf=brick_file.read().replace('\n', '')
|
||||
with open ("L3-table/model.sdf", "r") as table_file:table_sdf=table_file.read().replace('\n', '')
|
||||
with open ("brick/static-b.sdf", "r") as table_file:static_brick=table_file.read().replace('\n', '')
|
||||
|
||||
rospy.wait_for_service('/gazebo/spawn_sdf_model')
|
||||
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
|
||||
@ -184,6 +185,21 @@ def spawn_tables():
|
||||
|
||||
rospy.init_node("I_still_have_some_hope") # Am I wrong??
|
||||
|
||||
|
||||
#####
|
||||
brick9 = Pose()
|
||||
brick9.position.x = 0.635
|
||||
brick9.position.y = 0.349
|
||||
brick9.position.z = 0.406
|
||||
brick9.orientation.x = -0.7071067811865476
|
||||
brick9.orientation.y = -0.7071067811865475
|
||||
brick9.orientation.z = 4.329780281177466e-17
|
||||
brick9.orientation.w = 4.329780281177467e-17
|
||||
brick_reference_frame = 'world'
|
||||
brick_id = 'static-9'
|
||||
spawn_sdf(brick_id, static_brick, "/", brick9, brick_reference_frame)
|
||||
#####
|
||||
|
||||
if simulation:
|
||||
cleanup()
|
||||
spawn_tables()
|
||||
@ -204,21 +220,19 @@ def V_Routine():
|
||||
x = raw_input('Ready?')
|
||||
if x == 'n':exit(0)
|
||||
left_pnp.send(ta.V_pickup)
|
||||
left_pnp.gripper_close()
|
||||
|
||||
|
||||
left_pnp.gripper_close()
|
||||
gripper_state = left_pnp.gripperPosition()
|
||||
|
||||
if gripper_state < 10:
|
||||
command = raw_input('\n \n PROBLEM DETECTED!!!\nGripper has nothing in it...\n (C)ontinue, (A)bort, (O)pen gripper\n >_ ')
|
||||
if command == 'C':
|
||||
command = raw_input('\n \nPROBLEM DETECTED!!!\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
|
||||
if command in {'C', 'c', 'continue'}:
|
||||
pass
|
||||
elif command == 'c':
|
||||
pass
|
||||
elif command == 'O':
|
||||
open_and_wait()
|
||||
elif command == 'o':
|
||||
elif command in {'O', 'o', 'open'}:
|
||||
open_and_wait()
|
||||
else:
|
||||
print('Exiting')
|
||||
exit(0)
|
||||
|
||||
time.sleep(0.5)
|
||||
@ -244,7 +258,7 @@ def H_Routine():
|
||||
gripper_state = left_pnp.gripperPosition()
|
||||
|
||||
if gripper_state < 10:
|
||||
command = raw_input('\n \n PROBLEM DETECTED!!!\nGripper has nothing in it...\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
|
||||
command = raw_input('\n \nPROBLEM DETECTED!!!\nGripper has nothing in it...\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
|
||||
if command == 'C':
|
||||
pass
|
||||
elif command == 'c':
|
||||
@ -262,92 +276,92 @@ def H_Routine():
|
||||
|
||||
left_pnp.gripper_open()
|
||||
|
||||
V_Routine()
|
||||
# V_Routine()
|
||||
|
||||
left_pnp.send(ta.B_1_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_1_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_1_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_1_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# 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)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_2_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_2_A)
|
||||
# left_pnp.send(ta.B_2_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_2_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_2_A)
|
||||
|
||||
V_Routine()
|
||||
# V_Routine()
|
||||
|
||||
left_pnp.send(ta.B_3_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_3_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_3_A)
|
||||
# left_pnp.send(ta.B_3_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_3_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_3_A)
|
||||
|
||||
|
||||
H_Routine()
|
||||
left_pnp.send(ta.B_4_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_4_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_4_A)
|
||||
# H_Routine()
|
||||
# left_pnp.send(ta.B_4_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_4_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_4_A)
|
||||
|
||||
H_Routine()
|
||||
left_pnp.send(ta.B_5_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_5_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_5_A)
|
||||
# H_Routine()
|
||||
# left_pnp.send(ta.B_5_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_5_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_5_A)
|
||||
|
||||
|
||||
V_Routine()
|
||||
left_pnp.send(ta.B_6_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_6_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_6_A)
|
||||
# V_Routine()
|
||||
# left_pnp.send(ta.B_6_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_6_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_6_A)
|
||||
|
||||
V_Routine()
|
||||
left_pnp.send(ta.B_7_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_7_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_7_A)
|
||||
# V_Routine()
|
||||
# left_pnp.send(ta.B_7_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_7_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_7_A)
|
||||
|
||||
|
||||
H_Routine()
|
||||
left_pnp.send(ta.B_8_A)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.send(ta.B_8_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_8_A)
|
||||
# H_Routine()
|
||||
# left_pnp.send(ta.B_8_A)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.send(ta.B_8_P)
|
||||
# if debug:
|
||||
# x = raw_input('Continue?: ')
|
||||
# left_pnp.gripper_open()
|
||||
# left_pnp.send(ta.B_8_A)
|
||||
|
||||
|
||||
V_Routine()
|
||||
@ -358,7 +372,19 @@ left_pnp.send(ta.B_9_P)
|
||||
if debug:
|
||||
x = raw_input('Continue?: ')
|
||||
left_pnp.gripper_open()
|
||||
left_pnp.send(ta.B_9_Z)
|
||||
|
||||
brick9after = Pose()
|
||||
brick9after.position.x = 0.635
|
||||
brick9after.position.y = 0.349
|
||||
brick9after.position.z = 0.506
|
||||
brick9after.orientation.x = -0.7071067811865476
|
||||
brick9after.orientation.y = -0.7071067811865475
|
||||
brick9after.orientation.z = 4.329780281177466e-17
|
||||
brick9after.orientation.w = 4.329780281177467e-17
|
||||
|
||||
new_x = left_pnp.ik_request(brick9after)
|
||||
|
||||
left_pnp.send(new_x)
|
||||
|
||||
|
||||
left_pnp.send(ta.H_pickup)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user