mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-11 05:17:59 -05:00
0df55e943e
* Without equals sign, sometimes get points that are in the wrong direction - relative to the points before and after it- when change in x or change in y along path is 0 * Created test script for dubins path generator * Made len == 0 it's own case, also changed 'l' to 'len' to appease travisCI * More variable renaming to appease CI * Broke == 0 into its own case in dubins planner, also Renaming files to appease CI * Reverting some naming changes * Turns out theres already a test for dubins.. not sure how I missed that * Note to self: run the test cases on your own before throwing them at CI * Added handling of length=0 case in generate_local_course() * Missed reverting 'mode' back to 'm' in one spot * Addressing style issues (line length) * Mostly works, now just need to setup linear regression to solve for weights * Re-arranged class * Wrote DMP program and added tests file * Styling fixes * More styling * Missed one indent * Multi-dimension path learning (e.g. in x and y instead of just x) * Added potential field obstacle avoidance * Potential field working much better but has issues with reaching goal state * Path ending to short not a result of obstacles, should be fix-able * Mostly working! end won't go to goal * split DMP and path following * pretty close * Okay this is working pretty well * looks.. okay. was using the wrong vector before * a plan to fix this mess * Okay seriously going to pivot to the dubins approach im done with potential field lol * Finished obstacle circle handling (and merging circles that are closer than their radii) * Finished circle event finder function * Some progress in preparing for dubins curves * Finished angle finding algo, need to test * Okay getting back to this, going to ignore the navigation and just focus on path generation since that's what DMP is for * Moved DMP files to path planning * changed folder name * Made demo path cooler * All working and added visualization tools (will remove * Fixed unit test and handled TODOs * not gonna handle this one * demo now scales with data * CI errors * CI errors * Fixing code style issues * more styling * fixing CI errors * formatting * Removed dead code * removed unused imports * removed uneccesary initialization * Applying PR feedback * fixing CI errors * added description to header and removed unused variable
50 lines
1.5 KiB
Python
50 lines
1.5 KiB
Python
import conftest
|
|
import numpy as np
|
|
from PathPlanning.DynamicMovementPrimitives import \
|
|
dynamic_movement_primitives
|
|
|
|
|
|
def test_1():
|
|
# test that trajectory can be learned from user-passed data
|
|
T = 5
|
|
t = np.arange(0, T, 0.01)
|
|
sin_t = np.sin(t)
|
|
train_data = np.array([t, sin_t]).T
|
|
|
|
DMP_controller = dynamic_movement_primitives.DMP(train_data, T)
|
|
DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4)
|
|
|
|
|
|
def test_2():
|
|
# test that length of trajectory is equal to desired number of timesteps
|
|
T = 5
|
|
t = np.arange(0, T, 0.01)
|
|
sin_t = np.sin(t)
|
|
train_data = np.array([t, sin_t]).T
|
|
|
|
DMP_controller = dynamic_movement_primitives.DMP(train_data, T)
|
|
t, path = DMP_controller.recreate_trajectory(train_data[0],
|
|
train_data[-1], 4)
|
|
|
|
assert(path.shape[0] == DMP_controller.timesteps)
|
|
|
|
|
|
def test_3():
|
|
# check that learned trajectory is close to initial
|
|
T = 3*np.pi/2
|
|
A_noise = 0.02
|
|
t = np.arange(0, T, 0.01)
|
|
noisy_sin_t = np.sin(t) + A_noise*np.random.rand(len(t))
|
|
train_data = np.array([t, noisy_sin_t]).T
|
|
|
|
DMP_controller = dynamic_movement_primitives.DMP(train_data, T)
|
|
t, pos = DMP_controller.recreate_trajectory(train_data[0],
|
|
train_data[-1], T)
|
|
|
|
diff = abs(pos[:, 1] - noisy_sin_t)
|
|
assert(max(diff) < 5*A_noise)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
conftest.run_this_test(__file__)
|