bugfixes and cleanup

controller.py

 from copy import copy, deepcopy
 from pynput import keyboard
 from enum import Enum
-from utils import ur_kinematics
 from math import pi
 import time
 import warnings
-from utils import *
 import sys
 
 
@@ -76,7 +74,6 @@ class controller:
         self.joint_state = None
         self.calibration = None
         self._temp_calib = None
-        self.kin = ur_kinematics("UR5e")
         time.sleep(1)
 
         # initialise MoveIt commander
@@ -101,7 +98,8 @@ class controller:
     def fk(self, joints):
         """
         compute forward kinematics
-        This uses the /compute_fk rosservice - which doesnt work
+        Using /compute_fk rosservice 
+            - which doesnt work
         """
 
         rospy.wait_for_service('compute_fk')
@@ -111,7 +109,7 @@ class controller:
             rospy.logerr("Service call failed: %s"%e)
 
         req = GetPositionFKRequest()
-        req.header.frame_id = 'base'
+        req.header.frame_id = 'world'
         req.fk_link_names = ['ee_link']
         req.robot_state.joint_state.name = self.group.get_joints()
         req.robot_state.joint_state.position = joints
@@ -126,7 +124,7 @@ class controller:
             - this one actually works
         """
 
-        if start_angle = None:
+        if start_angle == None:
             start_angle = con.group.get_current_joint_values()
 
         rospy.wait_for_service('compute_ik')
@@ -165,37 +163,10 @@ class controller:
         """
 
         self.group.go(joint_angles)
-#        now = self.robot.get_current_state().joint_state.position
-#        max_dTheta = max([abs(now[a]-joint_angles[a]) for a in range(len(now))])
-#        total_time = max_dTheta/max_speed
-#
-#        t = self.empty_traj()
-#        p1 = JointTrajectoryPoint()
-#        p1.positions = copy(now)
-#        p1.time_from_start = rospy.Time(0)
-#        p2 = JointTrajectoryPoint()
-#        p2.positions = copy(tuple(joint_angles))
-#        p2.time_from_start = rospy.Time(total_time)
-#        t.points.append(p1)
-#        t.points.append(p2)
-#
-#        print t
-#        self.exec_traj(t)
-
 
-#    def get_joints(self):
-#        """
-#        Get current state of UR5
-#        """
-#        def joint_state_setter(data):
-#                self.joint_state = data
-#                self.sb.unregister()
-#
-#        self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
-#
-#        time.sleep(0.1)
-#        return self.joint_state.position
 
+    def get_joints(self):
+        return self.group.get_current_joint_values() 
 
     def empty_traj(self):
         """
@@ -246,7 +217,7 @@ class controller:
             self._internal_move_endpoint(0, 0, -0.01)
         elif key == keyboard.Key.enter:
             # save current pose into calibaration
-            return self._temp_calib.push_parameter(self.get_joints())
+            return self._temp_calib.push_parameter(self.group.get_current_joint_values())
 
 
     def _internal_on_release(self, key):