Skip to content

Commit f2268f3

Browse files
takayuki5168takayuki5168
committed
Merge branch 'master' into arm-manipulation
2 parents 47b4970 + b762fd0 commit f2268f3

File tree

200 files changed

+8829
-4100
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

200 files changed

+8829
-4100
lines changed

‎.github/FUNDING.yml

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# These are supported funding model platforms
2+
patreon: myenigma
3+
custom: https://www.paypal.me/myenigmapay/

‎.gitignore

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,6 @@
11
*.csv
2+
*.gif
3+
*.g2o
24

35
# Byte-compiled / optimized / DLL files
46
__pycache__/
@@ -66,3 +68,5 @@ target/
6668

6769
#Ipython Notebook
6870
.ipynb_checkpoints
71+
72+
matplotrecorder/*

‎.gitmodules

Lines changed: 0 additions & 4 deletions
This file was deleted.

‎.lgtm.yml

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
extraction:
2+
python:
3+
python_setup:
4+
version: 3

‎.travis.yml

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,6 @@ language: python
33
matrix:
44
include:
55
- os: linux
6-
#- os: osx
7-
#python: "3.6-dev"
86

97
python:
108
- 3.6
@@ -20,14 +18,14 @@ before_install:
2018
- conda update -q conda
2119
# Useful for debugging any issues with conda
2220
- conda info -a
21+
- conda install python==3.6.8
2322

2423
install:
25-
- conda install numpy
24+
- conda install numpy==1.15
2625
- conda install scipy
2726
- conda install matplotlib
2827
- conda install pandas
29-
- conda install -c conda-forge lapack
30-
- conda install -c cvxgrp cvxpy
28+
- pip install cvxpy
3129
- conda install coveralls
3230

3331
script:

‎AerialNavigation/drone_3d_trajectory_following/Quadrotor.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,12 +51,12 @@ def transformation_matrix(self):
5151
yaw = self.yaw
5252
return np.array(
5353
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],
54-
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
55-
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
54+
[sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch)
55+
* sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
5656
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
5757
])
5858

59-
def plot(self):
59+
def plot(self): # pragma: no cover
6060
T = self.transformation_matrix()
6161

6262
p1_t = np.matmul(T, self.p1)

‎AerialNavigation/drone_3d_trajectory_following/animation.gif

-12.1 MB
Binary file not shown.

‎AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,8 +73,8 @@ def quad_sim(x_c, y_c, z_c):
7373
# des_x_pos = calculate_position(x_c[i], t)
7474
# des_y_pos = calculate_position(y_c[i], t)
7575
des_z_pos = calculate_position(z_c[i], t)
76-
des_x_vel = calculate_velocity(x_c[i], t)
77-
des_y_vel = calculate_velocity(y_c[i], t)
76+
# des_x_vel = calculate_velocity(x_c[i], t)
77+
# des_y_vel = calculate_velocity(y_c[i], t)
7878
des_z_vel = calculate_velocity(z_c[i], t)
7979
des_x_acc = calculate_acceleration(x_c[i], t)
8080
des_y_acc = calculate_acceleration(y_c[i], t)

‎AerialNavigation/rocket_powered_landing/animation.gif

-929 KB
Binary file not shown.

‎AerialNavigation/rocket_powered_landing/rocket_powered_landing.ipynb

Lines changed: 33 additions & 23 deletions
Large diffs are not rendered by default.

‎AerialNavigation/rocket_powered_landing/rocket_powered_landing.py

Lines changed: 58 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
88
Ref:
99
- Python implementation of 'Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time' paper
10-
by Michael Szmuk and Behçet Açıkmeşe.
10+
by Michael Szmuk and Behcet Acıkmese.
1111
1212
- EmbersArc/SuccessiveConvexificationFreeFinalTime: Implementation of "Successive Convexification for 6-DoF Mars Rocket Powered Landing with Free-Final-Time" https://github.com/EmbersArc/SuccessiveConvexificationFreeFinalTime
1313
@@ -129,12 +129,12 @@ def f_func(self, x, u):
129129
[vx],
130130
[vy],
131131
[vz],
132-
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy *
133-
(q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
134-
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2 +
135-
2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
136-
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy *
137-
(q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
132+
[(-1.0 * m - ux * (2 * q2**2 + 2 * q3**2 - 1) - 2 * uy
133+
* (q0 * q3 - q1 * q2) + 2 * uz * (q0 * q2 + q1 * q3)) / m],
134+
[(2 * ux * (q0 * q3 + q1 * q2) - uy * (2 * q1**2
135+
+ 2 * q3**2 - 1) - 2 * uz * (q0 * q1 - q2 * q3)) / m],
136+
[(-2 * ux * (q0 * q2 - q1 * q3) + 2 * uy
137+
* (q0 * q1 + q2 * q3) - uz * (2 * q1**2 + 2 * q2**2 - 1)) / m],
138138
[-0.5 * q1 * wx - 0.5 * q2 * wy - 0.5 * q3 * wz],
139139
[0.5 * q0 * wx + 0.5 * q2 * wz - 0.5 * q3 * wy],
140140
[0.5 * q0 * wy - 0.5 * q1 * wz + 0.5 * q3 * wx],
@@ -154,20 +154,20 @@ def A_func(self, x, u):
154154
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
155155
[0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
156156
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
157-
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz -
158-
q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
159-
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz +
160-
q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
161-
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy -
162-
q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
163-
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy, -
164-
0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
165-
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz, -
166-
0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
157+
[(ux * (2 * q2**2 + 2 * q3**2 - 1) + 2 * uy * (q0 * q3 - q1 * q2) - 2 * uz * (q0 * q2 + q1 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q2 * uz
158+
- q3 * uy) / m, 2 * (q2 * uy + q3 * uz) / m, 2 * (q0 * uz + q1 * uy - 2 * q2 * ux) / m, 2 * (-q0 * uy + q1 * uz - 2 * q3 * ux) / m, 0, 0, 0],
159+
[(-2 * ux * (q0 * q3 + q1 * q2) + uy * (2 * q1**2 + 2 * q3**2 - 1) + 2 * uz * (q0 * q1 - q2 * q3)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (-q1 * uz
160+
+ q3 * ux) / m, 2 * (-q0 * uz - 2 * q1 * uy + q2 * ux) / m, 2 * (q1 * ux + q3 * uz) / m, 2 * (q0 * ux + q2 * uz - 2 * q3 * uy) / m, 0, 0, 0],
161+
[(2 * ux * (q0 * q2 - q1 * q3) - 2 * uy * (q0 * q1 + q2 * q3) + uz * (2 * q1**2 + 2 * q2**2 - 1)) / m**2, 0, 0, 0, 0, 0, 0, 2 * (q1 * uy
162+
- q2 * ux) / m, 2 * (q0 * uy - 2 * q1 * uz + q3 * ux) / m, 2 * (-q0 * ux - 2 * q2 * uz + q3 * uy) / m, 2 * (q1 * ux + q2 * uy) / m, 0, 0, 0],
163+
[0, 0, 0, 0, 0, 0, 0, 0, -0.5 * wx, -0.5 * wy,
164+
- 0.5 * wz, -0.5 * q1, -0.5 * q2, -0.5 * q3],
165+
[0, 0, 0, 0, 0, 0, 0, 0.5 * wx, 0, 0.5 * wz,
166+
- 0.5 * wy, 0.5 * q0, -0.5 * q3, 0.5 * q2],
167167
[0, 0, 0, 0, 0, 0, 0, 0.5 * wy, -0.5 * wz, 0,
168168
0.5 * wx, 0.5 * q3, 0.5 * q0, -0.5 * q1],
169-
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy, -
170-
0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
169+
[0, 0, 0, 0, 0, 0, 0, 0.5 * wz, 0.5 * wy,
170+
- 0.5 * wx, 0, -0.5 * q2, 0.5 * q1, 0.5 * q0],
171171
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
172172
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
173173
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]])
@@ -184,12 +184,12 @@ def B_func(self, x, u):
184184
[0, 0, 0],
185185
[0, 0, 0],
186186
[0, 0, 0],
187-
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2 *
188-
(-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
189-
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2 *
190-
q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
191-
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3) /
192-
m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
187+
[(-2 * q2**2 - 2 * q3**2 + 1) / m, 2
188+
* (-q0 * q3 + q1 * q2) / m, 2 * (q0 * q2 + q1 * q3) / m],
189+
[2 * (q0 * q3 + q1 * q2) / m, (-2 * q1**2 - 2
190+
* q3**2 + 1) / m, 2 * (-q0 * q1 + q2 * q3) / m],
191+
[2 * (-q0 * q2 + q1 * q3) / m, 2 * (q0 * q1 + q2 * q3)
192+
/ m, (-2 * q1**2 - 2 * q2**2 + 1) / m],
193193
[0, 0, 0],
194194
[0, 0, 0],
195195
[0, 0, 0],
@@ -227,12 +227,12 @@ def skew(self, v):
227227

228228
def dir_cosine(self, q):
229229
return np.matrix([
230-
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] +
231-
q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
232-
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2 *
233-
(q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
234-
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] -
235-
q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
230+
[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2]
231+
+ q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
232+
[2 * (q[1] * q[2] - q[0] * q[3]), 1 - 2
233+
* (q[1] ** 2 + q[3] ** 2), 2 * (q[2] * q[3] + q[0] * q[1])],
234+
[2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3]
235+
- q[0] * q[1]), 1 - 2 * (q[1] ** 2 + q[2] ** 2)]
236236
])
237237

238238
def omega(self, w):
@@ -460,15 +460,15 @@ def __init__(self, m, K):
460460
# x_t+1 = A_*x_t+B_*U_t+C_*U_T+1*S_*sigma+zbar+nu
461461
constraints += [
462462
self.var['X'][:, k + 1] ==
463-
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x))
464-
* self.var['X'][:, k]
465-
+ cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u))
466-
* self.var['U'][:, k]
467-
+ cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u))
468-
* self.var['U'][:, k + 1]
469-
+ self.par['S_bar'][:, k] * self.var['sigma']
470-
+ self.par['z_bar'][:, k]
471-
+ self.var['nu'][:, k]
463+
cvxpy.reshape(self.par['A_bar'][:, k], (m.n_x, m.n_x)) *
464+
self.var['X'][:, k] +
465+
cvxpy.reshape(self.par['B_bar'][:, k], (m.n_x, m.n_u)) *
466+
self.var['U'][:, k] +
467+
cvxpy.reshape(self.par['C_bar'][:, k], (m.n_x, m.n_u)) *
468+
self.var['U'][:, k + 1] +
469+
self.par['S_bar'][:, k] * self.var['sigma'] +
470+
self.par['z_bar'][:, k] +
471+
self.var['nu'][:, k]
472472
for k in range(K - 1)
473473
]
474474

@@ -486,10 +486,10 @@ def __init__(self, m, K):
486486

487487
# Objective:
488488
sc_objective = cvxpy.Minimize(
489-
self.par['weight_sigma'] * self.var['sigma']
490-
+ self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf')
491-
+ self.par['weight_delta'] * self.var['delta_norm']
492-
+ self.par['weight_delta_sigma'] * self.var['sigma_norm']
489+
self.par['weight_sigma'] * self.var['sigma'] +
490+
self.par['weight_nu'] * cvxpy.norm(self.var['nu'], 'inf') +
491+
self.par['weight_delta'] * self.var['delta_norm'] +
492+
self.par['weight_delta_sigma'] * self.var['sigma_norm']
493493
)
494494

495495
objective = sc_objective
@@ -550,20 +550,20 @@ def solve(self, **kwargs):
550550

551551
def axis3d_equal(X, Y, Z, ax):
552552

553-
max_range = np.array([X.max() - X.min(), Y.max() -
554-
Y.min(), Z.max() - Z.min()]).max()
555-
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
556-
1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
557-
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
558-
1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
559-
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2, -
560-
1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
553+
max_range = np.array([X.max() - X.min(), Y.max()
554+
- Y.min(), Z.max() - Z.min()]).max()
555+
Xb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
556+
- 1:2:2][0].flatten() + 0.5 * (X.max() + X.min())
557+
Yb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
558+
- 1:2:2][1].flatten() + 0.5 * (Y.max() + Y.min())
559+
Zb = 0.5 * max_range * np.mgrid[-1:2:2, -1:2:2,
560+
- 1:2:2][2].flatten() + 0.5 * (Z.max() + Z.min())
561561
# Comment or uncomment following both lines to test the fake bounding box:
562562
for xb, yb, zb in zip(Xb, Yb, Zb):
563563
ax.plot([xb], [yb], [zb], 'w')
564564

565565

566-
def plot_animation(X, U):
566+
def plot_animation(X, U): # pragma: no cover
567567

568568
fig = plt.figure()
569569
ax = fig.gca(projection='3d')
@@ -576,14 +576,14 @@ def plot_animation(X, U):
576576
axis3d_equal(X[2, :], X[3, :], X[1, :], ax)
577577

578578
rx, ry, rz = X[1:4, k]
579-
vx, vy, vz = X[4:7, k]
579+
# vx, vy, vz = X[4:7, k]
580580
qw, qx, qy, qz = X[7:11, k]
581581

582582
CBI = np.array([
583583
[1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy + qw * qz),
584584
2 * (qx * qz - qw * qy)],
585-
[2 * (qx * qy - qw * qz), 1 - 2 *
586-
(qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
585+
[2 * (qx * qy - qw * qz), 1 - 2
586+
* (qx ** 2 + qz ** 2), 2 * (qy * qz + qw * qx)],
587587
[2 * (qx * qz + qw * qy), 2 * (qy * qz - qw * qx),
588588
1 - 2 * (qx ** 2 + qy ** 2)]
589589
])
@@ -618,8 +618,6 @@ def main():
618618
integrator = Integrator(m, K)
619619
problem = SCProblem(m, K)
620620

621-
last_linear_cost = None
622-
623621
converged = False
624622
w_delta = W_DELTA
625623
for it in range(iterations):
@@ -633,7 +631,7 @@ def main():
633631
X_last=X, U_last=U, sigma_last=sigma,
634632
weight_sigma=W_SIGMA, weight_nu=W_NU,
635633
weight_delta=w_delta, weight_delta_sigma=W_DELTA_SIGMA)
636-
info = problem.solve()
634+
problem.solve()
637635

638636
X = problem.get_variable('X')
639637
U = problem.get_variable('U')
@@ -658,7 +656,7 @@ def main():
658656
print(f'Converged after {it + 1} iterations.')
659657
break
660658

661-
if show_animation:
659+
if show_animation: # pragma: no cover
662660
plot_animation(X, U)
663661

664662
print("done!!")

‎ArmNavigation/arm_obstacle_navigation/animation.gif

-2.91 MB
Binary file not shown.

0 commit comments

Comments
 (0)