Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
What's new
10
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
bigprint
twins-controller
Commits
f9cb4bf9
Commit
f9cb4bf9
authored
Feb 10, 2021
by
Jayant Khatkar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
r1 calibration done, living saving of calibration in case it crashes (
#19
)
parent
b60fcca2
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
48 additions
and
16 deletions
+48
-16
calibrations/r1_tforms_alt.yaml
calibrations/r1_tforms_alt.yaml
+37
-3
src/Calibrator.py
src/Calibrator.py
+11
-13
No files found.
calibrations/r1_tforms_alt.yaml
View file @
f9cb4bf9
calibration
:
pos_ET
:
[
0.1
4982225501989826
,
-0.048118796065005576
,
-0.03482924194973972
]
pos_WR
:
[
0.03
3410320051684865
,
-0.5779185680332759
,
-0.129
]
pos_ET
:
[
0.1
5007592984204163
,
-0.04830698520623148
,
-0.031668261140098686
]
pos_WR
:
[
0.03
655708997718542
,
-0.575405839173846
,
-0.13224784915908203
]
quat_ET
:
[
-0.2705980500730985
,
-0.6532814824381882
,
0.27059805007309845
,
0.6532814824381883
]
quat_WR
:
[
-0.0035406624036226648
,
0.00401943913741378
,
-0.9999807593358486
,
0.003128702105328147
]
quat_WR
:
[
-0.004907671722772035
,
0.0026006325187357495
,
-0.9999828094804524
,
0.0018794180869107977
]
measurements
:
errors
:
-
[
-0.00035093605232278113
,
0.0015154904646795808
,
0.0005004698912217387
]
-
[
0.0008203797060748544
,
0.0008922491648686859
,
0.0002722545866968018
]
-
[
0.001289894680197884
,
0.0009346809299097258
,
-0.0011471590539748888
]
-
[
-0.00026782915015455266
,
-0.00019411428454588426
,
-0.00017438502446384674
]
-
[
-0.000711953482581712
,
-0.00017432903503347696
,
0.0007754958989695804
]
-
[
-0.0009272510092923555
,
-0.001473229551918892
,
0.0009148858434974705
]
-
[
-0.0007779454099607619
,
-0.0013991536560543083
,
-9.067649884858531e-05
]
-
[
-0.0006907081723367681
,
-0.00013629415861853315
,
0.0005679499481896777
]
-
[
0.0014934971392888685
,
-0.000434524141417425
,
-0.001318136805991993
]
-
[
0.00012285175106990787
,
0.0004692242681248698
,
-0.0003006987853121923
]
pos_RE
:
-
[
0.28238526739189035
,
-0.743445281602937
,
0.2753943928759848
]
-
[
0.06856808024088912
,
-0.7433152737798132
,
0.27773399071687555
]
-
[
-0.14592886427691276
,
-0.7424717736062426
,
0.28125724809030916
]
-
[
-0.14669659051873055
,
-0.528554844189385
,
0.2813981571448582
]
-
[
-0.1463260383308909
,
-0.3135266064730779
,
0.28156287698542204
]
-
[
0.06844080035242153
,
-0.3156302723662834
,
0.27930760468097077
]
-
[
0.283587823473315
,
-0.31634541818249157
,
0.2781937485538753
]
-
[
0.28285396776950816
,
-0.530129589269635
,
0.2764449159253464
]
-
[
0.5066506093478649
,
-0.20096898804832836
,
0.16677352259446088
]
-
[
-0.369184926735048
,
-0.5275417892542257
,
0.2618225946251175
]
quat_RE
:
-
[
0.004515506751761712
,
0.7093312647790816
,
0.0005263393718474751
,
0.7048606174078313
]
-
[
0.004551772876819091
,
0.7093577385565347
,
0.000732948249208785
,
0.7048335568774383
]
-
[
0.004513807736845369
,
0.7093838031536429
,
0.0007461847780781305
,
0.7048075542807879
]
-
[
0.00442392266478542
,
0.709359777178923
,
0.0006503084317854549
,
0.7048324003107893
]
-
[
0.004471491373419877
,
0.7093586207919866
,
0.0005390274408393596
,
0.7048333578388627
]
-
[
0.004466760991158438
,
0.7093646956760709
,
0.0005683729336144619
,
0.7048272508401273
]
-
[
0.004391686236732675
,
0.7093756214129284
,
0.0005693173620683987
,
0.704816725620754
]
-
[
0.00448742433570489
,
0.7093749968660662
,
0.000748052655762252
,
0.7048165841276184
]
-
[
-0.00045970685855608695
,
0.6540762178342059
,
-0.653219981430286
,
0.38142855922639246
]
-
[
0.003642363656463826
,
0.38561399879651764
,
0.002617478574255283
,
0.922649297363341
]
targets
:
pos_W
:
-
[
-0.215
,
0.215
,
0
]
...
...
src/Calibrator.py
View file @
f9cb4bf9
...
...
@@ -28,12 +28,12 @@ class Calibrator(object):
self
.
controller
=
None
def
measureConfigurations
(
self
,
p
os_W
,
prior_calibs
):
def
measureConfigurations
(
self
,
p
rompts
,
prior_calibs
):
"""
Calibrate the arm for the robot's pose relative to the world
and the position of the tool tip relative to the end effector.
p
os_W: Nx3 sequence
of positions to visit for calibration
p
rompts: list
of positions to visit for calibration
"""
if
self
.
controller
is
None
:
if
DEBUG
:
...
...
@@ -44,9 +44,6 @@ class Calibrator(object):
disableExtruder
=
True
)
# form prompts to the user to satisfy
prompts
=
[
pos_W
[
iPos
,]
for
iPos
in
range
(
pos_W
.
shape
[
0
])]
# gather configurations
# [ ( [joint angles] , ROS pose message ) , ... ]
configurations
=
self
.
controller
.
prompt_configurations
(
prompts
,
prior_calibs
)
...
...
@@ -301,14 +298,15 @@ def main(calibFile_in, calibFile_out, calibData):
# If the number of measurements does not match the number of targets
if
len
(
pos_RE
)
!=
nTarget
or
len
(
quat_RE
)
!=
nTarget
:
# Take measurements of the targets
(
pos_RE_Mx3
,
quat_RE_Mx4
)
=
cal
.
measureConfigurations
(
pos_W_Mx3
,
calibration
)
pos_RE
.
extend
(
pos_RE_Mx3
.
tolist
())
quat_RE
.
extend
(
quat_RE_Mx4
.
tolist
())
# And save them to file
with
open
(
calibFile_out
,
'w'
)
as
file
:
yaml
.
dump
(
calibData
,
file
,
default_flow_style
=
None
)
for
i
in
range
(
len
(
pos_RE
),
nTarget
):
(
pos_RE_Mx3
,
quat_RE_Mx4
)
=
cal
.
measureConfigurations
([
pos_W_Mx3
[
i
,]],
calibration
)
pos_RE
.
extend
(
pos_RE_Mx3
.
tolist
())
quat_RE
.
extend
(
quat_RE_Mx4
.
tolist
())
# And save them to file
with
open
(
calibFile_out
,
'w'
)
as
file
:
yaml
.
dump
(
calibData
,
file
,
default_flow_style
=
None
)
else
:
pos_RE_Mx3
=
np
.
array
(
pos_RE
)
quat_RE_Mx4
=
np
.
array
(
quat_RE
)
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment