Update

new_main.py

@@ -77,6 +77,14 @@ class PickAndPlace(object):
         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
 
     def _guarded_move_to_joint_position(self, joint_angles):
@@ -99,10 +107,6 @@ class PickAndPlace(object):
         # approach with a pose the hover-distance above the requested pose
         approach.position.z = approach.position.z + self._hover_distance
         joint_angles = self.ik_request(approach)
-        print('---------')
-        print('IK Solution:')
-        print(joint_angles)
-        print('---------')
         self._guarded_move_to_joint_position(joint_angles)
 
     def _retract(self):
@@ -131,6 +135,7 @@ class PickAndPlace(object):
         # servo above pose
         print('Approaching')
         self._approach(pose)
+        return
         # servo to pose
         self._servo_to_pose(pose)
         print('Ready to grip')
@@ -143,6 +148,7 @@ class PickAndPlace(object):
     def place(self, pose):
         # servo above pose
         self._approach(pose)
+        return
         # servo to pose
         self._servo_to_pose(pose)
         # open the gripper
@@ -177,7 +183,6 @@ def cleanup():
     for obj in brick_ids:
         delete_model(obj)
 
-
 # LET THE SHITSTORM BEGIN
 import numpy as np
 import time
@@ -189,7 +194,6 @@ def etq(roll, pitch, yaw):
         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]
 
-
 def spawn_v_brick():
 	brick_pose = Pose()
 	brick_pose.position.x = 0.475

target_angles.py

@@ -0,0 +1,24 @@
+'''
+Pickup Vertical Offset : 
+Pickup Vertical Pick :
+Pickup Horizontal Offset : 
+Pickup Horizontal Pick :
+Place 1 Offset :
+Place 1 Down :
+Place 2 Offset :
+Place 2 Down :
+Place 3 Offset :
+Place 3 Down :
+Place 4 Offset :
+Place 4 Down :
+Place 5 Offset :
+Place 5 Down :
+Place 6 Offset :
+Place 6 Down :
+Place 7 Offset :
+Place 7 Down :
+Place 8 Offset :
+Place 8 Down :
+Place 9 Offset :
+Place 9 Down :
+'''