From d70c4401f43c3559d436fbb138a148d65c4c208f Mon Sep 17 00:00:00 2001
From: Max Hunt <mh2817@ic.ac.uk>
Date: Sun, 1 Mar 2020 22:06:19 +0000
Subject: [PATCH] Update

---
 brick/ststic-b.sdf |  63 +++++++++++++++
 new_main.py        | 192 +++++++++++++++++++++++++--------------------
 2 files changed, 172 insertions(+), 83 deletions(-)
 create mode 100644 brick/ststic-b.sdf

diff --git a/brick/ststic-b.sdf b/brick/ststic-b.sdf
new file mode 100644
index 0000000..7cf21a3
--- /dev/null
+++ b/brick/ststic-b.sdf
@@ -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>
diff --git a/new_main.py b/new_main.py
index de8bfd6..0cb5845 100644
--- a/new_main.py
+++ b/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)