remove prior calibs code since freedrive is easier

.gitignore

 *.pyc
+*.plan
 catkin_ws/build/
 catkin_ws/devel/
 catkin_ws/.catkin_workspace

src/Calibrator.py

@@ -46,7 +46,7 @@ class Calibrator(object):
 
         # gather configurations
         # [ ( [joint angles] , ROS pose message ) , ... ]
-        configurations = self.controller.prompt_configurations(prompts, prior_calibs, arm=arm)
+        configurations = self.controller.prompt_configurations(prompts, arm=arm)
 
         pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
                 for c in configurations]

src/KeyboardController.py

@@ -549,7 +549,7 @@ class KeyboardController(Controller):
             pass
 
 
-    def prompt_configurations(self, prompt=None, prior_calibs=None, arm=0):
+    def prompt_configurations(self, prompt=None, arm=0):
         """
         Prompts user to control the robot to a series of configurations.
         Relative to robot base, the robot can be translated in the:
@@ -582,53 +582,11 @@ class KeyboardController(Controller):
             'step_deg': 11.25,
             'success': True}
 
-        # 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('Move robot tip to {}'.format(str(p)))
 
-                if priors:
-                    # calculate IK for next point and move there
-                    mat = np.identity(4)
-                    #p[2] += 0.01 # go 1cm above bed for safety
-                    mat[:3,3] = p
-                    mat = world2armT.dot(mat).dot(_vert_or[arm]).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
-
-                    # check angle diff from current pos
-                    cur_pose = self.group.get_current_pose().pose
-                    orientation_diff = sum([
-                        (cur_pose.orientation.x - gpose.orientation.x)**2,
-                        (cur_pose.orientation.y - gpose.orientation.y)**2,
-                        (cur_pose.orientation.z - gpose.orientation.z)**2,
-                        (cur_pose.orientation.w - gpose.orientation.w)**2])**0.5
-
-                    # go home in between if significant angle diff from vertical
-                    if orientation_diff > 0.1:
-                        self.move_home()
-
-                    # compute path to next config
-                    waypoints = [self.group.get_current_pose().pose, gpose]
-                    plan, fraction = self.group.compute_cartesian_path(
-                            waypoints,
-                            0.01, # ee_step
-                            0)
-                    speed_multiplier(plan.joint_trajectory, 4) # 4x slower
-                    self.group.execute(plan, wait=True)
-
                 with keyboard.Listener(
                         on_release = lambda key:self._move_callback(key,configurations,params),
                         suppress=True) as listener:

src/main1.py

@@ -110,16 +110,15 @@ def execute_plan(plan,
     return
 
 
-def gohome():
-    con.group.go(plan.trajs[0][1].positions[0])
+def gohome(arm):
+    con.group.go(plan.trajs[arm][1].positions[0])
 
 
 if __name__ == '__main__':
-    plan = load_plan('knight')
+    plan = load_plan('r1_sq')
 
     # Test calibration
-    con = Controller(robot=1, disable_extruder=True) # r1 when both arms running
-    #con = Controller(robot=2, disable_extruder=True) # r2 when both arms running
+    con = Controller(robot=robot_num+1, disable_extruder=True) # one arm when both arms running
     #con = Controller(disable_extruder=True)         # any when one arm  running
 
-    execute_plan(plan, con, confirm=False, arm=robot_num, extrude=True)
+    execute_plan(plan, con, confirm=False, arm=robot_num, extrude=False)