Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
D
db-guided-mrmp
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
rmoan2
db-guided-mrmp
Commits
4ce16de4
Commit
4ce16de4
authored
3 weeks ago
by
Adam Sitabkhan
Browse files
Options
Downloads
Patches
Plain Diff
Initial diversified RRT*
parent
5e4af514
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
guided_mrmp/utils/rrt_diversified.py
+281
-0
281 additions, 0 deletions
guided_mrmp/utils/rrt_diversified.py
with
281 additions
and
0 deletions
guided_mrmp/utils/rrt_diversified.py
0 → 100644
+
281
−
0
View file @
4ce16de4
"""
Implements Klampt
'
s various motion planners
"""
from
OpenGL.GL
import
*
from
OpenGL.GLU
import
*
from
OpenGL.GLUT
import
*
import
math
import
random
from
klampt.plan.cspace
import
CSpace
,
MotionPlan
from
klampt.vis.glprogram
import
GLProgram
from
klampt.math
import
vectorops
import
numpy
as
np
class
Circle
:
def
__init__
(
self
,
x
=
0
,
y
=
0
,
radius
=
1
):
self
.
center
=
(
x
,
y
)
self
.
radius
=
radius
def
contains
(
self
,
point
):
return
(
vectorops
.
distance
(
point
,
self
.
center
)
<=
self
.
radius
)
def
drawGL
(
self
,
res
=
0.01
):
numdivs
=
int
(
math
.
ceil
(
self
.
radius
*
math
.
pi
*
2
/
res
))
glBegin
(
GL_TRIANGLE_FAN
)
glVertex2f
(
*
self
.
center
)
for
i
in
range
(
numdivs
+
1
):
u
=
float
(
i
)
/
float
(
numdivs
)
*
math
.
pi
*
2
glVertex2f
(
self
.
center
[
0
]
+
self
.
radius
*
math
.
cos
(
u
),
self
.
center
[
1
]
+
self
.
radius
*
math
.
sin
(
u
))
glEnd
()
class
Rectangle
:
def
__init__
(
self
,
x
=
0
,
y
=
0
,
width
=
1
,
height
=
1
):
self
.
bottom_left
=
(
x
,
y
)
self
.
width
=
width
self
.
height
=
height
def
contains
(
self
,
point
):
x
,
y
=
point
x0
,
y0
=
self
.
bottom_left
return
(
x
>=
x0
and
x
<=
x0
+
self
.
width
and
y
>=
y0
and
y
<=
y0
+
self
.
height
)
def
drawGL
(
self
):
x
,
y
=
self
.
bottom_left
w
=
self
.
width
h
=
self
.
height
glBegin
(
GL_QUADS
)
glVertex2f
(
x
,
y
)
glVertex2f
(
x
+
w
,
y
)
glVertex2f
(
x
+
w
,
y
+
h
)
glVertex2f
(
x
,
y
+
h
)
glEnd
()
class
CircleRectangleObstacleCSpace
(
CSpace
):
def
__init__
(
self
,
x_bounds
=
(
0.0
,
1.0
),
y_bounds
=
(
0.0
,
1.0
),
robot_radius
=
0.05
):
CSpace
.
__init__
(
self
)
#set bounds
# self.bound = [(0.0,1.0),(0.0,1.0)]
self
.
bound
=
[(
x_bounds
[
0
],
x_bounds
[
1
]),
(
y_bounds
[
0
],
y_bounds
[
1
])]
#set collision checking resolution
self
.
eps
=
1e-3
#setup a robot with radius 0.05
self
.
robot
=
Circle
(
0
,
0
,
robot_radius
)
#set obstacles here
self
.
obstacles
=
[]
#store paths here
self
.
paths
=
[]
def
addObstacle
(
self
,
obs
):
self
.
obstacles
.
append
(
obs
)
def
feasible
(
self
,
q
):
#bounds test
if
not
CSpace
.
feasible
(
self
,
q
):
return
False
#TODO: Problem 1: implement your feasibility tests here
#currently, only the center is checked, so the robot collides
#with boundary and obstacles
for
o
in
self
.
obstacles
:
if
o
.
contains
(
q
):
return
False
return
True
def
drawObstaclesGL
(
self
):
glColor3f
(
0.2
,
0.2
,
0.2
)
for
o
in
self
.
obstacles
:
o
.
drawGL
()
def
drawRobotGL
(
self
,
q
):
glColor3f
(
0
,
0
,
1
)
newc
=
vectorops
.
add
(
self
.
robot
.
center
,
q
)
c
=
Circle
(
newc
[
0
],
newc
[
1
],
self
.
robot
.
radius
)
c
.
drawGL
()
class
CSpaceObstacleProgram
(
GLProgram
):
def
__init__
(
self
,
space
,
start
=
(
0.1
,
0.5
),
goal
=
(
0.9
,
0.5
),
planner_name
=
"
rrt*
"
):
GLProgram
.
__init__
(
self
)
self
.
space
=
space
#PRM planner
# MotionPlan.setOptions(type="prm",knn=10,connectionThreshold=0.1)
self
.
optimizingPlanner
=
False
#FMM* planner
#MotionPlan.setOptions(type="fmm*")
#self.optimizingPlanner = True
#RRT planner
MotionPlan
.
setOptions
(
type
=
planner_name
,
perturbationRadius
=
0.25
,
bidirectional
=
True
)
self
.
optimizingPlanner
=
False
#RRT* planner
#MotionPlan.setOptions(type="rrt*")
#self.optimizingPlanner = True
#random-restart RRT planner
#MotionPlan.setOptions(type="rrt",perturbationRadius=0.25,bidirectional=True,shortcut=True,restart=True,restartTermCond="{foundSolution:1,maxIters:1000}")
#self.optimizingPlanner = True
#OMPL planners:
#Tested to work fine with OMPL's prm, lazyprm, prm*, lazyprm*, rrt, rrt*, rrtconnect, lazyrrt, lbtrrt, sbl, bitstar.
#Note that lbtrrt doesn't seem to continue after first iteration.
#Note that stride, pdst, and fmt do not work properly...
#MotionPlan.setOptions(type="ompl:rrt",suboptimalityFactor=0.1,knn=10,connectionThreshold=0.1)
#self.optimizingPlanner = True
self
.
planner
=
MotionPlan
(
space
)
self
.
start
=
start
self
.
goal
=
goal
self
.
planner
.
setEndpoints
(
start
,
goal
)
self
.
path
=
[]
self
.
G
=
None
def
keyboardfunc
(
self
,
key
,
x
,
y
):
if
key
==
'
'
:
if
self
.
optimizingPlanner
or
not
self
.
path
:
print
(
"
Planning 1...
"
)
self
.
planner
.
planMore
(
1
)
self
.
path
=
self
.
planner
.
getPath
()
self
.
G
=
self
.
planner
.
getRoadmap
()
self
.
refresh
()
elif
key
==
'
p
'
:
if
self
.
optimizingPlanner
or
not
self
.
path
:
print
(
"
Planning 100...
"
)
self
.
planner
.
planMore
(
100
)
self
.
path
=
self
.
planner
.
getPath
()
self
.
G
=
self
.
planner
.
getRoadmap
()
self
.
refresh
()
def
display
(
self
):
glMatrixMode
(
GL_PROJECTION
)
glLoadIdentity
()
glOrtho
(
0
,
1
,
1
,
0
,
-
1
,
1
);
glMatrixMode
(
GL_MODELVIEW
)
glLoadIdentity
()
glDisable
(
GL_LIGHTING
)
self
.
space
.
drawObstaclesGL
()
if
self
.
path
:
#draw path
glColor3f
(
0
,
1
,
0
)
glBegin
(
GL_LINE_STRIP
)
for
q
in
self
.
path
:
glVertex2f
(
q
[
0
],
q
[
1
])
glEnd
()
for
q
in
self
.
path
:
self
.
space
.
drawRobotGL
(
q
)
else
:
self
.
space
.
drawRobotGL
(
self
.
start
)
self
.
space
.
drawRobotGL
(
self
.
goal
)
if
self
.
G
:
#draw graph
V
,
E
=
self
.
G
glEnable
(
GL_BLEND
)
glBlendFunc
(
GL_SRC_ALPHA
,
GL_ONE_MINUS_SRC_ALPHA
)
glColor4f
(
0
,
0
,
0
,
0.5
)
glPointSize
(
3.0
)
glBegin
(
GL_POINTS
)
for
v
in
V
:
glVertex2f
(
v
[
0
],
v
[
1
])
glEnd
()
glColor4f
(
0.5
,
0.5
,
0.5
,
0.5
)
glBegin
(
GL_LINES
)
for
(
i
,
j
)
in
E
:
glVertex2f
(
V
[
i
][
0
],
V
[
i
][
1
])
glVertex2f
(
V
[
j
][
0
],
V
[
j
][
1
])
glEnd
()
glDisable
(
GL_BLEND
)
if
__name__
==
'
__main__
'
:
import
argparse
parser
=
argparse
.
ArgumentParser
()
parser
.
add_argument
(
"
--seed
"
,
type
=
int
,
default
=
None
)
args
=
parser
.
parse_args
()
if
args
.
seed
:
np
.
random
.
seed
(
args
.
seed
)
random
.
seed
(
args
.
seed
)
iter
=
1000
# robot_starts = [[0.05, 0.05], [4.95, 4.95], [0.05, 4.95], [4.95, 0.05]]
# robot_goals = [[4.95, 4.95], [0.05, 0.05], [4.95, 0.05], [0.05, 4.95]]
robot_starts
=
[[
0.05
,
0.05
],
[
4.95
,
4.95
]]
robot_goals
=
[[
4.95
,
4.95
],
[
0.05
,
0.05
]]
circ_obstacles
=
[(
3.5
,
2.5
,
1
)]
rect_obstacles
=
[]
robot_paths
=
[]
def
edgeCost
(
x1
,
x2
):
x2
=
np
.
array
(
x2
)
sum_dist
=
0
for
path
in
robot_paths
:
for
loc
in
path
:
sum_dist
+=
np
.
linalg
.
norm
(
x2
-
np
.
array
(
loc
))
cost
=
np
.
log
(
1
+
sum_dist
)
# print(cost)
return
cost
space
=
CircleRectangleObstacleCSpace
(
x_bounds
=
(
0
,
5
),
y_bounds
=
(
0
,
5
),
robot_radius
=
0.05
)
for
(
x
,
y
,
r
)
in
circ_obstacles
:
space
.
addObstacle
(
Circle
(
x
,
y
,
r
))
for
(
x
,
y
,
w
,
h
)
in
rect_obstacles
:
space
.
addObstacle
(
Rectangle
(
x
,
y
,
w
,
h
))
planner
=
"
rrt*
"
MotionPlan
.
setOptions
(
type
=
planner
,
perturbationRadius
=
0.25
,
bidirectional
=
True
)
for
start
,
goal
in
zip
(
robot_starts
,
robot_goals
):
mp
=
MotionPlan
(
space
,
type
=
planner
)
mp
.
setEndpoints
(
start
,
goal
)
mp
.
setCostFunction
(
edgeCost
)
mp
.
planMore
(
iter
)
path
=
mp
.
getPath
()
robot_paths
.
append
(
path
)
mp
.
close
()
# program = CSpaceObstacleProgram(space,start,goal)
# program.view.w = program.view.h = 640
# program.name = "Motion planning test"
# if vis:
# program.run()
# path = program.planner.getPath()
# else:
# program.planner.planMore(iter)
# path = program.planner.getPath()
import
matplotlib.pyplot
as
plt
import
matplotlib.patches
as
patches
fig
,
ax
=
plt
.
subplots
(
1
)
for
(
x
,
y
,
r
)
in
circ_obstacles
:
circle
=
patches
.
Circle
((
x
,
y
),
r
,
facecolor
=
'
black
'
,
edgecolor
=
None
)
ax
.
add_patch
(
circle
)
for
(
x
,
y
,
w
,
h
)
in
rect_obstacles
:
rect
=
patches
.
Rectangle
((
x
,
y
),
w
,
h
,
facecolor
=
'
black
'
,
edgecolor
=
None
)
ax
.
add_patch
(
rect
)
# colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
for
i
,
path
in
enumerate
(
robot_paths
):
path
=
np
.
array
(
path
)
print
(
f
"
Path
{
i
}
:
{
path
}
"
)
x_coords
=
path
[:,
0
]
y_coords
=
path
[:,
1
]
plt
.
plot
(
x_coords
,
y_coords
,
label
=
f
'
Robot
{
i
}
'
)
plt
.
legend
(
loc
=
'
upper left
'
)
ax
.
set_aspect
(
'
equal
'
)
plt
.
show
()
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment