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
5f991edc
Commit
5f991edc
authored
Feb 09, 2021
by
Jayant Khatkar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
fast calibration working but unsafe (
#18
)
parent
32a41a49
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
65 additions
and
23 deletions
+65
-23
src/Calibrator.py
src/Calibrator.py
+2
-3
src/Controller.py
src/Controller.py
+4
-3
src/KeyboardController.py
src/KeyboardController.py
+59
-17
No files found.
src/Calibrator.py
View file @
5f991edc
...
...
@@ -45,12 +45,11 @@ class Calibrator(object):
)
# form prompts to the user to satisfy
prompts
=
[
'Move robot tip to {}'
.
format
(
np
.
array2string
(
pos_W
[
iPos
,]))
for
iPos
in
range
(
pos_W
.
shape
[
0
])]
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
)
configurations
=
self
.
controller
.
prompt_configurations
(
prompts
,
prior_calibs
)
pos_RE
=
[[
c
[
1
].
pose
.
position
.
x
,
c
[
1
].
pose
.
position
.
y
,
c
[
1
].
pose
.
position
.
z
]
for
c
in
configurations
]
...
...
src/Controller.py
View file @
5f991edc
...
...
@@ -54,6 +54,7 @@ class Controller(object):
- wrist_2_joint
- wrist_3_joint
"""
def
__init__
(
self
,
close_deg
=
1.0
,
timeScale
=
1.0
,
...
...
@@ -257,7 +258,7 @@ class Controller(object):
return
True
def
move_to
(
self
,
conf_next
,
near_deg
=
1.0
):
def
move_to
(
self
,
conf_next
,
near_deg
=
1.0
):
"""
Moves the arm to the specified configuration by linearly
interpolating the joint angles.
...
...
@@ -278,14 +279,14 @@ class Controller(object):
dist_rad
=
np
.
linalg
.
norm
(
dConf
)
dist_deg
=
np
.
rad2deg
(
dist_rad
)
if
dist_deg
>
near_deg
:
rospy
.
logwarn
(
'Attempted to move a joint distance of {} > {} deg'
.
format
(
d
ist_conf_deg
,
jointLim
_deg
))
rospy
.
logwarn
(
'Attempted to move a joint distance of {} > {} deg'
.
format
(
d
Conf
,
near
_deg
))
return
False
self
.
group
.
go
(
conf_next
)
return
True
def
is_close
(
self
,
trajPoint
):
def
is_close
(
self
,
trajPoint
):
"""
Check if the arm is currently close to the configuration specified.
Ignores timestamp.
...
...
src/KeyboardController.py
View file @
5f991edc
...
...
@@ -17,7 +17,7 @@ import numpy as np
# ROS Libraries
import
rospy
from
geometry_msgs.msg
import
Pose
Stamped
from
geometry_msgs.msg
import
Pose
from
moveit_msgs.srv
import
GetPositionFK
,
GetPositionFKRequest
from
moveit_msgs.srv
import
GetPositionIK
,
GetPositionIKRequest
from
moveit_msgs.msg
import
MoveItErrorCodes
...
...
@@ -26,6 +26,20 @@ from moveit_msgs.msg import MoveItErrorCodes
from
Controller
import
Controller
,
getR_ET
# used for fast-calibration
def
getTransformMat
(
quat
,
pos
):
T
=
np
.
identity
(
4
)
T
[:
3
,:
3
]
=
Rotation
.
from_quat
(
quat
).
as_dcm
()
T
[:
3
,
3
]
=
pos
return
np
.
linalg
.
inv
(
T
)
# also used for fast-calibration-ik
_vert_or
=
np
.
array
([[
-
6.74517010e-04
,
8.33315658e-04
,
-
9.99999404e-01
,
-
2.90597873e-05
],
[
8.33732192e-04
,
-
9.99999285e-01
,
-
8.33877944e-04
,
-
4.81068309e-05
],
[
-
9.99999404e-01
,
-
8.34294187e-04
,
6.73821778e-04
,
2.74792069e-06
],
[
0.00000000e+00
,
0.00000000e+00
,
0.00000000e+00
,
1.00000000e+00
]])
class
KeyboardController
(
Controller
):
"""
Controller to control robot arms by keyboard.
...
...
@@ -163,9 +177,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the robot base.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get current pose
...
...
@@ -200,9 +214,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the end effector.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get current pose
...
...
@@ -224,9 +238,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the tool tip.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get orientation of tool tip relative to end effector
...
...
@@ -525,7 +539,7 @@ class KeyboardController(Controller):
pass
def
prompt_configurations
(
self
,
prompt
=
None
):
def
prompt_configurations
(
self
,
prompt
=
None
,
prior_calibs
=
None
):
"""
Prompts user to control the robot to a series of configurations.
Relative to robot base, the robot can be translated in the:
...
...
@@ -558,18 +572,45 @@ class KeyboardController(Controller):
'step_deg'
:
11.25
,
'success'
:
True
}
if
isinstance
(
prompt
,
list
):
# if previous calib data, create transformation matrix to calc approx IKs
if
prior_calibs
is
not
None
and
len
(
prior_calibs
)
>
0
:
world2armT
=
getTransformMat
(
prior_calibs
[
'quat_WR'
],
prior_calibs
[
'pos_WR'
])
tip2armT
=
getTransformMat
([
0
,
0
,
0
,
1
],
prior_calibs
[
'pos_ET'
])
priors
=
True
else
:
priors
=
False
if
isinstance
(
prompt
,
list
):
for
p
in
prompt
:
print
(
p
)
print
(
'Move robot tip to {}'
.
format
(
str
(
p
)))
if
priors
:
# calculate IK for next point and move there
mat
=
np
.
identity
(
4
)
mat
[:
3
,
3
]
=
p
mat
=
world2armT
.
dot
(
mat
).
dot
(
_vert_or
).
dot
(
tip2armT
)
q
=
Rotation
.
from_dcm
(
mat
[:
3
,:
3
]).
as_quat
()
p_r
=
mat
[
0
:
3
,
3
]
gpose
=
Pose
()
gpose
.
position
.
x
,
gpose
.
position
.
y
,
gpose
.
position
.
z
=
p_r
gpose
.
orientation
.
x
,
gpose
.
orientation
.
y
,
\
gpose
.
orientation
.
z
,
gpose
.
orientation
.
w
=
q
self
.
group
.
set_pose_target
(
gpose
)
self
.
group
.
go
(
wait
=
True
)
with
keyboard
.
Listener
(
on_release
=
lambda
key
:
self
.
_move_callback
(
key
,
configurations
,
params
),
suppress
=
True
)
as
listener
:
listener
.
join
()
if
DEBUG
:
print
(
'Keyboard listener thread joined'
)
if
not
params
[
'success'
]:
sys
.
exit
(
0
)
elif
isinstance
(
prompt
,
int
):
elif
isinstance
(
prompt
,
int
):
for
iConfig
in
range
(
prompt
):
print
(
'Enter configuration {}'
.
format
(
iConfig
))
with
keyboard
.
Listener
(
...
...
@@ -580,6 +621,7 @@ class KeyboardController(Controller):
print
(
'Keyboard listener thread joined'
)
if
not
params
[
'success'
]:
sys
.
exit
(
0
)
else
:
while
params
[
'success'
]:
params
[
'success'
]
=
False
...
...
@@ -594,14 +636,14 @@ class KeyboardController(Controller):
if
__name__
==
"__main__"
:
#
con = KeyboardController(disableExtruder=True)
con
=
KeyboardController
(
disableExtruder
=
True
)
#con = KeyboardController()
#con1 = KeyboardController(robot=1)
con2
=
KeyboardController
(
robot
=
2
,
disableExtruder
=
True
)
#
con2 = KeyboardController(robot=2, disableExtruder=True)
print
(
'Collecting configrations...'
)
#
configuations = con.prompt_configurations()
configuations
=
con
.
prompt_configurations
()
#configuations = con1.prompt_configurations()
configuations
=
con2
.
prompt_configurations
()
#
configuations = con2.prompt_configurations()
print
(
'Your configurations:'
)
print
(
configuations
)
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