diff --git a/.gitignore b/.gitignore
index f5743f46d59772606397545ef608aa89c9a96696..0f103fcac4193c7dc957f3466321a33e7f3067ce 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
clik/build/
build/
+**/.build
+**/build
diff --git a/clik/clik.py b/clik/clik.py
index af7ab74dc953e77350d0cfa36e6ea515729cc71b..55a899d2575a674f66782edb344bb82085926cc3 100644
--- a/clik/clik.py
+++ b/clik/clik.py
@@ -10,6 +10,8 @@ from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
from robotiq_gripper import RobotiqGripper
import os
+import copy
+import signal
#SIMULATION = True
SIMULATION = False
@@ -55,6 +57,7 @@ if not SIMULATION:
rtde_receive = RTDEReceive("192.168.1.102")
#NOTE: socket_timeout is the third argument, check what it does
gripper.connect("192.168.1.102", 63352)
+ # this is a blocking call
gripper.activate()
else:
rtde_control = RTDEControl("127.0.0.1")
@@ -63,18 +66,45 @@ else:
#gripper.connect("127.0.0.1", 63352)
# define goal
-Mgoal = pin.SE3(np.eye(3), np.array([0.5, 0.3, 0.5]))
-eps = 1e-4
-IT_MAX = 10000000
-DT = 1e-3
+
+JOINT_ID = 6
+q = rtde_receive.getActualQ()
+q.append(0.0)
+q.append(0.0)
+pin.forwardKinematics(model, data, np.array(q))
+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])
+print("goal", Mgoal)
+eps = 1e-3
+IT_MAX = 100000
+update_rate = 500
+dt = 1/update_rate
damp = 1e-6
+acceleration = 1.0
+
+# if you just stop it normally, it will continue running
+# the last speedj lmao
+def handler(signum, frame):
+ print('sending 100 speedjs full of zeros')
+ for i in range(100):
+ rtde_control.endFreedriveMode()
+ vel_cmd = np.zeros(6)
+ rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+ exit()
+
+signal.signal(signal.SIGINT, handler)
success = False
-JOINT_ID = 6
for i in range(IT_MAX):
+ start = time.time()
q = rtde_receive.getActualQ()
if not SIMULATION:
gripper_pos = gripper.get_current_position()
+ # all 3 are between 0 and 255
+ #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)
@@ -96,15 +126,26 @@ for i in range(IT_MAX):
# this does the whole thing unlike the C++ version lel
J = pin.computeJointJacobian(model, data, q, JOINT_ID)
J = J + np.eye(J.shape[0], J.shape[1]) * 10**-4
- v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector)
- v = np.array(v[:6])
+ # idk what i was thinking here lol
+ #v = np.matmul(np.linalg.pinv(J), pin.log(SEerror.inverse() * Mgoal).vector)
+ v = J.T @ err_vector
+ v_cmd = v[:6]
+ v_cmd = v_cmd * 5
+ v_cmd = np.clip(v_cmd, -2, 2)
if not SIMULATION:
- rtde_control.speedJ(v, 0.1, 1.0 / 500)
+ rtde_control.speedJ(v_cmd, acceleration, dt)
else:
- q = pin.integrate(model, q, v * DT)
- if not i % 5000:
+ q = pin.integrate(model, q, v * dt)
+ if not i % 1000:
print(" error = ", err_vector.transpose())
print("pos", data.oMi[JOINT_ID])
+ end = time.time()
+ diff = end - start
+ if dt < diff:
+ print("missed deadline by", diff - dt)
+ continue
+ else:
+ time.sleep(dt - diff)
if success:
print("Convergence achieved!")
diff --git a/dmp/motion_planning/dmp/.dmp.py.swp b/dmp/motion_planning/dmp/.dmp.py.swp
deleted file mode 100644
index a3910b56929b4cb2d4e3a6d1fde51c388d51c8a8..0000000000000000000000000000000000000000
Binary files a/dmp/motion_planning/dmp/.dmp.py.swp and /dev/null differ
diff --git a/dmp/my_sol/.test_traj_w_speedj.py.swp b/dmp/my_sol/.test_traj_w_speedj.py.swp
deleted file mode 100644
index b0a2e39c26f4e81689ada873f61d6c521e61172c..0000000000000000000000000000000000000000
Binary files a/dmp/my_sol/.test_traj_w_speedj.py.swp and /dev/null differ
diff --git a/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc b/dmp/my_sol/__pycache__/create_dmp.cpython-310.pyc
index 6624a8482e4f4610de7960c76fea932f0bb877bb..3ced647a15e91d71a1f56852c1755b0f84070bf1 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/create_dmp.py b/dmp/my_sol/create_dmp.py
index c524edceaefe4fa0c38701f60d072123571ebf70..a3a7d806f3f3ad6bf0ab1136eb9dee2d97ee3a82 100644
--- a/dmp/my_sol/create_dmp.py
+++ b/dmp/my_sol/create_dmp.py
@@ -42,7 +42,7 @@ class DMP:
# and this is just joint positions
trajectory_loadpath = './ur10_omega_trajectory.csv'
data = np.genfromtxt(trajectory_loadpath, delimiter=',')
- self.time = data[:, 0]
+ self.time = data[:, 0] * 1.5
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 171fa7fd2474d341c1481495166b4e99dcebca55..7e95e8f0378867e3c766fae3e0caa530d270ffeb 100644
--- a/dmp/my_sol/run_dmp.py
+++ b/dmp/my_sol/run_dmp.py
@@ -9,6 +9,10 @@ import time
import signal
import matplotlib.pyplot as plt
+# TODO
+# add scaling (point of albin's paper)
+# generate this from other trajectories
+
#urdf_path_relative = "../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
#urdf_path_absolute = os.path.abspath(urdf_path_relative)
@@ -25,7 +29,7 @@ rtde_receive = RTDEReceive("192.168.1.102")
#rtde_control = RTDEControl("127.0.0.1")
#rtde_receive = RTDEReceive("127.0.0.1")
-N_ITER = 10000
+N_ITER = 20000
qs = np.zeros((N_ITER, 6))
dmp_poss = np.zeros((N_ITER, 6))
dqs = np.zeros((N_ITER, 6))
@@ -74,8 +78,8 @@ update_rate = 500
dt = 1.0 / update_rate
JOINT_ID = 6
-kp = 100
-acceleration = 0.15
+kp = 10
+acceleration = 1.5
print("starting the trajectory")
for i in range(N_ITER):
@@ -93,7 +97,6 @@ for i in range(N_ITER):
vel_cmd = np.clip(vel_cmd, -2.0, 2.0)
if np.isnan(vel_cmd[0][0]):
break
- exit()
#rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
rtde_control.speedJ(vel_cmd, acceleration, dt)
@@ -103,11 +106,14 @@ for i in range(N_ITER):
dmp_vels[i] = dmp.vel.reshape((6,))
end = time.time()
diff = end - start
- print(diff)
- try:
+ if dt < diff:
+ print("missed deadline by", diff - dt)
+ continue
+ else:
time.sleep(dt - diff)
- except ValueError:
- print("it took", dt - (end - start), "more than", dt, "seconds")
+ # TODO do this with something sensible
+ if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
+ break
handler(None, None)
diff --git a/dmp/my_sol/test_traj_w_speedj.py b/dmp/my_sol/test_traj_w_speedj.py
index c00dc8eca82ee5ba4e62492873dc0b0be4da53aa..c275f1d955f56fa7d72a04258b95b5ae17a8f8d7 100644
--- a/dmp/my_sol/test_traj_w_speedj.py
+++ b/dmp/my_sol/test_traj_w_speedj.py
@@ -29,14 +29,23 @@ t = data[:, 0]
t = t.reshape(1, len(t))
y = np.array(data[:, 1:]).T
+update_rate = 500
+dt = 1.0 / update_rate
+kp = 2
+
n = y.shape[0]
-yd_demo = (y[:, 1:] - y[:, :-1]) / (t[0, 1:] - t[0, :-1])
-yd_demo = np.concatenate((yd_demo, np.zeros((n, 1))), axis=1)
-#print(yd_demo)
+yd = (y[:, 1:] - y[:, :-1]) / (t[0, 1:] - t[0, :-1])
+yd = np.concatenate((yd, np.zeros((n, 1))), axis=1)
+print(yd.shape)
rtde_control.moveJ(y[:,0])
-for i in range(yd_demo.shape[1]):
+for i in range(yd.shape[1]):
start = time.time()
- rtde_control.speedJ(y[:,i], 0.1, 1/500)
+ q = np.array(rtde_receive.getActualQ())
+ vel_cmd = yd[:,i] + kp * (y[:,i] - q.reshape((6,1)))
+ rtde_control.speedJ(yd[:,i], 0.1, 1/500)
end = time.time()
diff = end - start
- time.sleep(diff)
+ if dt < diff:
+ continue
+ else:
+ time.sleep(dt - diff)
diff --git a/util/freedrive.py b/util/freedrive.py
index c0f753429327e865419df2d8799c6af2d675f63a..aa33bbf4f948c92b79853287c2905988efe410a6 100644
--- a/util/freedrive.py
+++ b/util/freedrive.py
@@ -11,6 +11,9 @@ def handler(signum, frame):
rtde_control = RTDEControl("192.168.1.102")
rtde_receive = RTDEReceive("192.168.1.102")
+while not rtde_control.isConnected():
+ continue
+print("connected")
rtde_control.freedriveMode()
signal.signal(signal.SIGINT, handler)