Skip to content

Commit aa7679e

Browse files
jgoppertjgoppert
committed
Clean up and formatting. Also unified bezier code.
Signed-off-by: James Goppert <james.goppert@gmail.com>
1 parent 40b6772 commit aa7679e

File tree

16 files changed

+622
-901
lines changed

16 files changed

+622
-901
lines changed

‎.vscode/settings.json‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,11 @@
11
{
22
"cmake.configureOnOpen": false,
33
"python.defaultInterpreterPath": "${workspaceFolder}/.venv/bin/python",
4-
"python.terminal.activateEnvironment": true,
4+
"python.terminal.activateEnvironment": false,
55
"jupyter.defaultKernel": "cyecca",
66
"python.analysis.extraPaths": [
77
"${workspaceFolder}/.vscode/ros_path"
88
],
99
"python.envFile": "${workspaceFolder}/.vscode/env",
10-
"terminal.integrated.env.linux": {
11-
"VIRTUAL_ENV": "${workspaceFolder}/.venv",
12-
"PATH": "${workspaceFolder}/.venv/bin:${env:PATH}"
13-
},
1410
"terminal.integrated.defaultProfile.linux": "bash"
1511
}

‎CMakeLists.txt‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@ ament_python_install_package(cyecca)
2020

2121
install(PROGRAMS
2222
scripts/rdd2_sim.py
23-
scripts/rdd2_sim_sil.py
2423
scripts/fixedwing_sim.py
2524
DESTINATION lib/${PROJECT_NAME};
2625
)

‎config/rdd2.config.rviz‎

Lines changed: 114 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,11 @@ Panels:
44
Name: Displays
55
Property Tree Widget:
66
Expanded:
7-
- /Global Options1
8-
- /Status1
9-
- /TF1/Frames1
10-
Splitter Ratio: 0.5
11-
Tree Height: 602
7+
- /PoseWithCovariance1/Topic1
8+
- /Pose Setpoint1/Topic1
9+
- /Bezier Trajectory1/Topic1
10+
Splitter Ratio: 0.6823529601097107
11+
Tree Height: 595
1212
- Class: rviz_common/Selection
1313
Name: Selection
1414
- Class: rviz_common/Tool Properties
@@ -68,88 +68,39 @@ Visualization Manager:
6868
Scale: 1
6969
Value: true
7070
Value: true
71-
Enabled: true
71+
Enabled: false
7272
Head Length: 0.30000001192092896
7373
Head Radius: 0.10000000149011612
7474
Name: PoseWithCovariance
7575
Shaft Length: 1
7676
Shaft Radius: 0.05000000074505806
7777
Shape: Axes
7878
Topic:
79-
Depth: 5
79+
Depth: 1
8080
Durability Policy: Volatile
8181
Filter size: 10
8282
History Policy: Keep Last
83-
Reliability Policy: Reliable
83+
Reliability Policy: Best Effort
8484
Value: /pose
85-
Value: true
86-
- Alpha: 1
87-
Class: rviz_default_plugins/RobotModel
88-
Collision Enabled: false
89-
Description File: src/rdd2/rdd2_description/urdf/rdd2.urdf
90-
Description Source: File
91-
Description Topic:
92-
Depth: 5
93-
Durability Policy: Volatile
94-
History Policy: Keep Last
95-
Reliability Policy: Reliable
96-
Value: ""
97-
Enabled: true
98-
Links:
99-
All Links Enabled: true
100-
Expand Joint Details: false
101-
Expand Link Details: false
102-
Expand Tree: false
103-
Link Tree Style: Links in Alphabetic Order
104-
base_footprint:
105-
Alpha: 1
106-
Show Axes: false
107-
Show Trail: false
108-
base_link:
109-
Alpha: 1
110-
Show Axes: false
111-
Show Trail: false
112-
Value: true
113-
camera_link:
114-
Alpha: 1
115-
Show Axes: false
116-
Show Trail: false
117-
Value: true
118-
camera_link_optical:
119-
Alpha: 1
120-
Show Axes: false
121-
Show Trail: false
122-
lidar_link:
123-
Alpha: 1
124-
Show Axes: false
125-
Show Trail: false
126-
Value: true
127-
Mass Properties:
128-
Inertia: false
129-
Mass: false
130-
Name: RobotModel
131-
TF Prefix: ""
132-
Update Interval: 0
133-
Value: true
134-
Visual Enabled: true
85+
Value: false
13586
- Alpha: 0.5
136-
Axes Length: 1
137-
Axes Radius: 0.10000000149011612
87+
Axes Length: 0.5
88+
Axes Radius: 0.029999999329447746
13889
Class: rviz_default_plugins/Pose
13990
Color: 255; 25; 0
14091
Enabled: true
14192
Head Length: 0.20000000298023224
14293
Head Radius: 0.07500000298023224
143-
Name: Pose
94+
Name: Pose Setpoint
14495
Shaft Length: 1
14596
Shaft Radius: 0.02500000037252903
146-
Shape: Arrow
97+
Shape: Axes
14798
Topic:
148-
Depth: 5
99+
Depth: 1
149100
Durability Policy: Volatile
150-
Filter size: 10
101+
Filter size: 1
151102
History Policy: Keep Last
152-
Reliability Policy: Reliable
103+
Reliability Policy: Best Effort
153104
Value: /pose_sp
154105
Value: true
155106
- Class: rviz_default_plugins/TF
@@ -159,13 +110,53 @@ Visualization Manager:
159110
Frame Timeout: 15
160111
Frames:
161112
All Enabled: false
113+
base_footprint:
114+
Value: false
115+
base_link:
116+
Value: true
117+
base_link_est:
118+
Value: false
119+
camera_link:
120+
Value: false
121+
camera_link_optical:
122+
Value: false
123+
lidar_link:
124+
Value: false
125+
map:
126+
Value: false
127+
motor_0:
128+
Value: false
129+
motor_1:
130+
Value: false
131+
motor_2:
132+
Value: false
133+
motor_3:
134+
Value: false
162135
Marker Scale: 1
163136
Name: TF
164-
Show Arrows: true
137+
Show Arrows: false
165138
Show Axes: true
166139
Show Names: false
167140
Tree:
168-
{}
141+
map:
142+
base_link:
143+
base_footprint:
144+
{}
145+
camera_link:
146+
camera_link_optical:
147+
{}
148+
lidar_link:
149+
{}
150+
motor_0:
151+
{}
152+
motor_1:
153+
{}
154+
motor_2:
155+
{}
156+
motor_3:
157+
{}
158+
base_link_est:
159+
{}
169160
Update Interval: 0
170161
Value: true
171162
- Alpha: 1
@@ -178,7 +169,7 @@ Visualization Manager:
178169
Length: 0.30000001192092896
179170
Line Style: Lines
180171
Line Width: 0.029999999329447746
181-
Name: Path
172+
Name: Bezier Trajectory
182173
Offset:
183174
X: 0
184175
Y: 0
@@ -189,13 +180,62 @@ Visualization Manager:
189180
Shaft Diameter: 0.10000000149011612
190181
Shaft Length: 0.10000000149011612
191182
Topic:
192-
Depth: 5
183+
Depth: 1
193184
Durability Policy: Volatile
194185
Filter size: 10
195186
History Policy: Keep Last
196-
Reliability Policy: Reliable
187+
Reliability Policy: Best Effort
197188
Value: /path
198189
Value: true
190+
- Alpha: 1
191+
Class: rviz_default_plugins/RobotModel
192+
Collision Enabled: false
193+
Description File: ""
194+
Description Source: Topic
195+
Description Topic:
196+
Depth: 5
197+
Durability Policy: Volatile
198+
History Policy: Keep Last
199+
Reliability Policy: Reliable
200+
Value: /robot_description
201+
Enabled: true
202+
Links:
203+
All Links Enabled: true
204+
Expand Joint Details: false
205+
Expand Link Details: false
206+
Expand Tree: false
207+
Link Tree Style: Links in Alphabetic Order
208+
base_footprint:
209+
Alpha: 1
210+
Show Axes: false
211+
Show Trail: false
212+
base_link:
213+
Alpha: 1
214+
Show Axes: false
215+
Show Trail: false
216+
Value: true
217+
camera_link:
218+
Alpha: 1
219+
Show Axes: false
220+
Show Trail: false
221+
Value: true
222+
camera_link_optical:
223+
Alpha: 1
224+
Show Axes: false
225+
Show Trail: false
226+
lidar_link:
227+
Alpha: 1
228+
Show Axes: false
229+
Show Trail: false
230+
Value: true
231+
Mass Properties:
232+
Inertia: false
233+
Mass: false
234+
Name: RobotModel
235+
TF Prefix: ""
236+
Update Interval: 0
237+
Value: true
238+
Visual Enabled: true
199239
Enabled: true
200240
Global Options:
201241
Background Color: 48; 48; 48
@@ -242,7 +282,7 @@ Visualization Manager:
242282
Views:
243283
Current:
244284
Class: rviz_default_plugins/Orbit
245-
Distance: 4.085357189178467
285+
Distance: 5.8147735595703125
246286
Enable Stereo Rendering:
247287
Stereo Eye Separation: 0.05999999865889549
248288
Stereo Focal Distance: 1
@@ -257,18 +297,18 @@ Visualization Manager:
257297
Invert Z Axis: false
258298
Name: Current View
259299
Near Clip Distance: 0.009999999776482582
260-
Pitch: 0.7853981852531433
300+
Pitch: 0.5503981709480286
261301
Target Frame: base_link
262302
Value: Orbit (rviz_default_plugins)
263-
Yaw: 0.7853981852531433
303+
Yaw: 5.753571033477783
264304
Saved: ~
265305
Window Geometry:
266306
Displays:
267307
collapsed: false
268308
Height: 831
269309
Hide Left Dock: false
270310
Hide Right Dock: false
271-
QMainWindow State: 000000ff00000000fd000000040000000000000156000002e5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e5000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fe0000003efc0100000002fb0000000800540069006d0065020000004200000329000005fe0000003efb0000000800540069006d006501000000000000045000000000000000000000038d000002e500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
311+
QMainWindow State: 000000ff00000000fd000000040000000000000163000002e1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002e1000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002e1000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fe0000003efc0100000002fb0000000800540069006d0065020000004200000329000005fe0000003efb0000000800540069006d0065010000000000000450000000000000000000000380000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
272312
Selection:
273313
collapsed: false
274314
Time:

‎cyecca/models/bezier.py‎

Lines changed: 19 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,9 @@
11
import argparse
22
import os
3-
import sys
4-
import math
5-
import numpy as np
6-
import matplotlib.pyplot as plt
73
from pathlib import Path
84
import casadi as ca
9-
import cyecca.lie as lie
105
from cyecca.lie.group_so3 import SO3Quat, SO3EulerB321, SO3Dcm
116

12-
# print("python: ", sys.executable)
13-
14-
g = 9.8 # grav accel m/s^2
15-
m = 2.24 # mass of vehicle
16-
J_xx = 0.02166666666666667
17-
J_yy = 0.02166666666666667
18-
J_zz = 0.04000000000000001
19-
J_xz = 0
20-
217

228
class Bezier:
239
# https://en.wikipedia.org/wiki/B%C3%A9zier_curve
@@ -165,6 +151,14 @@ def derive_ref():
165151

166152
tol = 1e-6 # tolerance for singularities
167153

154+
# parameters
155+
g = ca.SX.sym("g")
156+
m = ca.SX.sym("m")
157+
Jx = ca.SX.sym("Jx")
158+
Jy = ca.SX.sym("Jy")
159+
Jz = ca.SX.sym("Jz")
160+
Jxz = ca.SX.sym("Jxz")
161+
168162
# flat output (input variables from trajectory planner)
169163
p_e = ca.SX.sym("p_e", 3) # position
170164
v_e = ca.SX.sym("v_e", 3) # velocity
@@ -232,10 +226,10 @@ def derive_ref():
232226
# Solve for Inputs
233227

234228
J = ca.SX(3, 3)
235-
J[0, 0] = J_xx
236-
J[1, 1] = J_yy
237-
J[2, 2] = J_zz
238-
J[0, 2] = J[2, 0] = J_xz
229+
J[0, 0] = Jx
230+
J[1, 1] = Jy
231+
J[2, 2] = Jz
232+
J[0, 2] = J[2, 0] = Jxz
239233

240234
M_b = J @ omega_dot_eb_b + ca.cross(omega_eb_b, J @ omega_eb_b)
241235

@@ -246,7 +240,7 @@ def derive_ref():
246240
functions = [
247241
ca.Function(
248242
"f_ref",
249-
[psi, psi_dot, psi_ddot, v_e, a_e, j_e, s_e],
243+
[psi, psi_dot, psi_ddot, v_e, a_e, j_e, s_e, m, g, Jx, Jy, Jz, Jxz],
250244
[v_b, quat, omega_eb_b, omega_dot_eb_b, M_b, T],
251245
[
252246
"psi",
@@ -256,6 +250,12 @@ def derive_ref():
256250
"a_e",
257251
"j_e",
258252
"s_e",
253+
"m",
254+
"g",
255+
"Jx",
256+
"Jy",
257+
"Jz",
258+
"Jxz",
259259
],
260260
["v_b", "quat", "omega_eb_b", "omega_dot_eb_b", "M_b", "T"],
261261
)

‎cyecca/models/fixedwing.py‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,7 @@
44
"""
55

66
import casadi as ca
7-
import cyecca
87
import numpy as np
9-
import matplotlib.pyplot as plt
10-
from pathlib import Path
118
from cyecca.lie.group_so3 import SO3Quat, SO3EulerB321
129
import cyecca.lie as lie
1310

‎cyecca/models/fixedwing_4ch.py‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,6 @@
55

66
import casadi as ca
77
import numpy as np
8-
import matplotlib.pyplot as plt
9-
from pathlib import Path
108
from cyecca.lie.group_so3 import SO3Quat, SO3EulerB321
119
import cyecca.lie as lie
1210

0 commit comments

Comments
 (0)