Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
C
cs598mp-fall2021-proj
Manage
Activity
Members
Labels
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Deploy
Releases
Model registry
Analyze
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
chsieh16
cs598mp-fall2021-proj
Commits
f502cc00
Commit
f502cc00
authored
1 year ago
by
chsieh16
Browse files
Options
Downloads
Patches
Plain Diff
Change constant forward velocity to a range
parent
99ba772f
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
gem_stanley_teacher.py
+19
-17
19 additions, 17 deletions
gem_stanley_teacher.py
with
19 additions
and
17 deletions
gem_stanley_teacher.py
+
19
−
17
View file @
f502cc00
...
...
@@ -11,7 +11,8 @@ WHEEL_BASE = 1.75 # m
K_P
=
0.45
CYCLE_TIME
=
0.05
# second
FORWARD_VEL
=
2.8
# m/s
FORWARD_VEL_MIN
=
2.5
# m/s
FORWARD_VEL_MAX
=
3.0
# m/s
STEERING_LIM
=
0.61
# rad
# Limits on unconstrained variables to avoid overflow and angle normalization
...
...
@@ -20,14 +21,10 @@ CTE_LIM = 2.0 # meter
# Bounds for intermediate variables.
# These are not necessary but can improve efficiency
K_CTE_V_LIM
=
K_P
*
CTE_LIM
/
FORWARD_VEL
K_CTE_V_LIM
=
K_P
*
20
*
CTE_LIM
/
FORWARD_VEL
_MIN
ATAN_K_CTE_V_LIM
=
np
.
arctan
(
K_CTE_V_LIM
)
RAW_ANG_ERR_LIM
=
(
ANG_LIM
+
ATAN_K_CTE_V_LIM
)
NEW_K_CTE_V_LIM
=
K_CTE_V_LIM
+
FORWARD_VEL
*
CYCLE_TIME
*
1.0
NEW_ATAN_K_CTE_V_LIM
=
np
.
arctan
(
NEW_K_CTE_V_LIM
)
NEW_RAW_ANG_ERR_LIM
=
ANG_LIM
+
FORWARD_VEL
*
CYCLE_TIME
class
DTreeGEMStanleyGurobiTeacherBase
(
DTreeGurobiTeacherBase
):
# Ideal perception as a linear transform from state to ground truth percept
...
...
@@ -54,9 +51,11 @@ class DTreeGEMStanleyGurobiTeacherBase(DTreeGurobiTeacherBase):
cte
,
psi
=
self
.
_percept
.
tolist
()
steering
,
=
self
.
_control
.
tolist
()
vel
=
m
.
addVar
(
name
=
"
Vf
"
,
lb
=
FORWARD_VEL_MIN
,
ub
=
FORWARD_VEL_MAX
)
# Controller
K_cte_V
=
m
.
addVar
(
name
=
"
K*d/Vf
"
,
lb
=-
K_CTE_V_LIM
,
ub
=
K_CTE_V_LIM
)
m
.
addConstr
(
K_
cte
_V
==
K_
P
*
cte
/
FORWARD_VEL
)
m
.
addConstr
(
cte
==
K_cte
_V
*
vel
/
K_P
)
atan_K_cte_V
=
m
.
addVar
(
name
=
"
atan(K*d/Vf)
"
,
lb
=-
ATAN_K_CTE_V_LIM
,
ub
=
ATAN_K_CTE_V_LIM
)
m
.
addGenConstrTan
(
xvar
=
atan_K_cte_V
,
yvar
=
K_cte_V
)
...
...
@@ -83,10 +82,10 @@ class DTreeGEMStanleyGurobiTeacherBase(DTreeGurobiTeacherBase):
sin_steer
=
m
.
addVar
(
name
=
"
sinδ
"
,
**
self
.
TRIGVAR
)
m
.
addGenConstrSin
(
steering
,
sin_steer
)
m
.
addConstr
(
new_x
==
old_x
+
FORWARD_VEL
*
CYCLE_TIME
*
cos_yaw_steer
)
m
.
addConstr
(
new_y
==
old_y
+
FORWARD_VEL
*
CYCLE_TIME
*
sin_yaw_steer
)
m
.
addConstr
(
new_x
==
old_x
+
vel
*
CYCLE_TIME
*
cos_yaw_steer
)
m
.
addConstr
(
new_y
==
old_y
+
vel
*
CYCLE_TIME
*
sin_yaw_steer
)
m
.
addConstr
(
new_yaw
==
old_yaw
+
sin_steer
*
FORWARD_VEL
*
CYCLE_TIME
/
WHEEL_BASE
)
old_yaw
+
sin_steer
*
vel
*
CYCLE_TIME
/
WHEEL_BASE
)
m
.
update
()
def
_add_objective
(
self
)
->
None
:
...
...
@@ -114,18 +113,20 @@ class DTreeGEMStanleyGurobiStabilityTeacher(DTreeGEMStanleyGurobiTeacherBase):
def
is_safe_state
(
self
,
ex
)
->
bool
:
assert
len
(
ex
)
==
self
.
state_dim
+
self
.
perc_dim
vel
=
FORWARD_VEL_MIN
def
g
(
perc
):
cte
,
psi
=
perc
error
=
psi
+
np
.
arctan
(
K_P
*
cte
/
FORWARD_VEL
)
error
=
psi
+
np
.
arctan
(
K_P
*
cte
/
vel
)
steer
=
np
.
clip
(
error
,
-
STEERING_LIM
,
STEERING_LIM
)
return
(
steer
,)
def
f
(
state
,
ctrl
):
x
,
y
,
theta
=
state
steer
,
=
ctrl
new_x
=
x
+
FORWARD_VEL
*
np
.
cos
(
theta
+
steer
)
*
CYCLE_TIME
new_y
=
y
+
FORWARD_VEL
*
np
.
sin
(
theta
+
steer
)
*
CYCLE_TIME
new_theta
=
theta
+
FORWARD_VEL
*
np
.
sin
(
steer
)
/
WHEEL_BASE
*
CYCLE_TIME
new_x
=
x
+
vel
*
np
.
cos
(
theta
+
steer
)
*
CYCLE_TIME
new_y
=
y
+
vel
*
np
.
sin
(
theta
+
steer
)
*
CYCLE_TIME
new_theta
=
theta
+
vel
*
np
.
sin
(
steer
)
/
WHEEL_BASE
*
CYCLE_TIME
return
new_x
,
new_y
,
new_theta
def
m_star
(
state
):
...
...
@@ -181,6 +182,7 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
def
is_safe_state
(
self
,
ex
)
->
bool
:
assert
len
(
ex
)
==
self
.
state_dim
+
self
.
perc_dim
vel
=
FORWARD_VEL_MIN
def
g
(
perc
):
cte
,
psi
=
perc
error
=
psi
+
np
.
arctan
(
K_P
*
cte
/
FORWARD_VEL
)
...
...
@@ -190,9 +192,9 @@ class DTreeGEMStanleyGurobiBarrierTeacher(DTreeGEMStanleyGurobiTeacherBase):
def
f
(
state
,
ctrl
):
x
,
y
,
theta
=
state
steer
,
=
ctrl
new_x
=
x
+
FORWARD_VEL
*
np
.
cos
(
theta
+
steer
)
*
CYCLE_TIME
new_y
=
y
+
FORWARD_VEL
*
np
.
sin
(
theta
+
steer
)
*
CYCLE_TIME
new_theta
=
theta
+
FORWARD_VEL
*
np
.
sin
(
steer
)
/
WHEEL_BASE
*
CYCLE_TIME
new_x
=
x
+
vel
*
np
.
cos
(
theta
+
steer
)
*
CYCLE_TIME
new_y
=
y
+
vel
*
np
.
sin
(
theta
+
steer
)
*
CYCLE_TIME
new_theta
=
theta
+
vel
*
np
.
sin
(
steer
)
/
WHEEL_BASE
*
CYCLE_TIME
return
new_x
,
new_y
,
new_theta
def
m_star
(
state
):
...
...
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