commit before making a branch

dmp/my_sol/forcemode_example.py

-from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_control import RTDEControlInterface
+from rtde_receive import RTDEReceiveInterface
+import pinocchio as pin
+import numpy as np
+import os
+import time
+import signal
+import matplotlib.pyplot as plt
 
-rtde_c = RTDEControl("192.168.1.102")
+rtde_control = RTDEControlInterface("192.168.1.102")
+rtde_receive = RTDEReceiveInterface("192.168.1.102")
 
+def handler(signum, frame):
+    rtde_control.forceModeStop()
+    print('sending 100 speedjs full of zeros')
+    for i in range(100):
+        vel_cmd = np.zeros(6)
+        rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+    rtde_control.stopScript()
+    exit()
+
+signal.signal(signal.SIGINT, handler)
+
+# task frame defines force frame relative to base frame
 task_frame = [0, 0, 0, 0, 0, 0]
-selection_vector = [0, 0, 1, 0, 0, 0]
-wrench_down = [0, 0, -10, 0, 0, 0]
-wrench_up = [0, 0, 10, 0, 0, 0]
-force_type = 2
+# these are in {0,1} and select which task frame direction compliance is active in
+selection_vector = [1, 1, 0, 0, 0, 0]
+# the wrench applied to the environment: 
+# position is adjusted to achieve the specified wrench
+wrench = [-10, 0, 0, 0, 0, 0]
+# type is in {1,2,3}
+# 1: force frame is transformed so that it's y-axis is aligned
+#    with a vector from tcp to origin of force frame (what and why??)
+# 2: force frame is not transformed
+# 3: transforms force frame s.t. it's x-axis is the projection of
+#    tcp velocity to the x-y plane of the force frame (again, what and why??)
+ftype = 2
+# limits for:
+# - compliant axes: highest tcp velocities allowed on compliant axes
+# - non-compliant axes: maximum tcp position error compared to the program (which prg,
+#                       and where is this set?)
 limits = [2, 2, 1.5, 1, 1, 1]
-joint_q = [-1.52, -1.411, -1.374, -2.026, 1.375, -0.034]
+# NOTE: avoid movements parallel to compliant direction
+# NOTE: avoid high a-/de-celeration because this decreses force control accuracy
+# there's also force_mode_set_damping:
+# - A value of 1 is full damping, so the robot will decellerate quickly if no
+#   force is present. A value of 0 is no damping, here the robot will maintain
+#   the speed. 
+# - call this before calling force mode if you want it to work.
 
-# Move to initial joint position with a regular moveJ
-rtde_c.moveJ(joint_q)
+update_rate = 500
+dt = 1 / update_rate
+
+q = np.array(rtde_receive.getActualQ())
+rtde_control.forceMode(task_frame, selection_vector, wrench, ftype, limits)
 
-# Execute 500Hz control loop for 4 seconds, each cycle is 2ms
 for i in range(20000):
-    t_start = rtde_c.initPeriod()
-    # First move the robot down for 2 seconds, then up for 2 seconds
+    start = time.time()
     if i > 10000:
-        rtde_c.forceMode(task_frame, selection_vector, wrench_up, force_type, limits)
+        wrench[0] = 10
+        rtde_control.forceMode(task_frame, selection_vector, wrench, ftype, limits)
+    else:
+        rtde_control.forceMode(task_frame, selection_vector, wrench, ftype, limits)
+    end = time.time()
+    diff = end - start
+    if dt < diff:
+        print("missed deadline by", diff - dt)
+        continue
     else:
-        rtde_c.forceMode(task_frame, selection_vector, wrench_down, force_type, limits)
-    rtde_c.waitPeriod(t_start)
+        time.sleep(dt - diff)
 
-rtde_c.forceModeStop()
-rtde_c.stopScript()
+rtde_control.forceModeStop()
+rtde_control.stopScript()

dmp/my_sol/run_dmp.py

@@ -56,24 +56,24 @@ def handler(signum, frame):
         rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
 
     print('also plotting')
-    time = np.arange(N_ITER)
+    t = np.arange(N_ITER)
     ax_q = plt.subplot(221)
     ax_dq = plt.subplot(222)
     ax_dmp_q = plt.subplot(223)
     ax_dmp_dq = plt.subplot(224)
     for i in range(5):
-        ax_q.plot(time, qs[:,i], color='blue')
-        ax_dq.plot(time, dqs[:,i], color='red')
-        ax_dmp_q.plot(time, dmp_poss[:,i], color='green')
-        ax_dmp_dq.plot(time, dmp_vels[:,i], color='orange')
+        ax_q.plot(t, qs[:,i], color='blue')
+        ax_dq.plot(t, dqs[:,i], color='red')
+        ax_dmp_q.plot(t, dmp_poss[:,i], color='green')
+        ax_dmp_dq.plot(t, dmp_vels[:,i], color='orange')
     # for labels
-    ax_q.plot(time, qs[:,5], color='blue', label='q')
+    ax_q.plot(t, qs[:,5], color='blue', label='q')
     ax_q.legend()
-    ax_dq.plot(time, dqs[:,5], color='red', label='dq')
+    ax_dq.plot(t, dqs[:,5], color='red', label='dq')
     ax_dq.legend()
-    ax_dmp_q.plot(time, dmp_poss[:,5], color='green', label='dmp_q')
+    ax_dmp_q.plot(t, dmp_poss[:,5], color='green', label='dmp_q')
     ax_dmp_q.legend()
-    ax_dmp_dq.plot(time, dmp_vels[:,5], color='orange', label='dmp_dq')
+    ax_dmp_dq.plot(t, dmp_vels[:,5], color='orange', label='dmp_dq')
     ax_dmp_dq.legend()
     plt.show()
     exit()
@@ -84,7 +84,7 @@ signal.signal(signal.SIGINT, handler)
 dmp = DMP()
 rtde_control.moveJ(dmp.pos.reshape((6,)))
 # TODO check that you're there instead of a sleep
-#time.sleep(3)
+#t.sleep(3)
 
 TEMPORAL_COUPLING = True
 update_rate = 500