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

almost works

parent 040fac18
No related branches found
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
dmp/my_sol/drawing_gen/cliking_the_path.py
+ 1
− 1
View file @ abe5208f
...@@ -74,7 +74,7 @@ transf_body_to_board = pin.SE3(R, p) ...@@ -74,7 +74,7 @@ transf_body_to_board = pin.SE3(R, p)
# offset in z # offset in z
# very close # very close
#path = path + np.array([0.0, 0.0, -0.0238]) #path = path + np.array([0.0, 0.0, -0.0238])
path = path + np.array([0.0, 0.0, -0.1038]) path = path + np.array([0.0, 0.0, -0.0948])
#path = path + np.array([0.0, 0.2, -0.1438]) #path = path + np.array([0.0, 0.2, -0.1438])
for i in range(len(path)): for i in range(len(path)):
......
dmp/my_sol/drawing_gen/path_in_pixels.csv
+ 153
− 62
View file @ abe5208f
0.14553,0.49903 0.27227,0.61767
0.14763,0.49903 0.27375,0.61767
0.14973,0.49903 0.27523,0.61767
0.15183,0.49903 0.27671,0.61767
0.16024,0.49903 0.27819,0.61767
0.16654,0.49903 0.27967,0.61767
0.17495,0.49903 0.28115,0.61767
0.19176,0.49903 0.28263,0.61767
0.19807,0.49903 0.28411,0.61767
0.22118,0.49903 0.28411,0.61445
0.22328,0.49903 0.28559,0.61445
0.22749,0.49903 0.28707,0.61445
0.24010,0.49903 0.28855,0.61445
0.24850,0.49903 0.29003,0.61445
0.25271,0.49903 0.29151,0.61445
0.27372,0.49903 0.29299,0.61445
0.27582,0.49903 0.29447,0.61445
0.29894,0.49903 0.29595,0.61445
0.30524,0.49903 0.29743,0.61445
0.30945,0.49903 0.29891,0.61445
0.31785,0.49903 0.30038,0.61445
0.33677,0.49903 0.30186,0.61445
0.34097,0.49903 0.30630,0.61445
0.35568,0.49903 0.30778,0.61445
0.37459,0.49903 0.30926,0.61445
0.38090,0.49903 0.31222,0.61445
0.41032,0.49903 0.31370,0.61123
0.41662,0.49903 0.31518,0.61123
0.42293,0.49903 0.32850,0.61123
0.42713,0.49903 0.32998,0.61123
0.45025,0.49903 0.33146,0.61123
0.45655,0.49903 0.34626,0.61123
0.47967,0.49903 0.35070,0.61123
0.48387,0.49903 0.35218,0.60800
0.49228,0.49903 0.35661,0.60800
0.49648,0.49903 0.36401,0.60800
0.50699,0.50202 0.36845,0.60800
0.51960,0.50202 0.37289,0.60800
0.54271,0.50202 0.38177,0.60156
0.54902,0.50202 0.38325,0.60156
0.56793,0.50202 0.39361,0.59189
0.60156,0.50649 0.39805,0.59189
0.60786,0.50649 0.40397,0.58867
0.63518,0.50649 0.41136,0.58222
0.64989,0.51097 0.41728,0.58222
0.67090,0.51097 0.41876,0.57900
0.71714,0.51545 0.42024,0.57900
0.73185,0.51993 0.42172,0.57578
0.76547,0.51993 0.42320,0.57256
0.79489,0.52441 0.42468,0.57256
0.80330,0.52441 0.42616,0.57256
0.80960,0.52441 0.43060,0.56611
0.83272,0.52739 0.43356,0.56289
0.84113,0.52739 0.43948,0.55967
0.84533,0.52739 0.44540,0.55322
0.86634,0.52739 0.44836,0.55322
0.86845,0.52739 0.45132,0.55000
0.87685,0.53038 0.45428,0.55000
0.88105,0.53038 0.46315,0.54355
0.89577,0.53038 0.46463,0.54033
0.90207,0.53038 0.46907,0.54033
0.90207,0.53038 0.47795,0.53389
0.47943,0.53066
0.48387,0.53066
0.49423,0.52422
0.49571,0.51777
0.50015,0.51777
0.50903,0.51455
0.51347,0.51455
0.51495,0.51133
0.52826,0.50166
0.52974,0.50166
0.53418,0.50166
0.54454,0.49521
0.54750,0.49521
0.55046,0.49199
0.55342,0.49199
0.56378,0.48555
0.56674,0.48555
0.56822,0.48555
0.58449,0.48555
0.58597,0.48232
0.59485,0.48232
0.59929,0.48232
0.60077,0.48232
0.60225,0.48232
0.60521,0.48232
0.60965,0.48232
0.61113,0.48232
0.61557,0.48232
0.61705,0.48232
0.61853,0.48232
0.62149,0.48232
0.62297,0.48232
0.62445,0.48232
0.62888,0.48232
0.63332,0.48232
0.63480,0.48232
0.64072,0.48232
0.64220,0.48232
0.64516,0.48232
0.64664,0.48232
0.64812,0.48232
0.65256,0.48232
0.65404,0.48232
0.65848,0.48232
0.66292,0.48232
0.67032,0.48877
0.67476,0.48877
0.67624,0.48877
0.68511,0.48877
0.68659,0.48877
0.68807,0.48877
0.69251,0.48877
0.69399,0.48877
0.69843,0.49199
0.69991,0.49199
0.70139,0.49199
0.70287,0.49199
0.70435,0.49199
0.70583,0.49199
0.70731,0.49199
0.70879,0.49199
0.71323,0.49199
0.71471,0.49199
0.71915,0.49521
0.72359,0.49521
0.72803,0.49521
0.73542,0.49521
0.73690,0.49521
0.73838,0.49521
0.74282,0.49521
0.74430,0.49521
0.74874,0.49521
0.75318,0.50166
0.75466,0.50166
0.75910,0.50166
0.76058,0.50166
0.76206,0.50166
0.76354,0.50166
0.76502,0.50166
0.76650,0.50166
0.76798,0.50166
0.76946,0.50166
0.77094,0.50166
0.77242,0.50166
0.77390,0.50166
0.77538,0.50166
0.77982,0.50166
0.78130,0.50166
0.78130,0.50488
0.78278,0.50488
0.78278,0.50488
dmp/my_sol/new_traj.csv
+ 242
− 241
View file @ abe5208f
This diff is collapsed.
dmp/my_sol/run_dmp.py
+ 57
− 20
View file @ abe5208f
...@@ -10,25 +10,21 @@ import os ...@@ -10,25 +10,21 @@ import os
import time import time
import signal import signal
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import sys
import copy
sys.path.insert(0, '../../util')
from give_me_the_calibrated_model import get_model
# TODO urdf_path_relative = "../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
# add scaling (point of albin's paper) urdf_path_absolute = os.path.abspath(urdf_path_relative)
# generate this from other trajectories mesh_dir = "../../robot_descriptions/"
mesh_dir_absolute = os.path.abspath(mesh_dir)
model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
#urdf_path_relative = "../../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
#urdf_path_absolute = os.path.abspath(urdf_path_relative)
#mesh_dir = "../../robot_descriptions/"
#mesh_dir_absolute = os.path.abspath(mesh_dir)
##print(mesh_dir_absolute)
#model = pin.buildModelFromUrdf(urdf_path_absolute)
##print(model)
#data = pin.Data(model)
##print(data)
rtde_control = RTDEControl("192.168.1.102") rtde_control = RTDEControl("192.168.1.102")
rtde_receive = RTDEReceive("192.168.1.102") rtde_receive = RTDEReceive("192.168.1.102")
rtde_io = RTDEIOInterface("192.168.1.102") rtde_io = RTDEIOInterface("192.168.1.102")
# on scale from 0 to 1
rtde_io.setSpeedSlider(0.2) rtde_io.setSpeedSlider(0.2)
# run if marker isn't gripped # run if marker isn't gripped
...@@ -48,10 +44,10 @@ dmp_poss = np.zeros((N_ITER, 6)) ...@@ -48,10 +44,10 @@ dmp_poss = np.zeros((N_ITER, 6))
dqs = np.zeros((N_ITER, 6)) dqs = np.zeros((N_ITER, 6))
dmp_vels = np.zeros((N_ITER, 6)) dmp_vels = np.zeros((N_ITER, 6))
def handler(signum, frame): def handler(signum, frame):
print('sending 100 speedjs full of zeros') print('sending 100 speedjs full of zeros')
for i in range(100): for i in range(100):
rtde_control.endFreedriveMode()
vel_cmd = np.zeros(6) vel_cmd = np.zeros(6)
rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500) rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
...@@ -82,6 +78,9 @@ def handler(signum, frame): ...@@ -82,6 +78,9 @@ def handler(signum, frame):
signal.signal(signal.SIGINT, handler) signal.signal(signal.SIGINT, handler)
dmp = DMP() dmp = DMP()
current_pose = rtde_receive.getActualTCPPose()
current_pose[2] = current_pose[2] + 0.1
rtde_control.moveL(current_pose)
rtde_control.moveJ(dmp.pos.reshape((6,))) rtde_control.moveJ(dmp.pos.reshape((6,)))
# TODO check that you're there instead of a sleep # TODO check that you're there instead of a sleep
#t.sleep(3) #t.sleep(3)
...@@ -91,6 +90,21 @@ update_rate = 500 ...@@ -91,6 +90,21 @@ update_rate = 500
dt = 1.0 / update_rate dt = 1.0 / update_rate
JOINT_ID = 6 JOINT_ID = 6
task_frame = [0, 0, 0, 0, 0, 0]
# these are in {0,1} and select which task frame direction compliance is active in
# just be soft everywhere
selection_vector = [1, 1, 1, 1, 1, 1]
# the wrench applied to the environment:
# position is adjusted to achieve the specified wrench
# let's pretend speedjs are this and see what happens (idk honestly)
wrench = [0, 0, 0, 0, 0, 0]
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?)
# why these values?
limits = [2, 2, 2, 2, 2, 2]
# parameters from yaml config file in albin's repo # parameters from yaml config file in albin's repo
kp = 2 kp = 2
...@@ -109,6 +123,10 @@ else: ...@@ -109,6 +123,10 @@ else:
a_max = np.ones(6) * 1.7 a_max = np.ones(6) * 1.7
tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps) tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps)
# TODO: check sign!
alpha = 0.1
# lame hack for the current orientation (which it doesn't change so it's ok)
wrench_offset = np.array([-1.07988, -1.407225, -2.63069, 0.071115, -0.0487768, 0.089271])
print("starting the trajectory") print("starting the trajectory")
for i in range(N_ITER): for i in range(N_ITER):
...@@ -118,16 +136,35 @@ for i in range(N_ITER): ...@@ -118,16 +136,35 @@ for i in range(N_ITER):
# temporal coupling # temporal coupling
tau = dmp.tau + tc.update(dmp, dt) * dt tau = dmp.tau + tc.update(dmp, dt) * dt
dmp.set_tau(tau) dmp.set_tau(tau)
q = np.array(rtde_receive.getActualQ()) q = rtde_receive.getActualQ()
dq = np.array(rtde_receive.getActualQd()) # TODO this fucker feel the weight of the gripper, this causes bias -> fix it!
vel_cmd = dmp.vel + kp * (dmp.pos - q.reshape((6,1))) wrench = np.array(rtde_receive.getActualTCPForce())
wrench = wrench - wrench_offset
q6 = np.array(copy.deepcopy(q))
q.append(0.0)
q.append(0.0)
q = np.array(q)
pin.forwardKinematics(model, data, q)
J = pin.computeJointJacobian(model, data, q, JOINT_ID)
dq = np.array(rtde_receive.getActualQd()).reshape((6,1))
tau = J.T @ wrench
tau = tau[:6].reshape((6,1))
vel_cmd = dmp.vel + kp * (dmp.pos - q6.reshape((6,1))) - alpha * tau
vel_cmd = vel_cmd.reshape((6,)) vel_cmd = vel_cmd.reshape((6,))
vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max) vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
if np.isnan(vel_cmd[0]): if np.isnan(vel_cmd[0]):
break break
rtde_control.speedJ(vel_cmd, acceleration, dt) rtde_control.speedJ(vel_cmd, acceleration, dt)
# this is very stupind, but it was quick to implement
qs[i] = q.reshape((6,)) #vel_cmd8 = list(vel_cmd)
#vel_cmd8.append(0.0)
#vel_cmd8.append(0.0)
#vel_cmd8 = np.array(vel_cmd8)
#vel_tcp = J @ vel_cmd8
#vel_tcp = vel_tcp * 10
#rtde_control.forceMode(task_frame, selection_vector, vel_tcp, ftype, limits)
qs[i] = q6.reshape((6,))
dmp_poss[i] = dmp.pos.reshape((6,)) dmp_poss[i] = dmp.pos.reshape((6,))
dqs[i] = dq.reshape((6,)) dqs[i] = dq.reshape((6,))
dmp_vels[i] = dmp.vel.reshape((6,)) dmp_vels[i] = dmp.vel.reshape((6,))
......
util/ft_readings.py 0 → 100644
+ 46
− 0
View file @ abe5208f
import pinocchio as pin
import numpy as np
import sys
import os
from os.path import dirname, join, abspath
import time
from pinocchio.visualize import GepettoVisualizer
import gepetto.corbaserver
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
import os
import copy
import signal
from give_me_the_calibrated_model import get_model
def handler(signum, frame):
print('i will end freedrive and exit')
rtde_control.endFreedriveMode()
exit()
rtde_control = RTDEControl("192.168.1.102")
rtde_receive = RTDEReceive("192.168.1.102")
while not rtde_control.isConnected():
continue
print("connected")
signal.signal(signal.SIGINT, handler)
urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
urdf_path_absolute = os.path.abspath(urdf_path_relative)
mesh_dir = "../robot_descriptions/"
mesh_dir_absolute = os.path.abspath(mesh_dir)
model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
while True:
q = rtde_receive.getActualQ()
ft = rtde_receive.getActualTCPForce()
print(ft)
# q.append(0.0)
# q.append(0.0)
# pin.forwardKinematics(model, data, np.array(q))
# print(data.oMi[6])
# print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
# print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
# time.sleep(0.005)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment