switch to moveit interface, fk implemented but not working in simulation

controller.py

@@ -20,6 +20,9 @@ from trajectory_msgs.msg import (
 from sensor_msgs.msg import JointState
 from std_msgs.msg import String
 from moveit_commander.conversions import pose_to_list
+from moveit_msgs.srv import GetPositionFK, GetPositionFKRequest, GetPositionFKResponse
+from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
+from moveit_msgs.msg import RobotState
 
 
 class CalibrationType(Enum):
@@ -88,13 +91,47 @@ class controller:
             queue_size=20)
 
         # Potentially useful but not used right now
-        # self.planning_frame = group.get_planning_frame()
-        # self.eef_link = group.get_end_effector_link()
-        # self.group_names = robot.get_group_names()
+        # self.planning_frame = self.group.get_planning_frame()
+        self.ee_link = self.group.get_end_effector_link()
+        # self.group_names = self.robot.get_group_names()
 
         print self.robot.get_current_state()
 
 
+    def fk(self, joints):
+        """
+        compute forward kinematics
+        """
+
+        rospy.wait_for_service('compute_fk')
+        try:
+            moveitFK = rospy.ServiceProxy('compute_fk', GetPositionFK)
+        except rospy.ServiceException as e:
+            rospy.logerr("Service call failed: %s"%e)
+
+        req = GetPositionFKRequest()
+        req.header.frame_id = 'base_link'
+        req.fk_link_names = ['ee_link']
+        req.robot_state.joint_state.name = self.group.get_joints()
+        req.robot_state.joint_state.position = joints
+
+        return moveitFK.call(req)
+
+
+    def ik(self, pose):
+        """
+        Inverse Kinematics
+        """
+
+        rospy.wait_for_service('compute_ik')
+        try:
+            moveitIK = rospy.ServiceProxy('compute_ik', GetPositionFK)
+        except rospy.ServiceException as e:
+            rospy.logerr("Service call failed: %s"%e)
+
+
+        pass
+
     def exec_traj(self, traj):
         """
         execute a JointTrajectory object
@@ -114,36 +151,37 @@ class controller:
         max speed is in rad/s, deafault 10s to do a 180
         """
 
-        now = self.get_joints()
-        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
+        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 empty_traj(self):
@@ -232,27 +270,27 @@ if __name__=="__main__":
 
     con = controller()
 
-    print con.get_joints()
+#    print con.get_joints()
 
     ########################################
-    print "Testing Joint Trajectory controller"
-
-    t = con.empty_traj()
-    js1 = (-1.3825197219848633,-1.7300764522948207,1.800080776214599,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
-    js2 = (-1.3825197219848633,-1.7300764522948207,2.323414109547933,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
-
-    p1 = JointTrajectoryPoint()
-    p1.positions = copy(js1)
-    p1.time_from_start = rospy.Time(0)
-    p2 = JointTrajectoryPoint()
-    p2.positions = copy(js2)
-    p2.time_from_start = rospy.Time(3)
-    p3 = JointTrajectoryPoint()
-    p3.positions = copy(js1)
-    p3.time_from_start = rospy.Time(6)
-    t.points.append(p1)
-    t.points.append(p2)
-    t.points.append(p3)
-
-    print t
-    con.exec_traj(t)
+#    print "Testing Joint Trajectory controller"
+#
+#    t = con.empty_traj()
+#    js1 = (-1.3825197219848633,-1.7300764522948207,1.800080776214599,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
+#    js2 = (-1.3825197219848633,-1.7300764522948207,2.323414109547933,-1.4899586004069825,1.5598526000976562,0.7699174880981445)
+#
+#    p1 = JointTrajectoryPoint()
+#    p1.positions = copy(js1)
+#    p1.time_from_start = rospy.Time(0)
+#    p2 = JointTrajectoryPoint()
+#    p2.positions = copy(js2)
+#    p2.time_from_start = rospy.Time(3)
+#    p3 = JointTrajectoryPoint()
+#    p3.positions = copy(js1)
+#    p3.time_from_start = rospy.Time(6)
+#    t.points.append(p1)
+#    t.points.append(p2)
+#    t.points.append(p3)
+#
+#    print t
+#    con.exec_traj(t)