forward kinematics working, executing contour trajectory

controller.py

@@ -6,6 +6,7 @@ import time
 import warnings
 import sys
 import gcode2contour as gc
+from utils import clean_contour
 
 
 import rospy
@@ -22,6 +23,7 @@ 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
+from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
 
 
 class CalibrationType(Enum):
@@ -103,7 +105,6 @@ class controller:
         """
         compute forward kinematics
         Using /compute_fk rosservice 
-            - which doesnt work
         """
 
         rospy.wait_for_service('compute_fk')
@@ -115,7 +116,7 @@ class controller:
         req = GetPositionFKRequest()
         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.name = self.group.get_active_joints()
         req.robot_state.joint_state.position = joints
 
         return moveitFK.call(req)
@@ -150,9 +151,15 @@ class controller:
     def exec_traj(self, traj):
         """
         execute a JointTrajectory object
+
+        NOTE: If not currently at starting point, this will fail
         """
         rospy.loginfo("sending trajectroy to UR5")
-        self.traj.publish(traj)
+
+        #self.traj.publish(traj)
+        plan = RobotTrajectory()
+        plan.joint_trajectory = traj
+        self.group.execute(plan)
 
 
     def move_to(self, joint_angles, max_speed = pi/100):
@@ -172,12 +179,13 @@ class controller:
     def get_joints(self):
         return self.group.get_current_joint_values()
 
+
     def empty_traj(self):
         """
         Empty trajectory with Joint names filled in
         """
         t = JointTrajectory()
-        t.joint_names = self.group.get_joints()
+        t.joint_names = self.group.get_active_joints()
         return t
 
 
@@ -187,14 +195,15 @@ class controller:
         """
         # step 1 get current pose
         now = self.group.get_current_pose().pose
+        new = deepcopy(now)
 
         # step 2 shift pose
-        now.position.x += dx
-        now.position.y += dy
-        now.position.z += dz
+        new.position.x += dx
+        new.position.y += dy
+        new.position.z += dz
 
         # step 3 compute cartesian plan
-        (plan, frac) = self.group.compute_cartesian_path([now], 0.01, 0.0)
+        (plan, frac) = self.group.compute_cartesian_path([now, new], 0.01, 0.0)
 
         # step 4 execute plan
         self.group.execute(plan)
@@ -283,6 +292,7 @@ class controller:
             4. save points and timestamp in trajectory
         """
 
+        clean_contour(contour)
         # Get calibration pose:
         p0 = self.calibration.cb_pose[0]
         p_last = deepcopy(p0)
@@ -309,31 +319,4 @@ class controller:
 
 if __name__=="__main__":
 
-    ########################################
-    print "Testing joint angle subscriber"
-
     con = controller()
-
-#    print con.get_joints()
-
-    ########################################
-#    print "Testing Joint Trajectory controller"
-#
-#    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)

main.py

@@ -26,4 +26,5 @@ if __name__ == "__main__":
     traj = con.contour2traj(contours[0])
 
     # Execute trajectory
+    con.move_to(traj.points[0].positions)
     con.exec_traj(traj)