From 92c656665f527127c2cf52fbe0e1e4cefd1bef2f Mon Sep 17 00:00:00 2001
From: Max Hunt <mh2817@ic.ac.uk>
Date: Wed, 19 Feb 2020 00:29:07 +0000
Subject: [PATCH] Update

---
 new_main.py      | 16 ++++++++++------
 target_angles.py | 24 ++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 6 deletions(-)
 create mode 100644 target_angles.py

diff --git a/new_main.py b/new_main.py
index 3fe0de2..df0a1cf 100644
--- a/new_main.py
+++ b/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
diff --git a/target_angles.py b/target_angles.py
new file mode 100644
index 0000000..893f934
--- /dev/null
+++ b/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 :
+'''
\ No newline at end of file