diff --git a/dmp/my_sol/forcemode_example.py b/dmp/my_sol/forcemode_example.py
index ade993431d38bd78282412aca67aaa4a6b3528ac..c15dc17df0e66a11b6a26bfd141d98b4d14aff86 100644
--- a/dmp/my_sol/forcemode_example.py
+++ b/dmp/my_sol/forcemode_example.py
@@ -1,27 +1,73 @@
-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()
diff --git a/dmp/my_sol/run_dmp.py b/dmp/my_sol/run_dmp.py
index 504261685dd6c18f51a2d97de251ca5215eafe27..be774e58af25b8e02c40a990de8fdc460e8f7154 100644
--- a/dmp/my_sol/run_dmp.py
+++ b/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