Update

new_main.py

@@ -22,6 +22,10 @@ import target_angles as ta
 
 brickstuff = tps.brick_directions_notf
 
+global sumulation
+
+simulation = True
+
 class PickAndPlace(object):
     def __init__(self, limb, hover_distance = 0.10, verbose=True, speed=0.2, accuracy=baxter_interface.settings.JOINT_ANGLE_TOLERANCE):
         self._accuracy = accuracy
@@ -58,6 +62,46 @@ class PickAndPlace(object):
     def send(self, angles):
         self._guarded_move_to_joint_position(angles)
 
+    def ik_request(self, pose):
+         hdr = Header(stamp=rospy.Time.now(), frame_id='base')
+         ikreq = SolvePositionIKRequest()
+         ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose))
+         try:
+             resp = self._iksvc(ikreq)
+         except (rospy.ServiceException, rospy.ROSException), e:
+             rospy.logerr("Service call failed: %s" % (e,))
+             return False
+         # Check if result valid, and type of seed ultimately used to get solution
+         # convert rospy's string representation of uint8[]'s to int's
+         resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type)
+         limb_joints = {}
+         if (resp_seeds[0] != resp.RESULT_INVALID):
+             seed_str = {
+                         ikreq.SEED_USER: 'User Provided Seed',
+                         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
+         print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
+         print()
+         print()
+         print('Linb Joints:')
+         print(limb_joints)
+         print()
+         print()
+         print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
+         return limb_joints
+
 
 brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
 
@@ -84,6 +128,8 @@ def etq(roll, pitch, yaw):
         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]
+#LET THE SHITSTORM END
+
 
 def spawn_v_brick():
     brick_pose = Pose()
@@ -134,8 +180,8 @@ def spawn_tables():
 
 rospy.init_node("I_still_have_some_hope")  # Am I wrong??
 
+if simulation:
     cleanup()
-
     spawn_tables()
 
 tuck_arms.init_arms()
@@ -146,7 +192,8 @@ left_pnp = PickAndPlace('left', hover_distance)
 left_pnp.gripper_open()
 
 def V_Routine():
-    # spawn_v_brick()
+    if simulation:
+        spawn_v_brick()
 
     left_pnp.send(ta.V_approach)
     x = raw_input('Ready?')
@@ -160,8 +207,8 @@ def V_Routine():
 
 
 def H_Routine():
-    # spawn_h_brick()
+    if simulation:
-
+        spawn_h_brick()
     left_pnp.send(ta.H_approach)
     x = raw_input('Ready?')
     if x == 'n':exit(0)
@@ -173,6 +220,10 @@ def H_Routine():
 
 
 V_Routine()
+
+sendy_a = left_pnp.ik_request(tps.brick1mid)
+left_pnp.send(sendy_a)
+
 left_pnp.send(ta.B_1_A)
 x = raw_input('Continue?: ')
 left_pnp.send(ta.B_1_P)
@@ -181,6 +232,10 @@ left_pnp.gripper_open()
 left_pnp.send(ta.B_1_A)
 
 V_Routine()
+
+sendy_c = left_pnp.ik_request(tps.brick3mid)
+left_pnp.send(sendy_c)
+
 left_pnp.send(ta.B_2_A)
 x = raw_input('Continue?: ')
 left_pnp.send(ta.B_2_P)
@@ -246,7 +301,10 @@ x = raw_input('Continue?: ')
 left_pnp.send(ta.B_9_P)
 x = raw_input('Continue?: ')
 left_pnp.gripper_open()
-left_pnp.send(ta.B_9_A)
+
+
+sendy_i = left_pnp.ik_request(tps.brick9after)
+left_pnp.send(sendy_i)
 
 
 left_pnp.send(ta.H_pickup)

target_poses.py

@@ -54,8 +54,8 @@ brick1.orientation.w = 4.329780281177467e-17
 
 brick1mid = Pose()
 brick1mid.position.x = 0.615
-brick1mid.position.y = 0.164
+brick1mid.position.y = 0.4
-brick1mid.position.z = -0.131
+brick1mid.position.z = 0.1
 brick1mid.orientation.x = -0.7071067811865476
 brick1mid.orientation.y = -0.7071067811865475
 brick1mid.orientation.z = 4.329780281177466e-17
@@ -81,8 +81,8 @@ brick3.orientation.w = 4.329780281177467e-17
 
 brick3mid = Pose()
 brick3mid.position.x = 0.615
-brick3mid.position.y = 0.164
+brick3mid.position.y = 0.6
-brick3mid.position.z = -0.131
+brick3mid.position.z = 0.2
 brick3mid.orientation.x = -0.7071067811865476
 brick3mid.orientation.y = -0.7071067811865475
 brick3mid.orientation.z = 4.329780281177466e-17
@@ -142,6 +142,15 @@ brick9.orientation.y = -0.7071067811865475
 brick9.orientation.z = 4.329780281177466e-17
 brick9.orientation.w = 4.329780281177467e-17
 
+brick9after = Pose()
+brick9after.position.x = 0.635
+brick9after.position.y = 0.349
+brick9after.position.z = 0.436
+brick9after.orientation.x = -0.7071067811865476
+brick9after.orientation.y = -0.7071067811865475
+brick9after.orientation.z = 4.329780281177466e-17
+brick9after.orientation.w = 4.329780281177467e-17
+
 
 brick_directions_notf = [
 	{