Add files via upload

src/cobot_control/scripts/arjun2.py

@@ -269,6 +269,8 @@ def fix_error_x(self , result ,  pose):
     return( True , [j1,j2,j3,j4,j5,j6] )
 
   def fix_error_y(self , result ,  pose):
+
+    global BASE_ADJ
     [j1,j2,j3,j4,j5,j6] = pose[0],pose[1],pose[2],pose[3],pose[4],pose[5]
 
 
@@ -278,25 +280,25 @@ def fix_error_y(self , result ,  pose):
     else:
       if result[2] == 1:
         self.dir_y= +1
-        j2 += 0.2
+        j3 += 0.2
         self.count_y += 1
         self.go_to_joint_state(j1,j2,j3,j4,j5,j6)
         time.sleep(0.5)
 
       elif result[2] == -1 :
         self.dir_y = -1
-        j2 -= 0.2
+        j3 -= 0.2
         self.count_y += 1
         self.go_to_joint_state(j1,j2,j3,j4,j5,j6)
         time.sleep(0.5)
 
       elif result[2] == 0 :
 
-        BASE_ADJ =  self.count_y*(0.2)*self.dir_y
-        print(BASE_ADJ)
-        j3 += self.count_y*(0.2)*(-self.dir_y)
+        BASE_ADJ =  2*self.count_y*(0.2)*self.dir_y
+        #print(BASE_ADJ)
+        j2 += self.count_y*(0.2)*(self.dir_y)
         #BASE_ADJ = self.count_y*(0.2)*dir*(-1)
-        #self.go_to_joint_state(j1,j2,j3,j4,j5,j6)
+        self.go_to_joint_state(j1,j2,j3,j4,j5,j6)
         self.pick_down = [j1,j2,j3,j4,j5,j6]
         time.sleep(0.5)
         print("Y error fixed")
@@ -321,7 +323,7 @@ def fix_error_orient(self, result, pose) :
 
     if w > WIDTH_THRESH :
       #print("rotate_iter : ",self.rotate_iters)
-      j6 += 0.7*self.rotate_dir
+      j6 += 1.5*self.rotate_dir
       self.rotate_iters += 1
       self.go_to_joint_state(j1,j2,j3,j4,j5,j6)
 
@@ -350,7 +352,7 @@ def fix_error(self,start_pose) :
 
     flag_x= True  
     flag_orient = True
-    flag_y = False
+    flag_y =  True
     ip = ImageProcessing((20, 100 , 100 ),( 30 , 255, 255))
 
     loop = 0
@@ -388,7 +390,7 @@ def fix_error(self,start_pose) :
 def main():
 
   try:
-    goalList=[[-185 , 36 , 54, 172 , -91 , -181],[-145 , 42 , 54 , 172 , -90 , -181],[-145,48, 54 ,167 ,-90 ,-181], [-144, 49, 53.8, 168.8, -89, 181]]
+    goalList=[[-185 , 36 , 54, 172 , -91 , -181],[-145 , 42 , 54 , 172 , -90 , -181],[-145,48, 54 ,167 ,-90 ,-181], [-145, 49, 54, 168.8, -89, 181]]
     [j01,j02,j03,j04,j05,j06] = goalList[0][0],goalList[0][1],goalList[0][2],goalList[0][3],goalList[0][4],goalList[0][5]
     [j11,j12,j13,j14,j15,j16] = goalList[1][0],goalList[1][1],goalList[1][2],goalList[1][3],goalList[1][4],goalList[1][5]
 
@@ -411,7 +413,7 @@ def main():
 
     time.sleep(5)
 
-    ur5.go_to_joint_state(ur5.pick_down[0] - ur5.rotate_iters*0.04*ur5.rotate_dir , ur5.pick_down[1] + 23 - BASE_ADJ, ur5.pick_down[2] - 23 + BASE_ADJ, ur5.pick_down[3] , ur5.pick_down[4]  ,ur5.pick_down[5])
+    ur5.go_to_joint_state(ur5.pick_down[0] - ur5.rotate_iters*0.04*ur5.rotate_dir , ur5.pick_down[1] + 23 - BASE_ADJ, ur5.pick_down[2] - 23 + BASE_ADJ , ur5.pick_down[3] , ur5.pick_down[4]  ,ur5.pick_down[5])
     #print("Pick Down")
 
     time.sleep(5)

src/cobot_control/scripts/arjun_dl.py

@@ -2,7 +2,7 @@
 
 import sys
 import time
-import roslib
+#import roslib
 from math import pi
 import math
 import copy
@@ -17,7 +17,7 @@
 import numpy as np
 #from mask import ImageProcessing
 import cv2
-import imutils
+#import imutils
 #goalList=[[-185 , 36 , 54, 172 , -91 , -181],[-145 , 42 , 54 , 172 , -90 , -181],[-145,48, 54 ,167 ,-90 ,-181]]
 
 pick_down = [-185 , 36 , 54, 172 , -91 , -181]
@@ -29,18 +29,21 @@
 NMS_THRESHOLD = 0.4
 CONFIDENCE_THRESHOLD = 0.4
 
+CONFIG_FILE='yolov3-custom.cfg'
+WEIGHTS_FILE='yolov3-custom_last.weights'
+
 colors = [(255, 255, 0), (0, 255, 0), (0, 255, 255), (255, 0, 0)]
 
 is_cuda = len(sys.argv) > 1 and sys.argv[1] == "cuda"
 
 #net = build_model(is_cuda)
 #capture = load_capture()
 
-start = time.time_ns()
+'''start = time.time_ns()
 frame_count = 0
 total_frames = 0
 fps = -1
-
+'''
 
 cap = cv2.VideoCapture(-1)
 
@@ -49,11 +52,11 @@ def __init__(self):
         self.frame_count = 0
         self.total_frames = 0
         self.fps = -1
-        self.start = time.time_ns()
+        #self.start = time.time_ns()
         self.class_list = [ "Yellow", "Cube", "Arc" ]
 
     def build_model(self , is_cuda):
-        self.net = cv2.dnn.readNet("best_epoch_50.onnx")
+        self.net = cv2.dnn.readNetFromDarknet(CONFIG_FILE, WEIGHTS_FILE )
         if is_cuda:
             print("Attempty to use CUDA")
             self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_CUDA)
@@ -472,22 +475,22 @@ def fix_error_orient(self, result, pose) :
 
             if self.rotate_dir == 1 and self.rotate_iters > 5 and w > self.init_width :
 
-              self.rotate_dir = -1
-              self.rotate_iters = 0
-              self.go_to_joint_state(-185 , 36 , 54, 172 , -91 , -181)
+                self.rotate_dir = -1
+                self.rotate_iters = 0
+                self.go_to_joint_state(-185 , 36 , 54, 172 , -91 , -181)
 
-              print("DIR CHANGE")
-              time.sleep(0.5)
+                print("DIR CHANGE")
+                time.sleep(0.5)
 
 
             #print("Fixing orientation error")
-            time.sleep(0.5)      
+                time.sleep(0.5)      
 
-            else:
-                #print("Orientation error fixed")
-                self.pick_down = [j1,j2,j3,j4,j5,j6]
-                return( False , [j1,j2,j3,j4,j5,j6] )
-            return ( True , [j1,j2,j3,j4,j5,j6] )
+        else:
+            #print("Orientation error fixed")
+            self.pick_down = [j1,j2,j3,j4,j5,j6]
+            return( False , [j1,j2,j3,j4,j5,j6] )
+        return ( True , [j1,j2,j3,j4,j5,j6] )
 
     def fix_error(self,start_pose) :