Added arm motion control, fixed simpletest.py

common/Testing/arm_motion_playback.py

@@ -0,0 +1,56 @@
+import sys; sys.path.append('.')
+import time
+from Motion import motion
+from Motion import config
+from klampt.trajectory import Trajectory
+
+fn = "arm_motion.traj"
+speed = 1
+
+print "Plays back an arm motion"
+print "Usage: arm_motion_playback.py FILE [SPEED]"
+if len(sys.argv) <= 1:
+    print "Please specify a motion file"
+    exit(0)
+if len(sys.argv) > 2:
+    speed = float(sys.argv[2])
+
+fn = sys.argv[1]
+print "Loading motion from",fn
+traj = Trajectory()
+traj.load(fn)
+
+if len(traj.milestones[0]) != 14:
+    print "Error loading arms trajectory, size is not 14"
+
+#simple motion setup
+config.setup(parse_sys=False)
+
+motion.robot.startup()
+try:
+    print "Moving to start, 20% speed..."
+    motion.robot.arms_mq.setRamp(traj.milestones[0],speed=0.2)
+    while motion.robot.arms_mq.moving():
+        time.sleep(0.1)
+    print "Starting motion..."
+    """
+    #this is direct interpolation in Python at 50 Hz
+    t0 = time.time()
+    while time.time()-t0 < traj.times[-1]/speed:
+        q = traj.eval((time.time()-t0)/speed)
+        motion.robot.left_arm.setPosition(q[0:7])
+        motion.robot.right_arm.setPosition(q[7:14])
+        time.sleep(0.02)
+    """
+    #this uses the motion queue
+    t = 0
+    q = traj.milestones[0]
+    for i in range(len(traj.times)):
+        if traj.times[i] > t:
+            motion.robot.arms_mq.appendLinear((traj.times[i]-t)/speed,traj.milestones[i])
+        t = traj.times[i]
+        q = traj.milestones[i]
+    while motion.robot.arms_mq.moving():
+        time.sleep(0.1)        
+except KeyboardInterrupt:
+    print "Exiting."

common/Testing/arm_motion_record.py

@@ -0,0 +1,55 @@
+import sys; sys.path.append('.')
+import time
+from Motion import motion
+from Motion import config
+
+#save at 10Hz, by default.  Set to 0 to save by pressing enter
+rate = 10.0
+#save here, by default
+fn = "arm_motion.traj"
+
+print "Saves arm motions to disk for later inspection and playback"
+print "Usage: arm_motion_record.py [RATE] [FN]"
+print " [RATE] can be 0 to only save when enter is pressed"
+print " [FN] is the file to save to"
+if len(sys.argv) > 1:
+    rate = float(sys.argv[1])
+
+if rate > 0:
+    print "Saving motion at %g Hz... "%(rate,)
+else:
+    print "Saving motion when enter is pressed"
+
+if len(sys.argv) > 2:
+    fn = sys.argv[2]
+
+print "Saving motion to",fn
+f = open(fn,'w')
+
+#simple motion setup
+config.setup(parse_sys=False)
+
+motion.robot.startup()
+lastt = None
+lastq = None
+try:
+    print "Beginning capture loop, press Ctrl+C to quit"
+    t0 = time.time()
+    while True:
+        t = time.time() - t0
+        if t < 1e-3: t = 0
+        q = motion.robot.left_limb.sensedPosition()+motion.robot.right_limb.sensedPosition()
+        if q != lastq:
+            if lastq is not None:
+                f.write(str(lastt) + '\t' + str(len(lastq)) + '\t'+ ' '.join(str(v) for v in lastq)+'\n')
+            f.write(str(t) + '\t' + str(len(q)) + '\t'+ ' '.join(str(v) for v in q)+'\n')
+            lastt = t
+            lastq = q
+        if rate <= 0:
+            raw_input("Press enter to capture > ")
+        else:
+            time.sleep(1.0/rate)
+except KeyboardInterrupt:
+    f.close()
+    print "Saved %g seconds of motion. Exiting."%(time.time()-t0,)
+    pass