diff --git a/TODOS b/TODOS
index 2efde79ddc07f8349329f836aa510f3dc59362c2..c68c61e3e40f314d10c45e2448042f15e6707cb4 100644
--- a/TODOS
+++ b/TODOS
@@ -7,6 +7,10 @@ IMPORTANT
- do test to see if the calibration is ok
- recalibrate if necessary
+2. servoj
+- try to use this instead of speedj and see what you get
+- you need to set up tests inteligently to actually measure what you want
+
NICE TO HAVE
---------------
1. other inverse kinematics algorithms
diff --git a/clik/clik.py b/clik/clik.py
index 55a899d2575a674f66782edb344bb82085926cc3..30b38e958c905412a926644c8c78165378986834 100644
--- a/clik/clik.py
+++ b/clik/clik.py
@@ -76,7 +76,7 @@ Mtool = data.oMi[JOINT_ID]
print("pos", Mtool)
#SEerror = pin.SE3(np.zeros((3,3)), np.array([0.0, 0.0, 0.1])
Mgoal = copy.deepcopy(Mtool)
-Mgoal.translation = Mgoal.translation + np.array([-0.2, 0.2, 0.1])
+Mgoal.translation = Mgoal.translation + np.array([-0.2, -0.2, -0.1])
print("goal", Mgoal)
eps = 1e-3
IT_MAX = 100000
@@ -104,7 +104,7 @@ for i in range(IT_MAX):
if not SIMULATION:
gripper_pos = gripper.get_current_position()
# all 3 are between 0 and 255
- #gripper.move(i % 255, 100, 100)
+ gripper.move(i % 255, 100, 100)
# just padding to fill q, which is only needed for forward kinematics
#q.append(gripper_pos)
#q.append(gripper_pos)
diff --git a/dmp/my_sol/.run_dmp.py.swp b/dmp/my_sol/.run_dmp.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..362c6bac7ccaa4eac41286b03a9e5bfb24526142
Binary files /dev/null and b/dmp/my_sol/.run_dmp.py.swp differ
diff --git a/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc b/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc
index 3ced647a15e91d71a1f56852c1755b0f84070bf1..3758f9217ef8902406dbbdc7eeffcf674a23ca91 100644
Binary files a/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc and b/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc differ
diff --git a/dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc b/dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..31d5b4d59ffa1b2859605ad73e9336eadddcd992
Binary files /dev/null and b/dmp/my_sol/__pycache__/temporal_coupling.cpython-310.pyc differ
diff --git a/dmp/my_sol/create_dmp.py b/dmp/my_sol/create_dmp.py
index a3a7d806f3f3ad6bf0ab1136eb9dee2d97ee3a82..74282cf13a9958ab86f3ead837ecc7a324aa8d5c 100644
--- a/dmp/my_sol/create_dmp.py
+++ b/dmp/my_sol/create_dmp.py
@@ -42,7 +42,9 @@ class DMP:
# and this is just joint positions
trajectory_loadpath = './ur10_omega_trajectory.csv'
data = np.genfromtxt(trajectory_loadpath, delimiter=',')
- self.time = data[:, 0] * 1.5
+ # this one is slow enough
+ #self.time = data[:, 0] * 1.5
+ self.time = data[:, 0]
self.time = self.time.reshape(1, len(self.time))
self.y = np.array(data[:, 1:]).T
diff --git a/dmp/my_sol/run_dmp.py b/dmp/my_sol/run_dmp.py
index 7e95e8f0378867e3c766fae3e0caa530d270ffeb..e9577e5180273fcdbabafa5f66b641fd339eb4fc 100644
--- a/dmp/my_sol/run_dmp.py
+++ b/dmp/my_sol/run_dmp.py
@@ -2,6 +2,7 @@ from create_dmp import DMP
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
from robotiq_gripper import RobotiqGripper
+from temporal_coupling import NoTC, TCVelAccConstrained
import pinocchio as pin
import numpy as np
import os
@@ -29,7 +30,7 @@ rtde_receive = RTDEReceive("192.168.1.102")
#rtde_control = RTDEControl("127.0.0.1")
#rtde_receive = RTDEReceive("127.0.0.1")
-N_ITER = 20000
+N_ITER = 10000
qs = np.zeros((N_ITER, 6))
dmp_poss = np.zeros((N_ITER, 6))
dqs = np.zeros((N_ITER, 6))
@@ -73,31 +74,44 @@ rtde_control.moveJ(dmp.pos.reshape((6,)))
# TODO check that you're there instead of a sleep
#time.sleep(3)
-#exit()
+TEMPORAL_COUPLING = True
update_rate = 500
dt = 1.0 / update_rate
JOINT_ID = 6
-kp = 10
-acceleration = 1.5
+
+# parameters from yaml config file in albin's repo
+kp = 2
+acceleration = 1.7
+
+if not TEMPORAL_COUPLING:
+ tc = NoTC()
+else:
+ # TODO learn the math already
+ tau0 = 5
+ gamma_nominal = 1.0
+ gamma_a = 0.5
+ eps = 0.001
+ v_max = np.ones(6) * 2
+ a_max = np.ones(6) * 1.7
+ tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
+
print("starting the trajectory")
for i in range(N_ITER):
start = time.time()
-#while True:
+ # dmp
dmp.step(dt)
+ # temporal coupling
+ tau = dmp.tau + tc.update(dmp, dt) * dt
+ dmp.set_tau(tau)
q = np.array(rtde_receive.getActualQ())
dq = np.array(rtde_receive.getActualQd())
- #print("dq:", dq)
vel_cmd = dmp.vel + kp * (dmp.pos - q.reshape((6,1)))
- #print("dmp.vel", dmp.vel.reshape((6,)))
- #print("dmp.pos", dmp.pos.reshape((6,)))
- #print("q:", q)
- #print("vel_cmd", vel_cmd.reshape((6,)))
- vel_cmd = np.clip(vel_cmd, -2.0, 2.0)
- if np.isnan(vel_cmd[0][0]):
+ vel_cmd = vel_cmd.reshape((6,))
+ vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
+ if np.isnan(vel_cmd[0]):
break
- #rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
rtde_control.speedJ(vel_cmd, acceleration, dt)
qs[i] = q.reshape((6,))
diff --git a/dmp/my_sol/temporal_coupling.py b/dmp/my_sol/temporal_coupling.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d9107a02500449d00e5644d64f6291c46a9290b
--- /dev/null
+++ b/dmp/my_sol/temporal_coupling.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+
+class NoTC:
+ def update(self, dmp, dt):
+ return 0
+
+
+class TCVelAccConstrained:
+
+ def __init__(self, gamma_nominal, gamma_a, v_max, a_max, eps=0.001):
+ self.gamma_nominal = gamma_nominal
+ self.gamma_a = gamma_a
+ self.eps = eps
+ self.v_max = v_max.reshape((len(v_max), 1))
+ self.a_max = a_max.reshape((len(a_max), 1))
+
+ def generate_matrices(self, dmp, dt):
+ A = np.vstack((-dmp.z(), dmp.z()))
+ B = np.vstack((-self.a_max, -self.a_max))
+ C = np.vstack((dmp.h(), -dmp.h()))
+ D = np.vstack((-self.v_max, -self.v_max))
+ x_next = dmp.x + dmp.f(dmp.x) / dmp.tau * dt
+ A_next = np.vstack((-dmp.z(x_next), dmp.z(x_next)))
+ C_next = np.vstack((dmp.h(x_next), -dmp.h(x_next)))
+ return A, B, C, D, A_next, C_next
+
+ def update(self, dmp, dt):
+
+ A, B, C, D, A_next, C_next = self.generate_matrices(dmp, dt)
+
+ # Acceleration bounds
+ i = np.squeeze(A < 0)
+ if i.any():
+ taud_min_a = np.max(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_min_a = -np.inf
+ i = np.squeeze(A > 0)
+ if i.any():
+ taud_max_a = np.min(- (B[i] * dmp.tau ** 2 + C[i]) / A[i])
+ else:
+ taud_max_a = np.inf
+ # Velocity bounds
+ i = range(len(A_next))
+ tau_min_v = np.max(-A_next[i] / D[i])
+ taud_min_v = (tau_min_v - dmp.tau) / dt
+ # Feasibility bounds
+ ii = np.arange(len(A_next))[np.squeeze(A_next < 0)]
+ jj = np.arange(len(A_next))[np.squeeze(A_next > 0)]
+ tau_min_f = -np.inf
+ for i in ii:
+ for j in jj:
+ num = C_next[i] * abs(A_next[j]) + C_next[j] * abs(A_next[i])
+ if num > 0:
+ den = abs(B[i] * A_next[j]) + abs(B[j] * A_next[i])
+ tmp = np.sqrt(num / den)
+ if tmp > tau_min_f:
+ tau_min_f = tmp
+ taud_min_f = (tau_min_f - dmp.tau) / dt
+ # Nominal bound
+ taud_min_nominal = (dmp.tau0 - dmp.tau) / dt
+
+ taud_min = np.max((taud_min_a, taud_min_v, taud_min_f, taud_min_nominal))
+
+ # Base update law
+ ydd_bar = dmp.h() / (dmp.tau**2 * self.a_max)
+ if self.gamma_a > 0:
+ pot_a = self.gamma_a * np.sum(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ else:
+ pot_a = 0
+ #pot_a = self.gamma_a * np.amax(ydd_bar ** 2 / np.maximum(1 - ydd_bar ** 2, self.gamma_a * self.eps * np.ones((len(ydd_bar), 1))))
+ taud = self.gamma_nominal * (dmp.tau0 - dmp.tau) + dmp.tau * pot_a
+
+ # Saturate
+ taud = np.min((taud, taud_max_a))
+ taud = np.max((taud, taud_min))
+
+ return taud