github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 10:37:54 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #35 from yasnem/patch-1

Browse Source 
Jacobian computation in  model_predictive_trajectory_generator.py
This commit is contained in:
Atsushi Sakai
2018-01-31 09:54:55 -08:00
committed by GitHub
parent b5ecb67611 d70e4c5f34
commit f3d434b6fe
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -42,7 +42,7 @@ def calc_J(target, p, h, k0):
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0] - h[0, 0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
d1 = np.matrix((dp - dn) / (2.0 * h[0, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
@@ -50,7 +50,7 @@ def calc_J(target, p, h, k0):
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
d2 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)