Skip to content
Snippets Groups Projects
Commit f8f8c621 authored by m-guberina's avatar m-guberina
Browse files

fixed the python clik. theres now actually more things to put in the cpp clik...

fixed the python clik. theres now actually more things to put in the cpp clik (the signal handling for safe stopping, better goal definitions and a few more goodies). there also exists freedrive access from the computer in the utils folder
parent 11d7d098
No related branches found
No related tags found
No related merge requests found
clik/build/
build/
**/.build
**/build
......@@ -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!")
......
File deleted
File deleted
No preview for this file type
......@@ -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
......
......@@ -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)
......
......@@ -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)
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment