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

some calibration is being done. math is correct, but there are some errors... 

some calibration is being done. math is correct, but there are some errors coming from god knows where. right now i am blaming the curvature of the board. next step is to move while having force in the z direction. to do the calib, start in top left corner.
parent 488d99b2
No related branches found
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
Showing
with 438 additions and 866 deletions
dmp/my_sol/drawing_gen/cliking_the_path.py
+ 26
− 27
View file @ 056f87e3
......@@ -29,6 +29,8 @@ board_width = 0.35
board_height = 0.4
path[:,0] = path[:,0] * board_width
path[:,1] = path[:,1] * board_height
# in the new coordinate system we're going in the -y direction
path[:,1] = -1 * path[:,1] + board_height
# now the path is appropriately scaled and in the first quadrant
......@@ -51,37 +53,33 @@ path[:,1] = path[:,1] * board_height
# (in pin coordinates, real ones are [-x, -y, z])
# UPPER LEFT POINT
# 0.1065, 0.7083, 0.6362, -2.6137, -0.0248, -0.0033
rpy = np.array([-2.6137, -0.0248, -0.0033])
R = pin.rpy.rpyToMatrix(rpy)
#rpy = np.array([-2.6137, -0.0248, -0.0033])
#R = pin.rpy.rpyToMatrix(rpy)
#print(R)
#R = np.array([[ 1. , 0. , 0.14570094],
# [ 0. , -0.70647877, 0.69257417],
# [ 0. , -0.69257417, -0.70647877]])
#R = np.array([[ 1. , 0. , 0.1496001 ],
# [ 0. , -0.71010996, 0.68801429],
# [ 0. , -0.68801429, -0.71010996]])
R = np.array([[1., 0., 0.03236534],
[ 0., -0.82404727, 0.56559577],
[ 0. , -0.56559577, -0.82404727]])
print(R)
#R = np.eye(3)
# change +/- stuff if you go the other way idk or c lmao
p = np.array([0.1065, 0.7083, 0.6362])
transf_body_to_board = pin.SE3(R, p)
# LOWER LEFT POINT
# R =
# -0.998592 -0.0509392 -0.0147815
#-0.0332665 0.818552 -0.573468
# 0.0413114 -0.57217 -0.819094
# p = -0.114727 -0.3738 0.401655
# UPPER RIGHT POINT
# is not in the workspace lmao
# LOWER RIGHT POINT
# R =
# -0.999219 -0.0141156 -0.0369115
#0.00980125 0.816329 -0.577504
# 0.0382838 -0.577415 -0.815553
# p = -0.595579 -0.373942 0.413703
path = path + np.array([0.0, 0.0, -0.0838])
# offset in z
# very close
#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.2, -0.1438])
for i in range(len(path)):
path[i] = transf_body_to_board.act(path[i])
#print(path)
# make it hover a bit
#path = path + np.array([-0.02, 0.0, 0.02])
print(path)
#######################################################################
# STEP 2: clik that path #
#######################################################################
......@@ -97,7 +95,8 @@ JOINT_ID = 6
eps = 10**-2
dt = 0.01
# skip inital pos tho
q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
#q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
INIT_ITER = 10000
n_iter = INIT_ITER
RUNNING_ITER = 1000
......@@ -125,7 +124,7 @@ for goal in path:
if n_iter == INIT_ITER:
n_iter = RUNNING_ITER
else:
if i == RUNNING_ITER:
if i == RUNNING_ITER-1:
print("DID NOT CONVERGE")
#######################################################################
......
dmp/my_sol/drawing_gen/path_in_pixels.csv
+ 62
− 174
View file @ 056f87e3
0.23380,0.68264
0.23528,0.68264
0.24268,0.67965
0.25599,0.67965
0.31666,0.67070
0.33590,0.66771
0.38917,0.66174
0.39657,0.65876
0.44540,0.64980
0.47351,0.64532
0.49275,0.63935
0.54010,0.62741
0.54898,0.62741
0.55786,0.62741
0.57413,0.62741
0.57561,0.62741
0.59485,0.62741
0.60077,0.62741
0.60521,0.62741
0.60965,0.62741
0.61113,0.62741
0.61261,0.62741
0.61557,0.62741
0.61705,0.62741
0.61853,0.62741
0.62149,0.62741
0.62297,0.62741
0.62740,0.62741
0.62888,0.62741
0.64812,0.62741
0.65256,0.62741
0.65552,0.62741
0.67624,0.62741
0.68067,0.62741
0.69695,0.62442
0.69843,0.62442
0.70731,0.62144
0.72063,0.62144
0.72803,0.61845
0.74134,0.61845
0.74578,0.61845
0.74726,0.61845
0.76650,0.61546
0.77094,0.61546
0.77982,0.61546
0.78278,0.61397
0.78722,0.61397
0.79017,0.61397
0.79461,0.61397
0.80053,0.61099
0.80201,0.61099
0.80349,0.61099
0.80793,0.61099
0.80941,0.61099
0.81237,0.60949
0.81385,0.60949
0.81533,0.60949
0.81829,0.60502
0.81977,0.60502
0.82125,0.60352
0.82421,0.59606
0.82421,0.59457
0.82421,0.59009
0.82125,0.55725
0.81829,0.55277
0.80053,0.51694
0.79313,0.50799
0.77686,0.48112
0.76502,0.46470
0.76058,0.46022
0.73838,0.43186
0.73099,0.41991
0.70435,0.39006
0.69843,0.38409
0.69251,0.37812
0.66440,0.34677
0.65108,0.33483
0.62740,0.31542
0.61261,0.30646
0.58597,0.29004
0.58005,0.28407
0.57413,0.28109
0.54750,0.26467
0.54158,0.25870
0.48831,0.21988
0.47795,0.21690
0.46759,0.20645
0.45576,0.19898
0.43208,0.18406
0.41876,0.17062
0.39953,0.16017
0.38917,0.15122
0.35809,0.13480
0.35218,0.13181
0.34034,0.12285
0.33886,0.12285
0.33442,0.11987
0.32998,0.11838
0.33146,0.11838
0.32998,0.11838
0.32998,0.11987
0.32998,0.12136
0.32998,0.13032
0.32998,0.16017
0.32998,0.17659
0.32406,0.23332
0.32406,0.24675
0.32406,0.26168
0.32258,0.26914
0.31222,0.32288
0.30630,0.36319
0.30334,0.37812
0.30334,0.37961
0.30038,0.38857
0.29891,0.39304
0.29003,0.41544
0.29003,0.41991
0.28707,0.42141
0.28411,0.43335
0.28263,0.43783
0.27967,0.44828
0.27523,0.46917
0.27375,0.47067
0.27375,0.47515
0.27375,0.49007
0.27227,0.49903
0.27227,0.50948
0.26931,0.51844
0.26635,0.52739
0.26635,0.53187
0.26635,0.53635
0.26339,0.54680
0.26339,0.55128
0.26043,0.56173
0.26043,0.56322
0.25747,0.57367
0.25747,0.57516
0.25747,0.57815
0.25451,0.59307
0.25451,0.59457
0.24859,0.60800
0.24859,0.61248
0.24711,0.61397
0.24711,0.62442
0.24268,0.63786
0.24268,0.63935
0.24268,0.64084
0.23972,0.64383
0.23972,0.64532
0.23972,0.65129
0.23972,0.65278
0.23972,0.65428
0.23972,0.65577
0.23972,0.65726
0.23972,0.65876
0.23972,0.66174
0.23972,0.66473
0.23972,0.66622
0.23972,0.66771
0.23972,0.66920
0.23972,0.67070
0.23824,0.67070
0.23824,0.67219
0.23824,0.67518
0.23824,0.67667
0.23824,0.67816
0.23824,0.67965
0.23676,0.68115
0.23676,0.68264
0.23380,0.69309
0.23084,0.69458
0.23084,0.69757
0.23084,0.69906
0.23084,0.69906
0.14553,0.49903
0.14763,0.49903
0.14973,0.49903
0.15183,0.49903
0.16024,0.49903
0.16654,0.49903
0.17495,0.49903
0.19176,0.49903
0.19807,0.49903
0.22118,0.49903
0.22328,0.49903
0.22749,0.49903
0.24010,0.49903
0.24850,0.49903
0.25271,0.49903
0.27372,0.49903
0.27582,0.49903
0.29894,0.49903
0.30524,0.49903
0.30945,0.49903
0.31785,0.49903
0.33677,0.49903
0.34097,0.49903
0.35568,0.49903
0.37459,0.49903
0.38090,0.49903
0.41032,0.49903
0.41662,0.49903
0.42293,0.49903
0.42713,0.49903
0.45025,0.49903
0.45655,0.49903
0.47967,0.49903
0.48387,0.49903
0.49228,0.49903
0.49648,0.49903
0.50699,0.50202
0.51960,0.50202
0.54271,0.50202
0.54902,0.50202
0.56793,0.50202
0.60156,0.50649
0.60786,0.50649
0.63518,0.50649
0.64989,0.51097
0.67090,0.51097
0.71714,0.51545
0.73185,0.51993
0.76547,0.51993
0.79489,0.52441
0.80330,0.52441
0.80960,0.52441
0.83272,0.52739
0.84113,0.52739
0.84533,0.52739
0.86634,0.52739
0.86845,0.52739
0.87685,0.53038
0.88105,0.53038
0.89577,0.53038
0.90207,0.53038
0.90207,0.53038
dmp/my_sol/forcemode_example.py 0 → 100644
+ 27
− 0
View file @ 056f87e3
from rtde_control import RTDEControlInterface as RTDEControl
rtde_c = RTDEControl("192.168.1.102")
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
limits = [2, 2, 1.5, 1, 1, 1]
joint_q = [-1.52, -1.411, -1.374, -2.026, 1.375, -0.034]
# Move to initial joint position with a regular moveJ
rtde_c.moveJ(joint_q)
# 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
if i > 10000:
rtde_c.forceMode(task_frame, selection_vector, wrench_up, force_type, limits)
else:
rtde_c.forceMode(task_frame, selection_vector, wrench_down, force_type, limits)
rtde_c.waitPeriod(t_start)
rtde_c.forceModeStop()
rtde_c.stopScript()
dmp/my_sol/new_traj.csv
+ 241
− 643
View file @ 056f87e3
This diff is collapsed.
dmp/my_sol/run_dmp.py
+ 1
− 1
View file @ 056f87e3
......@@ -29,7 +29,7 @@ import matplotlib.pyplot as plt
rtde_control = RTDEControl("192.168.1.102")
rtde_receive = RTDEReceive("192.168.1.102")
rtde_io = RTDEIOInterface("192.168.1.102")
rtde_io.setSpeedSlider(0.7)
rtde_io.setSpeedSlider(0.2)
# run if marker isn't gripped
#gripper = RobotiqGripper()
......
util/calib_board_hacks.py
+ 59
− 20
View file @ 056f87e3
......@@ -37,34 +37,73 @@ model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
init_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(init_pose)
# dangerous
#new_pose[3] = 0
#new_pose[4] = 0
#new_pose[5] = 0
#new_pose[0] += 0.05
#new_pose[1] -= 0.1
#rtde_control.moveL(new_pose)
#exit()
# max offset is (+0.3, -0.3)
# generate 10 and try them out
# but let's try to get it done with only 3 to begin with
# TODO change this, i just shoved the pen through the board
speed = [0, 0, -0.05, 0, 0, 0]
for i in range(3):
speed = [0, 0, -0.5, 0, 0, 0]
n_tests = 5
positions = []
for i in range(n_tests):
rtde_control.moveUntilContact(speed)
print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
init_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(init_pose)
new_pose[2] += 0.1
rtde_control.moveL(new_pose)
new_pose[1] += 0.1
rtde_control.moveL(new_pose)
exit()
while True:
q = rtde_receive.getActualQ()
q.append(0.0)
q.append(0.0)
pin.forwardKinematics(model, data, np.array(q))
print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
print("pin:", *data.oMi[7].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
print("pin:", *data.oMi[8].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)
positions.append(copy.deepcopy(data.oMi[6].translation))
if i < n_tests -1:
current_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(current_pose)
new_pose[2] = init_pose[2]
rtde_control.moveL(new_pose)
new_pose[0] = init_pose[0] + np.random.random() * 0.3
new_pose[1] = init_pose[1] - np.random.random() * 0.2
rtde_control.moveL(new_pose)
current_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(current_pose)
new_pose[2] = init_pose[2]
rtde_control.moveL(new_pose)
rtde_control.moveL(init_pose)
# find the angle
positions = np.array(positions)
print("positions")
print(*positions, sep=',\n')
# the offset does not matter, we only need the angle
# i.e. the normal vector to the plane
n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
print("n", *n, sep=',')
# normalize
#n = n - 1
n = n / np.linalg.norm(n)
print("n normalized:", *n, sep=',')
for p in positions:
print("cdot", p @ n)
z_new = n
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
# it just is
z_new = z_new * -1
# y is good tho
R = np.hstack((x_new.reshape((3,1)), y_new.reshape((3,1))))
R = np.hstack((R, z_new.reshape((3,1))))
print('rot mat to new frame:')
print(*R, sep=',\n')
# question now is in what f-ing frame is this n in
# has to be base frame because that's where the position numbers are from
# so
# rot @ np.eye(3) = n
# but n is one vector
# keep x where it was?, make y complete the frame?
util/jog_example
+ 0
− 0
View file @ 056f87e3
No preview for this file type
util/math_tst.py 0 → 100644
+ 20
− 0
View file @ 056f87e3
import numpy as np
positions = np.array([[0.102 ,0.5755 ,0.5474 ],
[0.102 ,0.5755 ,0.5474 ],
[0.1882, 0.3965, 0.4246],
[0.1882, 0.3965, 0.4246],
[0.2379, 0.4054, 0.4328],
[0.2379, 0.4054, 0.4328],
[0.1641, 0.4749, 0.4823],
[0.1641, 0.4749, 0.4823],
[0.2363, 0.4265, 0.4495],
[0.2363, 0.4265, 0.4495]])
print(np.linalg.matrix_rank(positions))
# of course it's not 2 - both the board isn't flat
# nor are the measurements precise
# (m x 3) @ (3) = as close to 0
sol = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)
util/measuring_stick.py
+ 1
− 0
View file @ 056f87e3
......@@ -39,6 +39,7 @@ while True:
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)
util/open_close_gripper.py
+ 1
− 1
View file @ 056f87e3
......@@ -11,7 +11,7 @@ import matplotlib.pyplot as plt
def handler(signum, frame):
gripper.move(255,100,100)
gripper.move(255,255,255)
time.sleep(3)
exit()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment