fast calibration working but unsafe (#18)

src/Calibrator.py

@@ -45,12 +45,11 @@ class Calibrator(object):
                     )
 
         # form prompts to the user to satisfy
-        prompts = ['Move robot tip to {}'.format(np.array2string(pos_W[iPos,]))
-                for iPos in range(pos_W.shape[0])]
+        prompts = [pos_W[iPos,] for iPos in range(pos_W.shape[0])]
 
         # gather configurations
         # [ ( [joint angles] , ROS pose message ) , ... ]
-        configurations = self.controller.prompt_configurations(prompts)
+        configurations = self.controller.prompt_configurations(prompts, prior_calibs)
 
         pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
                 for c in configurations]

src/Controller.py

@@ -54,6 +54,7 @@ class Controller(object):
     - wrist_2_joint
     - wrist_3_joint
     """
+
     def __init__(self,
         close_deg=1.0,
         timeScale=1.0,
@@ -257,7 +258,7 @@ class Controller(object):
         return True
 
 
-    def move_to(self,conf_next,near_deg = 1.0):
+    def move_to(self, conf_next, near_deg = 1.0):
         """
         Moves the arm to the specified configuration by linearly
         interpolating the joint angles.
@@ -278,14 +279,14 @@ class Controller(object):
             dist_rad = np.linalg.norm(dConf)
             dist_deg = np.rad2deg(dist_rad)
             if dist_deg > near_deg:
-                rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dist_conf_deg,jointLim_deg))
+                rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dConf, near_deg))
                 return False
 
         self.group.go(conf_next)
         return True
 
 
-    def is_close(self,trajPoint):
+    def is_close(self, trajPoint):
         """
         Check if the arm is currently close to the configuration specified.
         Ignores timestamp.

src/KeyboardController.py

@@ -17,7 +17,7 @@ import numpy as np
 
 # ROS Libraries
 import rospy
-from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose
 from moveit_msgs.srv import GetPositionFK,GetPositionFKRequest
 from moveit_msgs.srv import GetPositionIK,GetPositionIKRequest
 from moveit_msgs.msg import MoveItErrorCodes
@@ -26,6 +26,20 @@ from moveit_msgs.msg import MoveItErrorCodes
 from Controller import Controller,getR_ET
 
 
+# used for fast-calibration
+def getTransformMat(quat, pos):
+    T = np.identity(4)
+    T[:3,:3] = Rotation.from_quat(quat).as_dcm()
+    T[:3,3] = pos
+    return np.linalg.inv(T)
+
+# also used for fast-calibration-ik
+_vert_or = np.array([[-6.74517010e-04,  8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
+                     [ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
+                     [-9.99999404e-01, -8.34294187e-04,  6.73821778e-04,  2.74792069e-06],
+                     [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,  1.00000000e+00]])
+
+
 class KeyboardController(Controller):
     """
     Controller to control robot arms by keyboard.
@@ -163,9 +177,9 @@ class KeyboardController(Controller):
         Translate the endpoint of the arm from where it currently is,
         relative to the robot base.
 
-        dx_m: 
-        dy_m: 
-        dz_m: 
+        dx_m:
+        dy_m:
+        dz_m:
         resolution_m: Largest end effector distance between two configurations
         """
         ## Get current pose
@@ -200,9 +214,9 @@ class KeyboardController(Controller):
         Translate the endpoint of the arm from where it currently is,
         relative to the end effector.
 
-        dx_m: 
-        dy_m: 
-        dz_m: 
+        dx_m:
+        dy_m:
+        dz_m:
         resolution_m: Largest end effector distance between two configurations
         """
         ## Get current pose
@@ -224,9 +238,9 @@ class KeyboardController(Controller):
         Translate the endpoint of the arm from where it currently is,
         relative to the tool tip.
 
-        dx_m: 
-        dy_m: 
-        dz_m: 
+        dx_m:
+        dy_m:
+        dz_m:
         resolution_m: Largest end effector distance between two configurations
         """
         ## Get orientation of tool tip relative to end effector
@@ -525,7 +539,7 @@ class KeyboardController(Controller):
             pass
 
 
-    def prompt_configurations(self,prompt=None):
+    def prompt_configurations(self, prompt=None, prior_calibs=None):
         """
         Prompts user to control the robot to a series of configurations.
         Relative to robot base, the robot can be translated in the:
@@ -558,18 +572,45 @@ class KeyboardController(Controller):
             'step_deg': 11.25,
             'success': True}
 
-        if isinstance(prompt,list):
+        # if previous calib data, create transformation matrix to calc approx IKs
+        if prior_calibs is not None and len(prior_calibs)>0:
+            world2armT = getTransformMat(prior_calibs['quat_WR'], prior_calibs['pos_WR'])
+            tip2armT = getTransformMat([0,0,0,1], prior_calibs['pos_ET'])
+            priors = True
+        else:
+            priors = False
+
+        if isinstance(prompt, list):
+
             for p in prompt:
                 print(p)
+                print('Move robot tip to {}'.format(str(p)))
+
+                if priors:
+                    # calculate IK for next point and move there
+                    mat = np.identity(4)
+                    mat[:3,3] = p
+                    mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT)
+                    q = Rotation.from_dcm(mat[:3,:3]).as_quat()
+                    p_r = mat[0:3,3]
+                    gpose = Pose()
+                    gpose.position.x, gpose.position.y, gpose.position.z = p_r
+                    gpose.orientation.x, gpose.orientation.y, \
+                            gpose.orientation.z, gpose.orientation.w = q
+                    self.group.set_pose_target(gpose)
+                    self.group.go(wait=True)
+
                 with keyboard.Listener(
                         on_release = lambda key:self._move_callback(key,configurations,params),
                         suppress=True) as listener:
                     listener.join()
                     if DEBUG:
                         print('Keyboard listener thread joined')
+
                 if not params['success']:
                     sys.exit(0)
-        elif isinstance(prompt,int):
+
+        elif isinstance(prompt, int):
             for iConfig in range(prompt):
                 print('Enter configuration {}'.format(iConfig))
                 with keyboard.Listener(
@@ -580,6 +621,7 @@ class KeyboardController(Controller):
                         print('Keyboard listener thread joined')
                 if not params['success']:
                     sys.exit(0)
+
         else:
             while params['success']:
                 params['success'] = False
@@ -594,14 +636,14 @@ class KeyboardController(Controller):
 
 
 if __name__=="__main__":
-    #con = KeyboardController(disableExtruder=True)
+    con = KeyboardController(disableExtruder=True)
     #con = KeyboardController()
     #con1 = KeyboardController(robot=1)
-    con2 = KeyboardController(robot=2, disableExtruder=True)
+    #con2 = KeyboardController(robot=2, disableExtruder=True)
 
     print('Collecting configrations...')
-    #configuations = con.prompt_configurations()
+    configuations = con.prompt_configurations()
     #configuations = con1.prompt_configurations()
-    configuations = con2.prompt_configurations()
+    #configuations = con2.prompt_configurations()
     print('Your configurations:')
     print(configuations)