Skip to content
GitLab
Menu
Projects
Groups
Snippets
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
bigprint
twins-planner
Commits
d3bb5941
Commit
d3bb5941
authored
Feb 20, 2021
by
Jayant Khatkar
Browse files
updated scheduler works but is slow, need to run wider tests (
#70
)
parent
d21d2e44
Changes
2
Hide whitespace changes
Inline
Side-by-side
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