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
56dea719
Commit
56dea719
authored
Jan 20, 2021
by
Jayant Khatkar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
fix trajectory failed error (fix
#17
)
parent
e6261aba
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
32 additions
and
21 deletions
+32
-21
src/Controller.py
src/Controller.py
+15
-7
src/KeyboardController.py
src/KeyboardController.py
+2
-2
src/main.py
src/main.py
+15
-12
No files found.
src/Controller.py
View file @
56dea719
...
...
@@ -141,22 +141,31 @@ class Controller(object):
"""
plan
=
RobotTrajectory
()
plan
.
joint_trajectory
=
JTraj2ROS
(
jtraj
)
# plan.joint_trajectory.points = plan.joint_trajectory.points[1:]
# NOTE: This can only be done safely becuase assuming that the current
# joint position is already very close to where we want it to be
cj
=
self
.
group
.
get_current_joint_values
()
sj
=
plan
.
joint_trajectory
.
points
[
0
].
positions
if
any
([
0.01
<
abs
(
cj
[
i
]
-
sj
[
i
])
for
i
in
range
(
6
)]):
print
(
"Potentially would've failed here, Retrying"
)
#time.sleep(0.1)
self
.
group
.
go
(
plan
.
joint_trajectory
.
points
[
0
].
positions
,
wait
=
True
)
return
False
if
contour
is
not
None
:
print
(
"THIS NEXT TRAJ REQUIRES EXTRUSION"
)
self
.
preprint_load
()
self
.
extrude
(
contour
.
Ext
[
-
1
],
jtraj
.
time
[
-
1
])
rospy
.
loginfo
(
'Sending trajectory to UR5e'
)
#
rospy.loginfo('Sending trajectory to UR5e')
self
.
traj_complete
=
False
self
.
group
.
execute
(
plan
,
wait
=
False
)
if
wait
:
s
=
time
.
time
()
t
=
jtraj
.
time
[
-
1
]
while
not
self
.
traj_complete
:
if
time
.
time
()
-
s
>
t
+
3
:
while
not
self
.
traj_complete
or
time
.
time
()
-
s
<
t
-
1
:
if
time
.
time
()
-
s
>
t
+
1
:
Exception
(
"traj_complete failed"
)
break
self
.
traj_complete
=
False
...
...
@@ -323,12 +332,11 @@ class Controller(object):
if
len
(
msg
.
status_list
)
>
0
:
self
.
traj_complete
=
(
msg
.
status_list
[
-
1
].
status
==
3
or
msg
.
status_list
[
-
1
].
status
==
2
)
if
msg
.
status_list
[
-
1
].
status
>=
4
:
print
(
"
Trajectory has failed to
exec
u
te
, please check ROS MoveIt execution planner to see why
"
)
if
msg
.
status_list
[
-
1
].
status
>=
4
and
msg
.
status_list
[
-
1
].
status
!=
6
:
print
(
"
Un
ex
p
ecte
d Trajectory Status
"
)
print
(
"STATUS: {}"
.
format
(
msg
.
status_list
[
-
1
].
status
))
print
(
"For info on status meaning, go to:
\n
"
+
\
"http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html"
)
sys
.
exit
(
0
)
def
getR_ET
():
...
...
src/KeyboardController.py
View file @
56dea719
...
...
@@ -592,8 +592,8 @@ class KeyboardController(Controller):
if
__name__
==
"__main__"
:
#
con = KeyboardController(disableExtruder=True)
con
=
KeyboardController
()
con
=
KeyboardController
(
disableExtruder
=
True
)
#
con = KeyboardController()
print
(
'Collecting configrations...'
)
configuations
=
con
.
prompt_configurations
()
...
...
src/main.py
View file @
56dea719
...
...
@@ -25,7 +25,7 @@ def execute_plan(plan,
skip_first
=
1
,
contour_speed
=
0.2
,
move_speed
=
1
,
first_n
=
None
):
n_trajs
=
None
):
"""
Executes a plan on MoveIt
...
...
@@ -37,9 +37,10 @@ def execute_plan(plan,
# execute trajectories in plan
for
i
,
t
in
enumerate
(
plan
.
trajs
[
0
]):
print
(
"---"
)
if
i
==
first
_n
:
print
(
"Finishing prematurely, completed {} trajectories"
.
format
(
i
))
if
i
==
n_trajs
+
skip_
first
:
print
(
"Finishing prematurely, completed {} trajectories"
.
format
(
i
-
skip_first
))
break
if
i
<
skip_first
:
...
...
@@ -51,13 +52,15 @@ def execute_plan(plan,
if
a
==
'n'
:
break
print
(
"Printing {}th trajectory in the plan, which is contour {}"
.
format
(
i
,
t
.
contour
))
if
t
.
contour
is
not
None
and
contours
is
not
None
:
print
(
"Printing {}th trajectory in the plan, which is contour {}"
.
format
(
i
,
t
.
contour
))
controller
.
exec_ctraj
(
speed_multiplier
(
t
,
1
/
contour_speed
),
contour
=
contours
[
t
.
contour
])
while
not
controller
.
exec_ctraj
(
speed_multiplier
(
t
,
1
/
contour_speed
),
contour
=
contours
[
t
.
contour
]):
pass
else
:
print
(
"
Printing {}th trajectory in the plan, which is contour {}"
.
format
(
i
,
t
.
contour
)
)
print
(
"Moving arm without extrusion"
)
controller
.
exec_ctraj
(
speed_multiplier
(
t
,
1
/
move_speed
))
#
print("
Moving arm without extrusion"
)
while
not
controller
.
exec_ctraj
(
speed_multiplier
(
t
,
1
/
move_speed
)):
pass
return
...
...
@@ -67,12 +70,12 @@ if __name__ == '__main__':
plan2
,
flexirex_contours
=
load_plan
(
'flexirex'
,
contours
=
True
)
# Printing
#
con = Controller()
#
execute_plan(plan2, con, contours=flexirex_contours, confirm=
True
)
con
=
Controller
()
execute_plan
(
plan2
,
con
,
contours
=
flexirex_contours
,
confirm
=
False
,
n_trajs
=
50
,
skip_first
=
200
)
# no printing
con
=
Controller
(
disable_extruder
=
True
)
execute_plan
(
plan2
,
con
,
confirm
=
False
,
move_speed
=
5
)
#
con = Controller(disable_extruder=True)
#
execute_plan(plan2, con, confirm=False,
n_trajs=200,
move_speed=
1
)
# go to origin and stay (to check calibration)
#plan05 = copy.deepcopy(plan1)
...
...
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