Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
bigprint
twins-controller
Commits
417cad0c
Commit
417cad0c
authored
Feb 09, 2021
by
Jayant Khatkar
Browse files
use cartesian plaln and display and confirm (
#18
)
parent
5f991edc
Changes
3
Hide whitespace changes
Inline
Side-by-side
calibrations/testfile.yaml
0 → 100644
View file @
417cad0c
calibration
:
pos_ET
:
[
0.14982225501989826
,
-0.048118796065005576
,
-0.03482924194973972
]
pos_WR
:
[
0.033410320051684865
,
-0.5779185680332759
,
-0.129
]
quat_ET
:
[
-0.2705980500730985
,
-0.6532814824381882
,
0.27059805007309845
,
0.6532814824381883
]
quat_WR
:
[
-0.0035406624036226648
,
0.00401943913741378
,
-0.9999807593358486
,
0.003128702105328147
]
targets
:
pos_W
:
-
[
0
,
0.215
,
0
]
-
[
0.215
,
-0.215
,
0
]
-
[
-0.215
,
-0.215
,
0
]
-
[
0.322
,
0
,
0
]
-
[
-0.322
,
-0.322
,
0
]
src/Controller.py
View file @
417cad0c
...
...
@@ -25,6 +25,7 @@ from control_msgs.msg import FollowJointTrajectoryActionResult
# MOVEit
import
moveit_commander
import
moveit_msgs
from
moveit_msgs.msg._RobotTrajectory
import
RobotTrajectory
## Our libraries
...
...
@@ -88,6 +89,10 @@ class Controller(object):
self
.
robot
=
moveit_commander
.
RobotCommander
()
self
.
scene
=
moveit_commander
.
PlanningSceneInterface
()
self
.
group
=
moveit_commander
.
MoveGroupCommander
(
'manipulator'
)
self
.
disp_traj_pub
=
\
rospy
.
Publisher
(
'/move_group/display_planned_path'
,
moveit_msgs
.
msg
.
DisplayTrajectory
,
queue_size
=
20
)
# Traj completion feedback
self
.
motion_feedback_sub
=
rospy
.
Subscriber
(
...
...
src/KeyboardController.py
View file @
417cad0c
...
...
@@ -4,6 +4,7 @@ VERBOSE = True
## Python libraries
from
copy
import
deepcopy
import
time
import
sys
## 3rd party libraries
...
...
@@ -21,6 +22,7 @@ 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
from
moveit_msgs.msg
import
DisplayTrajectory
## Our libraries
from
Controller
import
Controller
,
getR_ET
...
...
@@ -597,8 +599,31 @@ class KeyboardController(Controller):
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
)
# compute path to next config
waypoints
=
[
self
.
group
.
get_current_pose
().
pose
,
gpose
]
plan
,
fraction
=
self
.
group
.
compute_cartesian_path
(
waypoints
,
0.01
,
# ee_step
0
)
# display path on screen and confirm execution
try
:
print
(
"Displaying Trajectory to estimated point."
)
disp_traj
=
DisplayTrajectory
()
disp_traj
.
trajectory_start
=
self
.
robot
.
get_current_state
()
disp_traj
.
trajectory
.
append
(
plan
)
self
.
disp_traj_pub
.
publish
(
disp_traj
)
a
=
raw_input
(
"Execute displayed Trajectory (y/n) ? "
)
if
a
==
'y'
:
self
.
group
.
execute
(
plan
,
wait
=
True
)
else
:
print
(
"OK, move manually to next pos."
)
except
:
Exception
(
"Only calibrate one arm at a time"
)
time
.
sleep
(
1
)
with
keyboard
.
Listener
(
on_release
=
lambda
key
:
self
.
_move_callback
(
key
,
configurations
,
params
),
...
...
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