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
f73f460e
Commit
f73f460e
authored
1 month ago
by
rachelmoan
Browse files
Options
Downloads
Patches
Plain Diff
get ref trajectory purely from guide path, instead of trying to use velocities
parent
fcc10444
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/controllers/utils.py
+9
-82
9 additions, 82 deletions
guided_mrmp/controllers/utils.py
with
9 additions
and
82 deletions
guided_mrmp/controllers/utils.py
+
9
−
82
View file @
f73f460e
...
...
@@ -99,50 +99,27 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
"""
K
=
int
(
T
/
DT
)
xref
=
np
.
zeros
((
3
,
K
))
# Reference trajectory for [x, y, theta]
path_distances
=
[
0
]
for
i
in
range
(
1
,
len
(
path
)):
dist
=
np
.
linalg
.
norm
(
np
.
array
(
path
[
i
])
-
np
.
array
(
path
[
i
-
1
]))
path_distances
.
append
(
path_distances
[
-
1
]
+
dist
)
# Find the last visited point
last_visited_idx
=
0
if
path_visited_points
==
[]
else
path_visited_points
[
-
1
]
# Find the spatially closest point after the last visited point
next_ind
=
last_visited_idx
+
1
path_visited_points
.
append
(
next_ind
)
K
=
min
(
K
,
len
(
path
[
0
])
-
next_ind
)
xref
=
np
.
zeros
((
3
,
K
))
# Reference trajectory for [x, y, theta]
ind
=
next_ind
# ind = get_nn_idx(state, path, path_visited_points)
# min_dist = float('inf')
# for i in range(last_visited_idx+2, len(path)):
# dist = np.linalg.norm(np.array(path[i][:2]) - np.array(state[:2]))
# if dist < min_dist:
# min_dist = dist
# ind = i
path_visited_points
.
append
(
ind
)
# calculate the cumulative distance along the path
cdist
=
np
.
append
([
0.0
],
np
.
cumsum
(
np
.
hypot
(
np
.
diff
(
path
[
0
,
:]),
np
.
diff
(
path
[
1
,
:]))))
cdist
=
np
.
clip
(
cdist
,
cdist
[
0
],
cdist
[
-
1
])
# determine where we want the robot to be at each time step
start_dist
=
cdist
[
ind
]
interp_points
=
[
d
*
DT
*
target_v
+
start_dist
for
d
in
range
(
1
,
K
+
1
)]
# return the next k points on the path
xref
[
0
,:]
=
path
[
0
,
next_ind
:
next_ind
+
K
]
xref
[
1
,:]
=
path
[
1
,
next_ind
:
next_ind
+
K
]
xref
[
2
,:]
=
path
[
2
,
next_ind
:
next_ind
+
K
]
# interpolate between these points to get the reference trajectory
xref
[
0
,
:]
=
np
.
interp
(
interp_points
,
cdist
,
path
[
0
,
:])
xref
[
1
,
:]
=
np
.
interp
(
interp_points
,
cdist
,
path
[
1
,
:])
xref
[
2
,
:]
=
np
.
interp
(
interp_points
,
cdist
,
path
[
2
,
:])
# Transform to ego frame
dx
=
xref
[
0
,
:]
-
state
[
0
]
dy
=
xref
[
1
,
:]
-
state
[
1
]
xref
[
0
,
:]
=
dx
*
np
.
cos
(
-
state
[
2
])
-
dy
*
np
.
sin
(
-
state
[
2
])
# X
xref
[
1
,
:]
=
dy
*
np
.
cos
(
-
state
[
2
])
+
dx
*
np
.
sin
(
-
state
[
2
])
# Y
xref
[
2
,
:]
=
path
[
2
,
ind
]
-
state
[
2
]
# Theta
xref
[
2
,
:]
=
path
[
2
,
next_
ind
]
-
state
[
2
]
# Theta
# Normalize the angles
xref
[
2
,
:]
=
(
xref
[
2
,
:]
+
np
.
pi
)
%
(
2.0
*
np
.
pi
)
-
np
.
pi
...
...
@@ -150,53 +127,3 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
return
xref
,
path_visited_points
# def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
# """
# Generates a reference trajectory for the Roomba.
# Args:
# state (array-like): Current state [x, y, theta]
# path (ndarray): Path points [x, y, theta] in the global frame
# path_visited_points (array-like): Visited path points [[x, y], [x, y], ...]
# target_v (float): Desired speed
# T (float): Control horizon duration
# DT (float): Control horizon time-step
# Returns:
# ndarray: Reference trajectory [x_k, y_k, theta_k] in the ego frame
# """
# K = int(T / DT)
# xref = np.zeros((3, K)) # Reference trajectory for [x, y, theta]
# # find the nearest path point to the current state
# ind = get_nn_idx(state, path, path_visited_points)
# path_visited_points.append([path[0, ind], path[1, ind]])
# # calculate the cumulative distance along the path
# cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
# cdist = np.clip(cdist, cdist[0], cdist[-1])
# # determine where we want the robot to be at each time step
# start_dist = cdist[ind]
# interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
# # interpolate between these points to get the reference trajectory
# xref[0, :] = np.interp(interp_points, cdist, path[0, :])
# xref[1, :] = np.interp(interp_points, cdist, path[1, :])
# xref[2, :] = np.interp(interp_points, cdist, path[2, :])
# # Transform to ego frame
# dx = xref[0, :] - state[0]
# dy = xref[1, :] - state[1]
# xref[0, :] = dx * np.cos(-state[2]) - dy * np.sin(-state[2]) # X
# xref[1, :] = dy * np.cos(-state[2]) + dx * np.sin(-state[2]) # Y
# xref[2, :] = path[2, ind] - state[2] # Theta
# # Normalize the angles
# xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
# xref[2, :] = fix_angle_reference(xref[2, :], xref[2, 0])
# return xref, path_visited_points
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