Update

new_main.py

@@ -1,3 +1,4 @@
+
 import argparse
 import struct
 import sys
@@ -6,6 +7,8 @@ import copy
 import rospy
 import rospkg
 
+import numpy as np
+
 from gazebo_msgs.srv import (SpawnModel, DeleteModel)
 from geometry_msgs.msg import (PoseStamped, Pose, Point, Quaternion)
 from std_msgs.msg import (Header, Empty)
@@ -16,21 +19,18 @@ import time
 
 import baxter_interface
 
-import target_poses as tps
-
 import tuck_arms
 
 import target_angles as ta
 
-brickstuff = tps.brick_directions_notf
+#^Importing essential libraries for ROS and motion planning and running for the main script^
 
-global sumulation
+simulation = True #Flag for spawning objects in the gazebo if the script is running in a simulation
+debug = True #Flag for asking confirmation before robot performs key maneuvers
 
-simulation = True
+class PickAndPlace(object): #class that handles moving the robot, gripper and IK
-debug = True
-
-class PickAndPlace(object):
     def __init__(self, limb, hover_distance = 0.10, verbose=True, speed=0.2, accuracy=baxter_interface.settings.JOINT_ANGLE_TOLERANCE):
+        '''Robot limb control class initializer'''
         self._accuracy = accuracy
         self._limb_name = limb # string
         self._hover_distance = hover_distance # in meters
@@ -48,24 +48,33 @@ class PickAndPlace(object):
         self._rs.enable()
 
     def _guarded_move_to_joint_position(self, joint_angles):
+        '''Private class to move robot limbs'''
         if joint_angles:
-            self._limb.set_joint_position_speed(0.3)
+            self._limb.set_joint_position_speed(0.3) #For safety purposes, the speed was lockes to 0.3
             self._limb.move_to_joint_positions(joint_angles, timeout=20.0, threshold=self._accuracy)
         else:
             rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
 
     def gripper_open(self):
+        '''Opens Gripper'''
         self._gripper.open()
         rospy.sleep(0.2)
 
     def gripper_close(self):
+        '''Closes Gripper'''
         self._gripper.close()
         rospy.sleep(0.2)
 
-    def send(self, angles):
+    def goto(self, angles):
+        '''Function to send limb to predefined joint angles without running IK service'''
         self._guarded_move_to_joint_position(angles)
 
-    def ik_request(self, pose):
+
+
+
+    def ik_request(self, pose): 
+        '''Function to generate joint angles from target pose, does not take into account obstacles that are not the robot'''
+        #This function is only used wile in te simulation environment and only for generating valid joint angles, due to te random nature of te solver, it it not 'trusted' to run in real time
          hdr = Header(stamp=rospy.Time.now(), frame_id='base')
          ikreq = SolvePositionIKRequest()
          ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
@@ -84,14 +93,8 @@ class PickAndPlace(object):
                          ikreq.SEED_CURRENT: 'Current Joint Angles',
                          ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
                         }.get(resp_seeds[0], 'None')
-             if self._verbose:
-                 print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format(
-                          (seed_str)))
              # Format solution into Limb API-compatible dictionary
              limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
-             if self._verbose:
-                 # print("IK Joint Solution:\n{0}".format(limb_joints))
-                 print("------------------")
          else:
              rospy.logerr("INVALID POSE - No Valid Joint Solution Found.")
              return False
@@ -102,42 +105,45 @@ class PickAndPlace(object):
          print(limb_joints)
          print()
          print()
-         print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
+         print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@') #Visual statemet to easily find and copy calculated joint angles
          return limb_joints
 
-    def gripperPosition(self):
-        return self._gripper.position()#class that handles moving the robot, gripper ans IK
 
-if simulation: #deals with gazebo objects, not useful in the real world
+    def gripperPosition(self):
-    brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
+        '''Returns the current gripper state, 0 (Fully closed) -> 100 (Fully open)'''
+        return self._gripper.position() 
+
+if simulation: 
+    brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9'] #Each brick gets known id for easy deletion
 
     with open ("brick/new-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', '')
+    #^Loading sdf models into RAM^
 
     rospy.wait_for_service('/gazebo/spawn_sdf_model')
     spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
     delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
+    #^Initializing spawn and delete model services^
 
-def cleanup(): #deals with gazebo objects, not useful in the real world
+def cleanup():
+    '''Clears all (known) preexisting models in Gazebo'''
     for obj in brick_ids:
         delete_model(obj)
     delete_model('t1')
     delete_model('t2')
 
-# Horrible hack that deals with gazebo objects, not useful in the real world
+def etq(roll, pitch, yaw): #Euler To Quaternian
-import numpy as np
+    '''calculates Quaternian from euler angles'''
-
-def etq(roll, pitch, yaw):
         qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
         qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
         qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
         qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
         return [qx, qy, qz, qw]
-# Horrible hack that deals with gazebo objects, not useful in the real world
 
 
-def spawn_v_brick():#deals with gazebo objects(spawns a vertical brick), not useful in the real world
+def spawn_v_brick():
+    '''Spawns Vertical brick in Gazebo simulation'''
     brick_pose = Pose()
     brick_pose.position.x = 0.475
     brick_pose.position.y = 0.739
@@ -151,7 +157,8 @@ def spawn_v_brick():#deals with gazebo objects(spawns a vertical brick), not use
     brick_id = brick_ids.pop()
     spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
 
-def spawn_h_brick():#deals with gazebo objects(spawns a horizontal brick), not useful in the real world
+def spawn_h_brick():
+    '''Spawns Horizontal brick in Gazebo simulation'''
     brick_pose = Pose()
     brick_pose.position.x = 0.4664
     brick_pose.position.y = 0.8069
@@ -164,7 +171,8 @@ def spawn_h_brick():#deals with gazebo objects(spawns a horizontal brick), not u
     brick_id = brick_ids.pop()
     spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
 
-def spawn_tables():#deals with gazebo objects(spawns tables), not useful in the real world
+def spawn_tables():
+    '''Spawns Tables in Gazebo simulation'''
     table1 = Pose()
     table1.position.x = 1.160
     table1.position.y = 0.365
@@ -184,188 +192,171 @@ def spawn_tables():#deals with gazebo objects(spawns tables), not useful in the
     spawn_sdf(table2_id, table_sdf, "/", table2, table_reference_frame)
 
 
-rospy.init_node("I_still_have_some_hope")  # Am I wrong??
+rospy.init_node("MOTHER_CLUCKER")  #Initializes rospy node
 
 
 if simulation:
+    #Clean up any (known) preexisting objects in Gazebo and spawns the tables
     cleanup()
     spawn_tables()
 
-tuck_arms.init_arms() #Make sure the arms are where they meant to be
+tuck_arms.init_arms() #Makes sure the arms are in known starting place
 
 hover_distance = 0.2 #Used for IK, not useful to us anymore
 left_pnp = PickAndPlace('left', hover_distance) #Initializer
 
-
+def open_and_wait():
-def V_Routine(): #Spawns brick (if sim), sends gripper to brick position, asks if ready (if in debug), 
+    '''Opens gripper and waits for uer input to close'''
-#                 picks it up and checks that the brick is acc in the grippers
+    left_pnp.gripper_open()
-    if simulation:
+    x = raw_input('Close?')
-        spawn_v_brick()
-
-    left_pnp.send(ta.V_approach)
-    if debug:
-        x = raw_input('Ready?')
-    left_pnp.send(ta.V_pickup)
-
-
     left_pnp.gripper_close()
-    gripper_state = left_pnp.gripperPosition()
+    time.sleep(0.5)
 
+def gripperBrickChecker():
+    '''Checks if brick detected in gripper, asks what to do it not'''
     if gripper_state < 5:
         command = raw_input('\n \nPROBLEM DETECTED!!!\n(C)ontinue, (A)bort, (O)pen gripper\n>_ ')
         if command in {'C', 'c', 'continue'}:
-            pass
+            return
         elif command in {'O', 'o', 'open'}:
             open_and_wait()
         else:
             print('Exiting')
             exit(0)
 
-    time.sleep(0.5)
-    left_pnp.send(ta.V_approach)
 
+def V_Routine(): 
+    '''Spawns vertical brick (if sim), sends gripper to brick position, asks if ready (if in debug), picks it up and checks that the brick is in the gripper'''
+    if simulation: spawn_v_brick()
+    left_pnp.goto(ta.V_approach)
+
+    left_pnp.goto(ta.V_pickup)
+    if debug: x = raw_input('Ready?')
 
-def open_and_wait(): #Self explanatory
-    left_pnp.gripper_open()
-    if debug:
-        x = raw_input('Close?')
     left_pnp.gripper_close()
-    time.sleep(0.5)
+    gripperBrickChecker()
-
-
-def H_Routine():#Spawns brick (if sim), sends gripper to brick position, asks if ready (if in debug), 
-#                 picks it up and checks that the brick is acc in the grippers
-    if simulation:
-        spawn_h_brick()
-    left_pnp.send(ta.H_approach)
-    if debug:
-        x = raw_input('Ready?')
-    left_pnp.send(ta.H_pickup)
-    left_pnp.gripper_close()
-
     gripper_state = left_pnp.gripperPosition()
-
-    if gripper_state < 5:
-        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':
-            pass
-        elif command == 'O':
-            open_and_wait()
-        elif command == 'o':
-            open_and_wait()
-        else:
-            exit(0)
-
     time.sleep(0.5)
-    left_pnp.send(ta.H_approach)
+    left_pnp.goto(ta.V_approach)
+
+def H_Routine():
+    '''Spawns horizontal brick (if sim), sends gripper to brick position, asks if ready (if in debug), picks it up and checks that the brick is in the gripper'''
+    if simulation: spawn_h_brick()
+    left_pnp.goto(ta.H_approach)
+
+    if debug: x = raw_input('Ready?')
+    left_pnp.goto(ta.H_pickup)
+
+    left_pnp.gripper_close()
+    gripperBrickChecker()
+    time.sleep(0.5)
+    left_pnp.goto(ta.H_approach)
 
 
-left_pnp.gripper_open() #Duh
+left_pnp.gripper_open() #Ensures gripper is open before grabbing brick
-left_pnp.gripper_open()
-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)
 
 V_Routine()
 
-left_pnp.send(ta.B_2_A)
+left_pnp.goto(ta.B_1_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_2_P)
+left_pnp.goto(ta.B_1_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_2_A)
+
+left_pnp.goto(ta.B_1_A)
 
 V_Routine()
 
-left_pnp.send(ta.B_3_A)
+left_pnp.goto(ta.B_2_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_3_P)
+left_pnp.goto(ta.B_2_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_3_A)
+left_pnp.goto(ta.B_2_A)
+
+V_Routine()
+
+left_pnp.goto(ta.B_3_A)
+if debug:
+    x = raw_input('Continue?: ')
+left_pnp.goto(ta.B_3_P)
+if debug:
+    x = raw_input('Continue?: ')
+left_pnp.gripper_open()
+left_pnp.goto(ta.B_3_A)
 
 
 H_Routine()
-left_pnp.send(ta.B_4_A)
+left_pnp.goto(ta.B_4_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_4_P)
+left_pnp.goto(ta.B_4_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_4_A)
+left_pnp.goto(ta.B_4_A)
 
 H_Routine()
-left_pnp.send(ta.B_5_A)
+left_pnp.goto(ta.B_5_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_5_P)
+left_pnp.goto(ta.B_5_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_5_A)
+left_pnp.goto(ta.B_5_A)
 
 
 V_Routine()
-left_pnp.send(ta.B_6_A)
+left_pnp.goto(ta.B_6_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_6_P)
+left_pnp.goto(ta.B_6_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_6_A)
+left_pnp.goto(ta.B_6_A)
 
 V_Routine()
-left_pnp.send(ta.B_7_A)
+left_pnp.goto(ta.B_7_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_7_P)
+left_pnp.goto(ta.B_7_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_7_A)
+left_pnp.goto(ta.B_7_A)
 
 
 H_Routine()
-left_pnp.send(ta.B_8_A)
+left_pnp.goto(ta.B_8_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_8_P)
+left_pnp.goto(ta.B_8_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_8_A)
+left_pnp.goto(ta.B_8_A)
 
 
 V_Routine()
-left_pnp.send(ta.B_9_A)
+left_pnp.goto(ta.B_9_A)
 if debug:
     x = raw_input('Continue?: ')
-left_pnp.send(ta.B_9_P)
+left_pnp.goto(ta.B_9_P)
 if debug:
     x = raw_input('Continue?: ')
 left_pnp.gripper_open()
 
 
-left_pnp.send(ta.B_9_za)
+left_pnp.goto(ta.B_9_za)
-left_pnp.send(ta.B_9_zb)
+left_pnp.goto(ta.B_9_zb)
 
-left_pnp.send(ta.H_pickup)
+left_pnp.goto(ta.H_pickup)
 
 time.sleep(1)