reading joint states

.gitignore

+*.pyc

calibration.yaml

+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.1624898541114357
+    roll: -0
+    pitch: 0
+    yaw: -2.993110372051522e-08
+  upper_arm:
+    x: 0.000199748901491388
+    y: 0
+    z: 0
+    roll: 1.570191651791959
+    pitch: 0
+    yaw: 3.467196906109897e-06
+  forearm:
+    x: -0.4253389613409334
+    y: 0
+    z: 0
+    roll: 0.0005830482518067511
+    pitch: 0.0001777360512510511
+    yaw: 3.757108206717486e-07
+  wrist_1:
+    x: -0.3923171577167464
+    y: 7.90305563733867e-05
+    z: 0.1334493080654863
+    roll: 3.141000439600227
+    pitch: 3.140844198415325
+    yaw: 3.141590492022485
+  wrist_2:
+    x: 8.747259714909744e-05
+    y: -0.0998032513631754
+    z: -3.408488090727067e-05
+    roll: 1.571137847528105
+    pitch: 0
+    yaw: -1.455051852330621e-07
+  wrist_3:
+    x: 5.251979966562787e-06
+    y: 0.09980276859784269
+    z: -2.957416730143468e-06
+    roll: 1.570766694182787
+    pitch: 3.141592653589793
+    yaw: 3.141592441374947
+  hash: calib_17767642431275351403
\ No newline at end of file

controller.py

+import rospy
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+    JointTrajectory
+)
+from sensor_msgs.msg import JointState
+import time
+
+class controller:
+
+    def __init__(self):
+        """
+        Initiate connection to UR5
+        """
+        self.node = rospy.init_node("py_controller", anonymous = False)
+        self.traj = rospy.Publisher('scaled_pos_traj_controller/command', JointTrajectory, queue_size=10)
+        self.joint_state = None
+
+
+    def exec_traj(self, traj):
+        """
+        execute a JointTrajectory object
+
+        """
+        rospy.loginfo("sending trajectroy to UR5")
+        pub.publish(traj)
+
+
+    def get_joints(self):
+        """
+        Get current state of UR5
+        """
+        def joint_state_setter(data):
+                self.joint_state = data.position
+                self.sb.unregister()
+
+        self.sb = rospy.Subscriber("joint_states", JointState, joint_state_setter)
+
+        time.sleep(0.1)
+        return self.joint_state
+
+
+if __name__=="__main__":
+    
+    print "Testing joint angle"
+
+    con = controller()
+
+    print con.get_joints()
+

main.py

 import time
 import numpy as np
 import gcode2contour as gc
+
 import rospy
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+    JointTrajectory
+)
+
+
+def contour2traj(c):
+    """
+    Converts contours to JointTrajectory objects
+
+    Assumes that the contour XYZ locations have already been offset
+    """
+
+    # IK for each point in the trajectory
+        # choose valid starting IK -> closest to current position
+
+    # create Trajectory using IK.
+    traj = JointTrajectory()
+    for name in planner.joint_names[robot]:
+        traj_msg.joint_names.append(name)
+    for x in c.pos:
+        point = JointTrajectoryPoint()
+        point.positions = copy([0,0,0,0,0,0])# TODO change to get the relevant IK
+        point.time_from_start = rospy.Duration(0) # TODO get time out of contour
+    pass
 
 
 def print_contours(contours):
     """
     takes a list contours and prints them
     """
+
+    #TODO current pose - get from 
+    for c in contours:
+        t = contour2traj(c)#, current_pose)
+
+        # Execute Trajectory
     pass
 
 
@@ -17,4 +49,9 @@ if __name__ == "__main__":
     fname = "benchy.gcode"
     contours = gc.decode_gcode(fname)
 
-    print_contours(contours)
+    # Calibrate system
+    # TODO
+    # Apply calibration offset to contours
+    # TODO
+
+    #print_contours(contours)