Skip to content

Commit 77d94db

Browse files
Jaeyoung-LimJaeyoung-Lim
authored
Add rviz pos plugin for commanding target position (#9)
* Add target pos plugin * Tune MPC * Put initial setpoint in the air * Update readme to include gif
1 parent b466bc7 commit 77d94db

30 files changed

+914
-31
lines changed

‎README.md

+3-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
# px4-mpc
22
This package contains an MPC using the casadi integrated with with PX4
33

4+
![mpc_setpoint](https://github.com/Jaeyoung-Lim/px4-mpc/assets/5248102/35dae5bf-626e-4272-a552-5f5d7e3c04cd)
5+
46
## Setup
57
To build the code, clone this repository into a ros2 workspace
68
Dependencies
@@ -32,5 +34,5 @@ micro-ros-agent udp4 --port 8888
3234

3335
In order to launch the mpc quadrotor in a ros2 launchfile,
3436
```
35-
ros2 launch px4_mpc mpc_quadrotor.launch.py
37+
ros2 launch px4_mpc mpc_quadrotor_launch.py
3638
```

‎mpc_msgs/CMakeLists.txt

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(mpc_msgs)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
find_package(ament_cmake REQUIRED)
10+
find_package(geometry_msgs REQUIRED)
11+
# uncomment the following section in order to fill in
12+
# further dependencies manually.
13+
# find_package(<dependency> REQUIRED)
14+
find_package(rosidl_default_generators REQUIRED)
15+
16+
rosidl_generate_interfaces(${PROJECT_NAME}
17+
"srv/SetPose.srv"
18+
DEPENDENCIES geometry_msgs
19+
)
20+
# ament_target_dependencies(${PROJECT_NAME} geometry_msgs)
21+
rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
22+
23+
if(BUILD_TESTING)
24+
find_package(ament_lint_auto REQUIRED)
25+
# the following line skips the linter which checks for copyrights
26+
# comment the line when a copyright and license is added to all source files
27+
set(ament_cmake_copyright_FOUND TRUE)
28+
# the following line skips cpplint (only works in a git repo)
29+
# comment the line when this package is in a git repo and when
30+
# a copyright and license is added to all source files
31+
set(ament_cmake_cpplint_FOUND TRUE)
32+
ament_lint_auto_find_test_dependencies()
33+
endif()
34+
35+
ament_package()

‎mpc_msgs/package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>mpc_msgs</name>
5+
<version>0.0.0</version>
6+
<description>Custom messages for the px4-mpc package</description>
7+
<maintainer email="[email protected]">jaeyoung</maintainer>
8+
<license>BSD-3</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<build_depend>rosidl_default_generators</build_depend>
12+
<depend>geometry_msgs</depend>
13+
<exec_depend>rosidl_default_runtime</exec_depend>
14+
15+
<member_of_group>rosidl_interface_packages</member_of_group>
16+
<test_depend>ament_lint_auto</test_depend>
17+
<test_depend>ament_lint_common</test_depend>
18+
19+
<export>
20+
<build_type>ament_cmake</build_type>
21+
</export>
22+
</package>

‎mpc_msgs/srv/SetPose.srv

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
geometry_msgs/Pose pose
2+
---
3+
bool result

‎package.xml renamed to ‎px4_mpc/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99
<license>BSD-3</license>
1010

1111
<depend>px4_msgs</depend>
12+
<depend>mpc_msgs</depend>
13+
<depend>px4_offboard</depend>
1214
<exec_depend>ros2launch</exec_depend>
1315
<test_depend>ament_copyright</test_depend>
1416
<test_depend>ament_flake8</test_depend>

‎px4_mpc/__init__.py renamed to ‎px4_mpc/px4_mpc/__init__.py

File renamed without changes.

‎config/astrobee_tuning.yaml renamed to ‎px4_mpc/px4_mpc/config/astrobee_tuning.yaml

File renamed without changes.

‎px4_mpc/px4_mpc/config/config.rviz

+245
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,245 @@
1+
Panels:
2+
- Class: rviz_common/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
- /Status1
9+
- /Pose1
10+
- /Marker1
11+
- /Marker1/Topic1
12+
- /Path1
13+
- /Path2
14+
- /InteractiveMarkers1
15+
- /Marker2
16+
- /Marker2/Topic1
17+
Splitter Ratio: 0.5
18+
Tree Height: 907
19+
- Class: rviz_common/Selection
20+
Name: Selection
21+
- Class: rviz_common/Tool Properties
22+
Expanded:
23+
- /2D Goal Pose1
24+
- /Publish Point1
25+
Name: Tool Properties
26+
Splitter Ratio: 0.5886790156364441
27+
- Class: rviz_common/Views
28+
Expanded:
29+
- /Current View1
30+
Name: Views
31+
Splitter Ratio: 0.5
32+
Visualization Manager:
33+
Class: ""
34+
Displays:
35+
- Alpha: 0.5
36+
Cell Size: 1
37+
Class: rviz_default_plugins/Grid
38+
Color: 160; 160; 164
39+
Enabled: true
40+
Line Style:
41+
Line Width: 0.029999999329447746
42+
Value: Lines
43+
Name: Grid
44+
Normal Cell Count: 0
45+
Offset:
46+
X: 0
47+
Y: 0
48+
Z: 0
49+
Plane: XY
50+
Plane Cell Count: 10
51+
Reference Frame: <Fixed Frame>
52+
Value: true
53+
- Alpha: 1
54+
Axes Length: 1
55+
Axes Radius: 0.10000000149011612
56+
Class: rviz_default_plugins/Pose
57+
Color: 255; 25; 0
58+
Enabled: true
59+
Head Length: 0.30000001192092896
60+
Head Radius: 0.10000000149011612
61+
Name: Pose
62+
Shaft Length: 1
63+
Shaft Radius: 0.05000000074505806
64+
Shape: Axes
65+
Topic:
66+
Depth: 5
67+
Durability Policy: Volatile
68+
Filter size: 10
69+
History Policy: Keep Last
70+
Reliability Policy: Reliable
71+
Value: /px4_visualizer/vehicle_pose
72+
Value: true
73+
- Class: rviz_default_plugins/Marker
74+
Enabled: true
75+
Name: Marker
76+
Namespaces:
77+
arrow: true
78+
Topic:
79+
Depth: 5
80+
Durability Policy: Volatile
81+
Filter size: 10
82+
History Policy: Keep Last
83+
Reliability Policy: Reliable
84+
Value: /px4_visualizer/vehicle_velocity
85+
Value: true
86+
- Alpha: 1
87+
Buffer Length: 1
88+
Class: rviz_default_plugins/Path
89+
Color: 25; 255; 0
90+
Enabled: true
91+
Head Diameter: 0.30000001192092896
92+
Head Length: 0.20000000298023224
93+
Length: 0.30000001192092896
94+
Line Style: Lines
95+
Line Width: 0.029999999329447746
96+
Name: Path
97+
Offset:
98+
X: 0
99+
Y: 0
100+
Z: 0
101+
Pose Color: 255; 85; 255
102+
Pose Style: None
103+
Radius: 0.029999999329447746
104+
Shaft Diameter: 0.10000000149011612
105+
Shaft Length: 0.10000000149011612
106+
Topic:
107+
Depth: 5
108+
Durability Policy: Volatile
109+
Filter size: 10
110+
History Policy: Keep Last
111+
Reliability Policy: Reliable
112+
Value: /px4_visualizer/vehicle_path
113+
Value: true
114+
- Alpha: 1
115+
Buffer Length: 1
116+
Class: rviz_default_plugins/Path
117+
Color: 25; 0; 255
118+
Enabled: true
119+
Head Diameter: 0.30000001192092896
120+
Head Length: 0.20000000298023224
121+
Length: 0.30000001192092896
122+
Line Style: Lines
123+
Line Width: 0.029999999329447746
124+
Name: Path
125+
Offset:
126+
X: 0
127+
Y: 0
128+
Z: 0
129+
Pose Color: 255; 85; 255
130+
Pose Style: None
131+
Radius: 0.029999999329447746
132+
Shaft Diameter: 0.10000000149011612
133+
Shaft Length: 0.10000000149011612
134+
Topic:
135+
Depth: 5
136+
Durability Policy: Volatile
137+
Filter size: 10
138+
History Policy: Keep Last
139+
Reliability Policy: Reliable
140+
Value: /px4_mpc/predicted_path
141+
Value: true
142+
- Class: rviz_default_plugins/InteractiveMarkers
143+
Enable Transparency: true
144+
Enabled: true
145+
Interactive Markers Namespace: /px4_mpc/rviz_target_pose_marker
146+
Name: InteractiveMarkers
147+
Show Axes: false
148+
Show Descriptions: true
149+
Show Visual Aids: false
150+
Value: true
151+
- Class: rviz_default_plugins/Marker
152+
Enabled: true
153+
Name: Marker
154+
Namespaces:
155+
arrow: true
156+
Topic:
157+
Depth: 5
158+
Durability Policy: Volatile
159+
Filter size: 10
160+
History Policy: Keep Last
161+
Reliability Policy: Reliable
162+
Value: /px4_mpc/reference
163+
Value: true
164+
Enabled: true
165+
Global Options:
166+
Background Color: 255; 255; 255
167+
Fixed Frame: map
168+
Frame Rate: 30
169+
Name: root
170+
Tools:
171+
- Class: rviz_default_plugins/Interact
172+
Hide Inactive Objects: true
173+
- Class: rviz_default_plugins/MoveCamera
174+
- Class: rviz_default_plugins/Select
175+
- Class: rviz_default_plugins/FocusCamera
176+
- Class: rviz_default_plugins/Measure
177+
Line color: 128; 128; 0
178+
- Class: rviz_default_plugins/SetInitialPose
179+
Covariance x: 0.25
180+
Covariance y: 0.25
181+
Covariance yaw: 0.06853891909122467
182+
Topic:
183+
Depth: 5
184+
Durability Policy: Volatile
185+
History Policy: Keep Last
186+
Reliability Policy: Reliable
187+
Value: /initialpose
188+
- Class: rviz_default_plugins/SetGoal
189+
Topic:
190+
Depth: 5
191+
Durability Policy: Volatile
192+
History Policy: Keep Last
193+
Reliability Policy: Reliable
194+
Value: /goal_pose
195+
- Class: rviz_default_plugins/PublishPoint
196+
Single click: true
197+
Topic:
198+
Depth: 5
199+
Durability Policy: Volatile
200+
History Policy: Keep Last
201+
Reliability Policy: Reliable
202+
Value: /clicked_point
203+
Transformation:
204+
Current:
205+
Class: rviz_default_plugins/TF
206+
Value: true
207+
Views:
208+
Current:
209+
Class: rviz_default_plugins/Orbit
210+
Distance: 13.942571640014648
211+
Enable Stereo Rendering:
212+
Stereo Eye Separation: 0.05999999865889549
213+
Stereo Focal Distance: 1
214+
Swap Stereo Eyes: false
215+
Value: false
216+
Focal Point:
217+
X: 0
218+
Y: 0
219+
Z: 0
220+
Focal Shape Fixed Size: true
221+
Focal Shape Size: 0.05000000074505806
222+
Invert Z Axis: false
223+
Name: Current View
224+
Near Clip Distance: 0.009999999776482582
225+
Pitch: 0.630398154258728
226+
Target Frame: <Fixed Frame>
227+
Value: Orbit (rviz)
228+
Yaw: 1.7203983068466187
229+
Saved: ~
230+
Window Geometry:
231+
Displays:
232+
collapsed: true
233+
Height: 1136
234+
Hide Left Dock: true
235+
Hide Right Dock: true
236+
QMainWindow State: 000000ff00000000fd00000004000000000000028e00000416fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000416000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000039c0000041600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
237+
Selection:
238+
collapsed: false
239+
Tool Properties:
240+
collapsed: false
241+
Views:
242+
collapsed: true
243+
Width: 924
244+
X: 74
245+
Y: 27

‎config/quadrotor_tuning.yaml renamed to ‎px4_mpc/px4_mpc/config/quadrotor_tuning.yaml

File renamed without changes.

‎config/trajectory_quat.txt renamed to ‎px4_mpc/px4_mpc/config/trajectory_quat.txt

File renamed without changes.

‎px4_mpc/controllers/__init__.py renamed to ‎px4_mpc/px4_mpc/controllers/__init__.py

File renamed without changes.

‎px4_mpc/controllers/multirotor_rate_mpc.py renamed to ‎px4_mpc/px4_mpc/controllers/multirotor_rate_mpc.py

+7-7
Original file line numberDiff line numberDiff line change
@@ -66,9 +66,9 @@ def setup(self, x0, N_horizon, Tf):
6666
ocp.dims.N = N_horizon
6767

6868
# set cost
69-
Q_mat = 2*np.diag([1e1, 1e1, 1e1, 5e-2, 5e-2, 5e-2, 0.0, 0.1, 0.1, 0.1])
70-
Q_e = 2*np.diag([1e3, 1e3, 1e3, 1e1, 1e1, 1e1, 0.0, 0.0, 0.0, 0.0])
71-
R_mat = 2*np.diag([1e-1, 5e-2, 5e-2, 5e-2])
69+
Q_mat = 2*np.diag([1e1, 1e1, 1e1, 1e1, 1e1, 1e1, 0.0, 0.1, 0.1, 0.1])
70+
Q_e = 2*np.diag([3e2, 3e2, 3e2, 1e2, 1e2, 1e2, 0.0, 0.0, 0.0, 0.0])
71+
R_mat = 2*np.diag([1e1, 5e2, 5e2, 5e2])
7272

7373
# TODO: How do you add terminal costs?
7474

@@ -82,13 +82,13 @@ def setup(self, x0, N_horizon, Tf):
8282
ocp.cost.cost_type = 'NONLINEAR_LS'
8383
ocp.cost.cost_type_e = 'NONLINEAR_LS'
8484
ocp.cost.W = scipy.linalg.block_diag(Q_mat, R_mat)
85-
ocp.cost.W_e = Q_mat
85+
ocp.cost.W_e = scipy.linalg.block_diag(Q_e)
8686

8787

8888
ocp.model.cost_y_expr = cs.vertcat(model.x, model.u)
8989
ocp.model.cost_y_expr_e = model.x
90-
ocp.cost.yref = np.array([0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
91-
ocp.cost.yref_e = np.array([0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
90+
ocp.cost.yref = np.array([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.0])
91+
ocp.cost.yref_e = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
9292

9393
# set constraints
9494
ocp.constraints.lbu = np.array([0.0, -wmax, -wmax, -0.5*wmax])
@@ -151,4 +151,4 @@ def solve(self, x0, verbose=False):
151151
simU[i,:] = self.ocp_solver.get(i, "u")
152152
simX[N,:] = self.ocp_solver.get(N, "x")
153153

154-
return simU, simX
154+
return simU, simX

‎launch/mpc_quadrotor.launch.py renamed to ‎px4_mpc/px4_mpc/launch/mpc_quadrotor_launch.py

+9-1
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,14 @@ def generate_launch_description():
5151
output='screen',
5252
emulate_tty=True,
5353
),
54+
Node(
55+
package='px4_mpc',
56+
namespace='px4_mpc',
57+
executable='rviz_pos_marker',
58+
name='rviz_pos_marker',
59+
output='screen',
60+
emulate_tty=True,
61+
),
5462
# Node(
5563
# package='micro_ros_agent',
5664
# executable='micro_ros_agent',
@@ -68,6 +76,6 @@ def generate_launch_description():
6876
namespace='',
6977
executable='rviz2',
7078
name='rviz2',
71-
arguments=['-d', [os.path.join(get_package_share_directory('px4_offboard'), 'visualize.rviz')]]
79+
arguments=['-d', [os.path.join(get_package_share_directory('px4_mpc'), 'config.rviz')]]
7280
)
7381
])

‎px4_mpc/models/__init__.py renamed to ‎px4_mpc/px4_mpc/models/__init__.py

File renamed without changes.

0 commit comments

Comments
 (0)