AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

enable MPC test (#620)

Browse Source 
* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test

* enable_mpc_test
This commit is contained in:
Atsushi Sakai
2022-01-08 17:06:29 +09:00
committed by GitHub
parent c05a4fdada
commit a13ef29dc4
5 changed files with 58 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
AerialNavigation/rocket_powered_landing/rocket_powered_landing.py
View File

@@ -43,7 +43,7 @@ class Rocket_Model_6DoF:
A 6 degree of freedom rocket landing problem.
"""
def __init__(self):
def __init__(self, rng):
"""
A large r_scale for a small scale problem will
ead to numerical problems as parameters become excessively small
@@ -92,7 +92,7 @@ class Rocket_Model_6DoF:
# Vector from thrust point to CoM
self.r_T_B = np.array([-1e-2, 0., 0.])
self.set_random_initial_state()
self.set_random_initial_state(rng)
self.x_init = np.concatenate(
((self.m_wet,), self.r_I_init, self.v_I_init, self.q_B_I_init, self.w_B_init))
@@ -102,29 +102,32 @@ class Rocket_Model_6DoF:
self.r_scale = np.linalg.norm(self.r_I_init)
self.m_scale = self.m_wet
def set_random_initial_state(self):
def set_random_initial_state(self, rng):
if rng is None:
rng = np.random.default_rng()
self.r_I_init = np.array((0., 0., 0.))
self.r_I_init[0] = np.random.uniform(3, 4)
self.r_I_init[1:3] = np.random.uniform(-2, 2, size=2)
self.r_I_init[0] = rng.uniform(3, 4)
self.r_I_init[1:3] = rng.uniform(-2, 2, size=2)
self.v_I_init = np.array((0., 0., 0.))
self.v_I_init[0] = np.random.uniform(-1, -0.5)
self.v_I_init[1:3] = np.random.uniform(
-0.5, -0.2, size=2) * self.r_I_init[1:3]
self.v_I_init[0] = rng.uniform(-1, -0.5)
self.v_I_init[1:3] = rng.uniform(-0.5, -0.2,
size=2) * self.r_I_init[1:3]
self.q_B_I_init = self.euler_to_quat((0,
np.random.uniform(-30, 30),
np.random.uniform(-30, 30)))
rng.uniform(-30, 30),
rng.uniform(-30, 30)))
self.w_B_init = np.deg2rad((0,
np.random.uniform(-20, 20),
np.random.uniform(-20, 20)))
rng.uniform(-20, 20),
rng.uniform(-20, 20)))
def f_func(self, x, u):
m, rx, ry, rz, vx, vy, vz, q0, q1, q2, q3, wx, wy, wz = x[0], x[1], x[
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
return np.matrix([
return np.array([
[-0.01 * np.sqrt(ux**2 + uy**2 + uz**2)],
[vx],
[vy],
@@ -149,7 +152,7 @@ class Rocket_Model_6DoF:
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
return np.matrix([
return np.array([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
@@ -177,7 +180,7 @@ class Rocket_Model_6DoF:
2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13]
ux, uy, uz = u[0], u[1], u[2]
return np.matrix([
return np.array([
[-0.01 * ux / np.sqrt(ux**2 + uy**2 + uz**2),
-0.01 * uy / np.sqrt(ux ** 2 + uy**2 + uz**2),
-0.01 * uz / np.sqrt(ux**2 + uy**2 + uz**2)],
@@ -219,14 +222,14 @@ class Rocket_Model_6DoF:
return q
def skew(self, v):
return np.matrix([
return np.array([
[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]
])
def dir_cosine(self, q):
return np.matrix([
return np.array([
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
+ q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
@@ -236,7 +239,7 @@ class Rocket_Model_6DoF:
])
def omega(self, w):
return np.matrix([
return np.array([
[0, -w[0], -w[1], -w[2]],
[w[0], 0, w[2], -w[1]],
[w[1], -w[2], 0, w[0]],
@@ -304,7 +307,7 @@ class Rocket_Model_6DoF:
]
# linearized lower thrust constraint
rhs = [U_last_p[:, k] / cvxpy.norm(U_last_p[:, k]) * U_v[:, k]
rhs = [U_last_p[:, k] / cvxpy.norm(U_last_p[:, k]) @ U_v[:, k]
for k in range(X_v.shape[1])]
constraints += [
self.T_min <= cvxpy.vstack(rhs)
@@ -460,11 +463,11 @@ class SCProblem:
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
constraints += [
self.var['X'][:, k + 1] ==
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) @
self.var['X'][:, k] +
cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) @
self.var['U'][:, k] +
cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) @
self.var['U'][:, k + 1] +
self.par['S_bar'][:, k] * self.var['sigma'] +
self.par['z_bar'][:, k] +
@@ -606,9 +609,9 @@ def plot_animation(X, U): # pragma: no cover
plt.pause(0.5)
def main():
def main(rng=None):
print("start!!")
m = Rocket_Model_6DoF()
m = Rocket_Model_6DoF(rng)
# state and input list
X = np.empty(shape=[m.n_x, K])

16
PathPlanning/ProbabilisticRoadMap/probabilistic_road_map.py
View File

@@ -40,14 +40,14 @@ def prm_planning(start_x, start_y, goal_x, goal_y,
"""
Run probabilistic road map planning
:param start_x:
:param start_y:
:param goal_x:
:param goal_y:
:param obstacle_x_list:
:param obstacle_y_list:
:param robot_radius:
:param rng:
:param start_x: start x position
:param start_y: start y position
:param goal_x: goal x position
:param goal_y: goal y position
:param obstacle_x_list: obstacle x positions
:param obstacle_y_list: obstacle y positions
:param robot_radius: robot radius
:param rng: (Optional) Random generator
:return:
"""
obstacle_kd_tree = KDTree(np.vstack((obstacle_x_list, obstacle_y_list)).T)

19
tests/test_model_predictive_speed_and_steer_control.py
View File

@@ -1,18 +1,19 @@
import sys
import conftest
if 'cvxpy' in sys.modules: # pragma: no cover
from PathTracking.model_predictive_speed_and_steer_control \
import model_predictive_speed_and_steer_control as m
from PathTracking.model_predictive_speed_and_steer_control \
import model_predictive_speed_and_steer_control as m
def test_1():
m.show_animation = False
m.main()
def test_1():
m.show_animation = False
m.main()
def test_2():
m.show_animation = False
m.main2()
def test_2():
m.show_animation = False
m.main2()
if __name__ == '__main__':
conftest.run_this_test(__file__)

2
tests/test_probabilistic_road_map.py
View File

@@ -5,7 +5,7 @@ from PathPlanning.ProbabilisticRoadMap import probabilistic_road_map
def test1():
probabilistic_road_map.show_animation = False
probabilistic_road_map.main(rng=np.random.default_rng(1234))
probabilistic_road_map.main(rng=np.random.default_rng(1233))
if __name__ == '__main__':

17
tests/test_rocket_powered_landing.py
View File

@@ -1,13 +1,20 @@
import conftest # Add root path to sys.path
import sys
import numpy as np
from numpy.testing import suppress_warnings
if 'cvxpy' in sys.modules: # pragma: no cover
from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m
from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m
def test1():
m.show_animation = False
m.main()
def test1():
m.show_animation = False
with suppress_warnings() as sup:
sup.filter(UserWarning,
"You are solving a parameterized problem that is not DPP"
)
sup.filter(UserWarning,
"Solution may be inaccurate")
m.main(rng=np.random.default_rng(1234))
if __name__ == '__main__':