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-planner
Commits
d3bb5941
Commit
d3bb5941
authored
Feb 20, 2021
by
Jayant Khatkar
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
updated scheduler works but is slow, need to run wider tests (
#70
)
parent
d21d2e44
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
with
91 additions
and
67 deletions
+91
-67
src/decmcts.py
src/decmcts.py
+88
-49
src/tests/test_decmcts.py
src/tests/test_decmcts.py
+3
-18
No files found.
src/decmcts.py
View file @
d3bb5941
...
...
@@ -72,6 +72,7 @@ class CombinedSchedule:
self
.
end_times
=
[[
0
]
for
i
in
range
(
self
.
n
)]
self
.
t_mul
=
joint2time_mul
self
.
safe_time
=
SAFE_TIME
# safety buffer for Collisions
self
.
start_next_contour
=
[
0
]
*
self
.
n
def
planLenOrder
(
self
):
...
...
@@ -98,7 +99,7 @@ class CombinedSchedule:
return
max
([
self
.
end_times
[
r
][
-
1
]
for
r
in
range
(
self
.
n
)])
def
append
(
self
,
ct
,
robot
):
def
append
(
self
,
ct
,
robot
,
force
=
False
):
"""
add a contour trajectory to the CombinedSchedule
"""
...
...
@@ -108,11 +109,11 @@ class CombinedSchedule:
self
.
trajs
[
robot
].
append
(
ct
)
self
.
start_times
[
robot
].
append
(
self
.
end_times
[
robot
][
-
1
])
self
.
end_times
[
robot
].
append
(
self
.
start_times
[
robot
][
-
1
]
+
ct
.
time
[
-
1
])
return
0
return
True
# travel time based on joint_diff
trav_time
=
self
.
t_mul
*
joint_diff
(
ct
.
positions
[
-
1
],
self
.
lastJointPos
(
robot
))
t_0
=
self
.
end_times
[
robot
]
[
-
1
]
+
trav_time
t_0
=
self
.
start_next_contour
[
robot
]
+
trav_time
# for every other robot, get collision free time offsets
allowed_offsets
=
[]
...
...
@@ -147,11 +148,22 @@ class CombinedSchedule:
# easy for n = 2
if
self
.
n
==
2
:
t_wait
=
max
(
trav_time
,
-
min
(
0
,
allowed_offsets
[
0
][
-
1
][
1
]))
t_wait
=
max
(
trav_time
,
-
min
(
0
,
allowed_offsets
[
0
][
-
1
][
1
]))
#if this contour starts after other arm is finished
# and it isnt the first contour of the plan
# and it isn't being added regardless
if
t_0
+
t_wait
>
self
.
end_times
[
1
-
robot
][
-
1
]
\
and
not
(
self
.
end_times
[
robot
][
-
1
]
==
0
and
self
.
end_times
[
1
-
robot
][
-
1
]
==
0
)
\
and
not
force
:
# reject contour if it cannot be parallelised with current plan
self
.
start_next_contour
[
robot
]
=
self
.
end_times
[
1
-
robot
][
-
1
]
+
self
.
safe_time
return
False
self
.
trajs
[
robot
].
append
(
ct
)
self
.
start_times
[
robot
].
append
(
self
.
end_times
[
robot
][
-
1
]
+
t_wait
)
self
.
start_times
[
robot
].
append
(
t_0
+
t_wait
)
self
.
end_times
[
robot
].
append
(
self
.
start_times
[
robot
][
-
1
]
+
ct
.
time
[
-
1
])
return
t_wait
self
.
start_next_contour
[
robot
]
=
self
.
end_times
[
robot
][
-
1
]
return
True
# for n > 2
t_wait_last
=
-
1
...
...
@@ -211,27 +223,54 @@ def createCombinedSchedule(data, states):
plan
=
CombinedSchedule
(
data
,
len
(
states
))
cts_left
=
[
copy
.
copy
(
s
)
for
s
in
states
.
values
()]
if
len
(
cts_left
)
>
2
:
raise
Exception
(
"New CombinedScheduler does not support >2 arms"
)
done_contours
=
[]
t_used
=
0
robot
=
0
force
=
False
assert
(
len
(
cts_left
[
0
])
>
0
)
while
sum
([
len
(
r
)
for
r
in
cts_left
])
>
0
:
rs
=
plan
.
planLenOrder
()
for
r
in
rs
:
if
len
(
cts_left
[
r
])
!=
0
:
robot
=
r
break
# get next cts whose contour hasn't been done
while
len
(
cts_left
[
robot
])
>
0
:
ct
=
cts_left
[
robot
].
popleft
()
if
ct
.
contour
not
in
done_contours
:
# place contour in plan and sum wasted time
done_contours
.
append
(
ct
.
contour
)
plan
.
append
(
ct
,
robot
)
t_used
+=
ct
.
time
[
-
1
]
break
i
=
0
while
i
<
len
(
cts_left
[
robot
]):
ct
=
cts_left
[
robot
][
i
]
if
ct
.
contour
in
done_contours
:
del
cts_left
[
robot
][
i
]
if
len
(
cts_left
[
robot
])
==
0
:
robot
=
1
-
robot
break
continue
else
:
# try adding
if
plan
.
append
(
ct
,
robot
,
force
=
force
):
# log done contours and time
done_contours
.
append
(
ct
.
contour
)
t_used
+=
ct
.
time
[
-
1
]
force
=
False
# get next arm to add to
rs
=
plan
.
planLenOrder
()
for
r
in
rs
:
if
len
(
cts_left
[
r
])
!=
0
:
robot
=
r
break
break
# if tried everything, switch robots
elif
i
==
len
(
cts_left
[
robot
])
-
1
:
# TODO work with more than two robots
robot
=
1
-
robot
force
=
True
break
else
:
# try next contour in list
i
+=
1
return
plan
,
t_used
...
...
@@ -241,7 +280,7 @@ def reward(data, states):
"""
plan
,
t_used
=
createCombinedSchedule
(
data
,
states
)
# return t_used/(plan.len()*len(states)), plan # FOR TESTING
return
t_used
/
(
plan
.
len
()
*
len
(
states
))
return
t_used
/
plan
.
len
()
def
decmcts_layer
(
...
...
@@ -332,7 +371,7 @@ def decmcts_layer(
# grow the trees
for
tree
in
trees
:
tree
.
grow
(
nsims
=
1
,
depth
=
1e6
)
if
i
%
1
00
==
0
and
i
!=
0
:
if
i
%
1
==
0
and
i
!=
0
:
e
=
time
.
time
()
print
(
"Completed {} nodes in {:.2g}s"
.
format
(
i
,
e
-
s
))
...
...
@@ -382,31 +421,31 @@ def decmcts_layer(
joint_diff
(
home
,
traj
.
positions
[
0
])
# if there's enough time to get home in the wait period, do that
athome
=
list
(
plan
.
getLastJoint
(
arm
))
==
home
additional_time_spent
=
0
if
time_last2home
+
time_home2next
<=
decmcts_wait_time
:
# try going to home pos
while
not
athome
:
gohome
=
plan
.
plan_to
(
env
.
home
,
arm
)
# if no travel found, stay at last pos
if
gohome
is
not
None
and
plan
.
appendTrajectory
([
gohome
],
arm
)
==
0
:
print
(
arm
,
"returning to home to wait"
)
athome
=
True
else
:
print
(
"There's enough time to get home but couldn't find a path - Unfortunate"
)
print
(
"Staying put for {}s"
.
format
(
default_wait
))
last_js
=
plan
.
trajs
[
arm
][
-
1
].
positions
[
-
1
]
stayput
=
tu
.
JTrajectory
([
last_js
,
last_js
],[
0
,
default_wait
],
arm
)
if
plan
.
appendTrajectory
([
stayput
],
arm
)
!=
0
:
print
(
"cannot go home or stay put Logically possible but unlikely - TODO"
)
raise
Exception
(
"cannot go home or stay put Logically possible but unlikely - TODO"
)
additional_time_spent
+=
default_wait
if
time_last2home
+
additional_time_spent
+
default_wait
+
time_home2next
>
decmcts_wait_time
:
# can't go home anymore
break
#
athome = list(plan.getLastJoint(arm)) == home
#
additional_time_spent = 0
#
if time_last2home + time_home2next <= decmcts_wait_time:
#
# try going to home pos
#
while not athome:
#
gohome = plan.plan_to(env.home, arm)
#
# if no travel found, stay at last pos
#
if gohome is not None and plan.appendTrajectory([gohome], arm)==0:
#
print(arm, "returning to home to wait")
#
athome = True
#
else:
#
print("There's enough time to get home but couldn't find a path - Unfortunate")
#
print("Staying put for {}s".format(default_wait))
#
last_js = plan.trajs[arm][-1].positions[-1]
#
stayput = tu.JTrajectory([last_js, last_js],[0,default_wait], arm)
#
if plan.appendTrajectory([stayput], arm)!=0:
#
print("cannot go home or stay put Logically possible but unlikely - TODO")
#
raise Exception("cannot go home or stay put Logically possible but unlikely - TODO")
#
additional_time_spent += default_wait
#
if time_last2home + additional_time_spent + default_wait + time_home2next > decmcts_wait_time:
#
# can't go home anymore
#
break
# add to plan
traj_added
=
False
...
...
src/tests/test_decmcts.py
View file @
d3bb5941
...
...
@@ -43,32 +43,17 @@ def test_reward(contour_tracker, combined_schedule, real_plan, arm0=0, arm1=1):
if
__name__
==
"__main__"
:
# Read gcode file
model
=
"
flexirex_big
"
model
=
"
Kitchen_sponge_holder_small-ascii
"
tracker
=
tu
.
get_tracker
(
model
)
# 3arms simulation
env_desc3
=
tu
.
PlanningEnv
([[
0
,
0
,
1
,
0
],[
0
,
0
,
1
,
0
],[
0
,
0
,
0
,
1
]],
[[
0.9
,
-
0.35
,
0
],[
-
0.8
,
-
0.35
,
0.001
],[
0
,
0.5
,
0
]],
[[
0
,
0
,
0
,
1
]]
*
3
,
[[
0.15
,
0
,
0
]]
*
3
)
# WARNING - also edit urdf
# 2arms real calibration
env_desc2
=
tu
.
read_env
(
'calibrations/r1_tforms.yaml'
,
'calibrations/r2_tforms.yaml'
)
env
=
tu
.
SimEnv
(
env_desc
=
env_desc
3
,
gui
=
Fals
e
,
home
=
home
)
env
=
tu
.
SimEnv
(
env_desc
=
env_desc
2
,
gui
=
Tru
e
,
home
=
home
)
# decmcts plan first n contours
c_traj_file
=
"gcode/"
+
model
+
".c_traj"
cs
,
plan
=
decmcts
(
tracker
,
env
,
n_layers
=
1
,
mcts_nodes
=
1000
)
cs
,
plan
=
decmcts
(
env
,
tracker
,
nth_layer
=
50
,
nodes_per_layer
=
1000
)
# run test test_reward
print
(
"Compare arms 0 and 1"
)
test_reward
(
tracker
,
cs
,
plan
,
arm0
=
0
,
arm1
=
1
)
print
(
"Compare arms 1 and 2"
)
test_reward
(
tracker
,
cs
,
plan
,
arm0
=
1
,
arm1
=
2
)
print
(
"Compare arms 0 and 2"
)
test_reward
(
tracker
,
cs
,
plan
,
arm0
=
0
,
arm1
=
2
)
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