diff --git a/README.md b/README.md index fe01b12..559f030 100644 --- a/README.md +++ b/README.md @@ -85,7 +85,7 @@ make px4_sitl_spacecraft gz_atmos On another terminal, run ```bash -ros2 launch px4_mpc mpc_spacecraft_launch.py mode:=wrench setpoint_from_rviz:=False +ros2 launch px4_mpc mpc_spacecraft_launch.py mode:=wrench use_rviz:=False ``` Then, in the same terminal where you started the PX4 SITL, you can control ATMOS using the following commands: @@ -104,11 +104,11 @@ At this point, the vehicle should start following the desired setpoints. The `mpc_spacecraft_launch.py` file includes optional arguments: - **mode**: Control mode (wrench by default). Options: wrench, rate, direct_allocation and _offset free_. For offset free MPC, please install the package [OpenMPC](https://github.com/mikaelj-kth-se/OpenMPC). - **namespace**: Spacecraft namespace ('' by default). -- **setpoint_from_rviz**: Use RViz for setpoints (True by default). +- **use_rviz**: Use RViz for setpoints (True by default). **Example with no namespace:** ```bash -ros2 launch px4_mpc mpc_spacecraft_launch.py mode:=wrench setpoint_from_rviz:=False +ros2 launch px4_mpc mpc_spacecraft_launch.py mode:=wrench use_rviz:=False ``` **Example with namespace:** diff --git a/mpc_msgs/CMakeLists.txt b/mpc_msgs/CMakeLists.txt index 3e756ad..8cc6d08 100644 --- a/mpc_msgs/CMakeLists.txt +++ b/mpc_msgs/CMakeLists.txt @@ -1,6 +1,10 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.10) project(mpc_msgs) +if(POLICY CMP0148) + cmake_policy(SET CMP0148 OLD) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() diff --git a/px4_mpc/px4_mpc/config/config.rviz b/px4_mpc/px4_mpc/config/config.rviz index c120f23..2876370 100644 --- a/px4_mpc/px4_mpc/config/config.rviz +++ b/px4_mpc/px4_mpc/config/config.rviz @@ -10,8 +10,8 @@ Panels: - /Path1 - /Path2 - /InteractiveMarkers1 - - /Marker1 - - /Marker1/Topic1 + - /Setpoint1 + - /Setpoint1/Topic1 Splitter Ratio: 0.5 Tree Height: 895 - Class: rviz_common/Selection @@ -48,9 +48,9 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 + - Alpha: 0.7 + Axes Length: 0.3 + Axes Radius: 0.03 Class: rviz_default_plugins/Pose Color: 255; 25; 0 Enabled: true @@ -68,7 +68,7 @@ Visualization Manager: Reliability Policy: Reliable Value: __NS__/px4_visualizer/vehicle_pose Value: true - - Alpha: 1 + - Alpha: 0.7 Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 255; 0 @@ -76,8 +76,8 @@ Visualization Manager: Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.02 Name: Path Offset: X: 0 @@ -96,7 +96,7 @@ Visualization Manager: Reliability Policy: Reliable Value: __NS__/px4_visualizer/vehicle_path Value: true - - Alpha: 1 + - Alpha: 0.7 Buffer Length: 1 Class: rviz_default_plugins/Path Color: 25; 0; 255 @@ -104,8 +104,8 @@ Visualization Manager: Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.02 Name: Path Offset: X: 0 @@ -133,11 +133,18 @@ Visualization Manager: Show Descriptions: true Show Visual Aids: false Value: true - - Class: rviz_default_plugins/Marker + - Alpha: 1 + Axes Length: 0.25 + Axes Radius: 0.02 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 Enabled: true - Name: Marker - Namespaces: - arrow: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Setpoint + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes Topic: Depth: 5 Durability Policy: Volatile @@ -146,6 +153,27 @@ Visualization Manager: Reliability Policy: Reliable Value: __NS__/px4_mpc/reference Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.15 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/color/image_raw + Value: true + Visibility: + Grid: true + InteractiveMarkers: true + Setpoint: true + Path: true + Pose: true + Value: true + Zoom Factor: 1 Enabled: true Global Options: Background Color: 255; 255; 255 @@ -213,18 +241,20 @@ Visualization Manager: Yaw: 3.785431385040283 Saved: ~ Window Geometry: + Camera: + collapsed: true Displays: collapsed: false - Height: 1131 + Height: 1043 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000028e0000040dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000040d000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000001080000040d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000334000003b5fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000003b5000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000327000000cd0000000000000000fb0000000c00430061006d00650072006103000002100000061500000334000001e0000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000446000003b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 924 - X: 74 - Y: 403 + Width: 1920 + X: 0 + Y: 0 diff --git a/px4_mpc/px4_mpc/config/config_mod.rviz b/px4_mpc/px4_mpc/config/config_mod.rviz new file mode 100644 index 0000000..1564d98 --- /dev/null +++ b/px4_mpc/px4_mpc/config/config_mod.rviz @@ -0,0 +1,255 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Pose1 + - /Path1 + - /Path2 + - /InteractiveMarkers1 + - /Marker1 + - /Marker1/Topic1 + - /Camera1 + - /Camera1/Visibility1 + Splitter Ratio: 0.5 + Tree Height: 815 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Axes Length: 0.30000001192092896 + Axes Radius: 0.029999999329447746 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pop/px4_visualizer/vehicle_pose + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.019999999552965164 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pop/px4_visualizer/vehicle_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.019999999552965164 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pop/px4_mpc/predicted_path + Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /pop/rviz_target_pose_marker + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + arrow: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pop/px4_mpc/reference + Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.15000000596046448 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/camera/color/image_raw + Value: true + Visibility: + Grid: true + InteractiveMarkers: true + Marker: true + Path: true + Pose: true + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 11.216394424438477 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9703977108001709 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.1954169273376465 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000334000003b5fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000003b5000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000327000000cd0000000000000000fb0000000c00430061006d00650072006103000002100000061500000334000001e0000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000446000003b500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 0 diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py index 5a50e2d..84ec204 100644 --- a/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py +++ b/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py @@ -35,12 +35,13 @@ import numpy as np import casadi as cs import os +from px4_mpc.utils.rotations import quat_error_v_cs class SpacecraftDirectAllocationMPC(): def __init__(self, model): self.model = model self.Tf = 5.0 - self.N = 49 + self.N = 29 self.x0 = 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]) @@ -73,11 +74,11 @@ def setup(self, x0, N_horizon, Tf): ocp.solver_options.N_horizon = N_horizon # set cost - Q_mat = [5e1, 5e1, 5e1, - 1e1, 1e1, 1e1, - 8e3, - 1e1, 1e1, 1e1] - R_mat = [1e1] * 4 + Q_mat = [1e0, 1e0, 1e0, + 2e0, 2e0, 2e0, + 5e1, 5e1, 5e1, + 5e0, 5e0, 5e0] + R_mat = [5e-1] * 4 ocp.cost.W_0 = np.diag(Q_mat + R_mat) ocp.cost.W = np.diag(Q_mat + R_mat) @@ -93,9 +94,11 @@ def setup(self, x0, N_horizon, Tf): x = ocp.model.x u = ocp.model.u + q_error_v = quat_error_v_cs(x[6:10], x_ref[6:10]) + x_error = x[0:3] - x_ref[0:3] x_error = cs.vertcat(x_error, x[3:6] - x_ref[3:6]) - x_error = cs.vertcat(x_error, 1 - (x[6:10].T @ x_ref[6:10])**2) + x_error = cs.vertcat(x_error, q_error_v) x_error = cs.vertcat(x_error, x[10:13] - x_ref[10:13]) u_error = u - u_ref @@ -108,7 +111,6 @@ def setup(self, x0, N_horizon, Tf): ocp.model.cost_y_expr_0 = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) - ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr_e = x_error ocp.cost.yref_0 = np.zeros(ocp.model.cost_y_expr_0.shape[0]) @@ -125,14 +127,14 @@ def setup(self, x0, N_horizon, Tf): ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # set constraints on X - ocp.constraints.lbx = np.array([-5, -5, -5, -1, -1, -1, -1, -1, -1]) - ocp.constraints.ubx = np.array([+5, +5, +5, +1, +1, +1, +1, +1, +1]) + ocp.constraints.lbx = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5, -1, -1, -1]) + ocp.constraints.ubx = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5, +1, +1, +1]) ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 10, 11, 12]) # set constraints on X at the end of the horizon - ocp.constraints.lbx_e = np.array([-5, -5, -5, -1, -1, -1, -1, -1, -1]) - ocp.constraints.ubx_e = np.array([+5, +5, +5, +1, +1, +1, +1, +1, +1]) - ocp.constraints.idxbx_e = np.array([0, 1, 2, 3, 4, 5, 10, 11, 12]) + ocp.constraints.lbx_e = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5, -1, -1, -1]) + ocp.constraints.ubx_e = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5, +1, +1, +1]) + ocp.constraints.idxbx_e = ocp.constraints.idxbx # To constrain quaternion states, add indices 6–9 to idxbx/idxbx_e and set their bounds in lbx/ubx. # Usually not needed. Valid quaternions stay in [-1, 1], and drift is better fixed by renormalising. diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_propeller_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_propeller_mpc.py new file mode 100644 index 0000000..aba8160 --- /dev/null +++ b/px4_mpc/px4_mpc/controllers/spacecraft_propeller_mpc.py @@ -0,0 +1,235 @@ +############################################################################ +# +# Copyright (C) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver +import numpy as np +import casadi as cs +import os +from px4_mpc.utils.rotations import quat_error_v_cs + +class SpacecraftPropellerMPC(): + def __init__(self, model): + self.model = model + self.Tf = 3.0 + self.N = 29 + + self.x0 = 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]) + + self.ocp_solver, self.integrator = self.setup(self.x0, self.N, self.Tf) + + def setup(self, x0, N_horizon, Tf): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # Set directory for code generation and json file + this_file_dir = os.path.dirname(os.path.abspath(__file__)) + package_root = os.path.abspath(os.path.join(this_file_dir, '..')) + codegen_dir = os.path.join(package_root, 'mpc_codegen') + json_path = os.path.join(codegen_dir, 'acados_ocp.json') + os.makedirs(codegen_dir, exist_ok=True) + ocp.code_export_directory = codegen_dir + + # set model + model = self.model.get_acados_model() + Fmax = self.model.max_thrust + + ocp.model = model + + nx = model.x.size()[0] + nu = model.u.size()[0] + print(f"nx: {nx}, nu: {nu}") + + # set dimensions + ocp.dims.N = N_horizon + ocp.solver_options.N_horizon = N_horizon + + # set cost + Q_mat = [1e0, 1e0, 1e0, + 1e1, 1e1, 1e1, + 5e0, 5e0, 5e0, + 2e1, 2e1, 2e1] + R_mat = [5e-1] * 4 + + ocp.cost.W_0 = np.diag(Q_mat + R_mat) + ocp.cost.W = np.diag(Q_mat + R_mat) + ocp.cost.W_e = 10 * np.diag(Q_mat) + + # References: + x_ref = cs.MX.sym('x_ref', (13, 1)) + u_ref = cs.MX.sym('u_ref', (4, 1)) + + # Calculate errors + # x : p,v,q,w , R9 x SO(3) + # u : Thruster pairs (0&1, 2&3, 4&5, 6&7) , R4 + x = ocp.model.x + u = ocp.model.u + + q_error_v = quat_error_v_cs(x[6:10], x_ref[6:10]) + + x_error = x[0:3] - x_ref[0:3] + x_error = cs.vertcat(x_error, x[3:6] - x_ref[3:6]) + x_error = cs.vertcat(x_error, q_error_v) + x_error = cs.vertcat(x_error, x[10:13] - x_ref[10:13]) + u_error = u - u_ref + + ocp.model.p = cs.vertcat(x_ref, u_ref) + + # define cost with parametric reference + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.cost.cost_type_e = 'NONLINEAR_LS' + ocp.cost.cost_type_0 = 'NONLINEAR_LS' + + ocp.model.cost_y_expr_0 = cs.vertcat(x_error, u_error) + ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) + ocp.model.cost_y_expr_e = x_error + + ocp.cost.yref_0 = np.zeros(ocp.model.cost_y_expr_0.shape[0]) + ocp.cost.yref = np.zeros(ocp.model.cost_y_expr.shape[0]) + ocp.cost.yref_e = np.zeros(ocp.model.cost_y_expr_e.shape[0]) + + # Initialize parameters + p_0 = np.concatenate((x0, np.zeros(nu))) # First step is error 0 since x_ref = x0 + ocp.parameter_values = p_0 + + # set constraints on U + ocp.constraints.lbu = np.array([-Fmax, -Fmax, -Fmax, -Fmax]) + ocp.constraints.ubu = np.array([+Fmax, +Fmax, +Fmax, +Fmax]) + ocp.constraints.idxbu = np.array([0, 1, 2, 3]) + + # set constraints on X + ocp.constraints.lbx = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5, -1, -1, -1]) + ocp.constraints.ubx = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5, +1, +1, +1]) + ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 10, 11, 12]) + + # set constraints on X at the end of the horizon + ocp.constraints.lbx_e = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5, -1, -1, -1]) + ocp.constraints.ubx_e = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5, +1, +1, +1]) + ocp.constraints.idxbx_e = ocp.constraints.idxbx + + # To constrain quaternion states, add indices 6–9 to idxbx/idxbx_e and set their bounds in lbx/ubx. + # Usually not needed. Valid quaternions stay in [-1, 1], and drift is better fixed by renormalising. + + # Soft constraints are turned on by setting weights for slack variables + # TODO: This should be configured by config file + use_soft_constraints = True + if use_soft_constraints: + # set weights slack variables for X constraints + ocp.constraints.idxsbx = np.arange(len(ocp.constraints.idxbx)) + ocp.cost.Zl = np.array([1e6]*len(ocp.constraints.idxsbx)) + ocp.cost.Zu = np.array([1e6]*len(ocp.constraints.idxsbx)) + ocp.cost.zl = np.array([0.0]*len(ocp.constraints.idxsbx)) + ocp.cost.zu = np.array([0.0]*len(ocp.constraints.idxsbx)) + + # set weights slack variables for X_e constraints + ocp.constraints.idxsbx_e = np.arange(len(ocp.constraints.idxbx_e)) + ocp.cost.Zl_e = np.array([1e6]*len(ocp.constraints.idxsbx_e)) + ocp.cost.Zu_e = np.array([1e6]*len(ocp.constraints.idxsbx_e)) + ocp.cost.zl_e = np.array([0.0]*len(ocp.constraints.idxsbx_e)) + ocp.cost.zu_e = np.array([0.0]*len(ocp.constraints.idxsbx_e)) + + # set initial state + ocp.constraints.x0 = x0 + + # set options + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' + # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, + # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT' + ocp.solver_options.integrator_type = 'ERK' + + # ocp.solver_options.print_level = 1 + use_RTI=True + if use_RTI: + ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP + ocp.solver_options.sim_method_num_stages = 4 + ocp.solver_options.sim_method_num_steps = 3 + else: + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + + # ocp.solver_options.qp_solver_cond_N = N_horizon + # ocp.solver_options.print_level = 6 + + # set prediction horizon + ocp.solver_options.tf = Tf + + ocp_solver = AcadosOcpSolver(ocp, json_file=json_path) + # create an integrator with the same settings as used in the OCP solver. + acados_integrator = AcadosSimSolver(ocp, json_file=json_path) + + return ocp_solver, acados_integrator + + def solve(self, x0, verbose=False, ref=None): + + # preparation phase + ocp_solver = self.ocp_solver + + # Set reference, create zero reference + if ref is None: + zero_ref = np.zeros(self.model.get_acados_model().x.size()[0] + self.model.get_acados_model().u.size()[0]) + zero_ref[6] = 1.0 + + for i in range(self.N+1): + if ref is not None: + # Assumed ref structure: (nx+nu) x N+1 + # NOTE: last u_ref is not used + p_i = ref[:, i] + ocp_solver.set(i, "p", p_i) + else: + # set all references to 0 + ocp_solver.set(i, "p", zero_ref) + + # set initial state + ocp_solver.set(0, "lbx", x0.flatten()) + ocp_solver.set(0, "ubx", x0.flatten()) + + status = ocp_solver.solve() + if verbose: + self.ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") + + if status != 0: + raise Exception(f'acados returned status {status}.') + + N = self.N + nx = self.model.get_acados_model().x.size()[0] + nu = self.model.get_acados_model().u.size()[0] + + simX = np.ndarray((N+1, nx)) + simU = np.ndarray((N, nu)) + + # get solution + for i in range(N): + simX[i,:] = self.ocp_solver.get(i, "x") + simU[i,:] = self.ocp_solver.get(i, "u") + simX[N,:] = self.ocp_solver.get(N, "x") + + return simU, simX diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py index d74edde..00efd6d 100644 --- a/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py +++ b/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py @@ -35,12 +35,13 @@ import numpy as np import casadi as cs import os +from px4_mpc.utils.rotations import quat_mult_cs class SpacecraftRateMPC(): def __init__(self, model): self.model = model - self.Tf = 5.0 + self.Tf = 10.0 self.N = 49 self.x0 = np.array([0.01, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0]) @@ -76,8 +77,11 @@ def setup(self, x0, N_horizon, Tf): ocp.solver_options.N_horizon = N_horizon # set cost - Q_mat = [8e2, 8e2, 8e2, 7e1, 7e1, 7e1, 8e4] - R_mat = [2e1, 2e1, 2e1, 1e2, 1e2, 1e2] + Q_mat = [1e0, 1e0, 1e0, + 2e0, 2e0, 2e0, + 1e2, 5e1, 5e1, 5e1] + R_mat = [5e-1, 5e-1, 5e-1, + 2e1, 2e1, 2e1] ocp.cost.W_0 = np.diag(Q_mat + R_mat) ocp.cost.W = np.diag(Q_mat + R_mat) @@ -88,14 +92,20 @@ def setup(self, x0, N_horizon, Tf): u_ref = cs.MX.sym('u_ref', (6, 1)) # Calculate errors - # x : p,v,q,w , R9 x SO(3) + # x : p,v,q , R6 x SO(3) # u : Fx,Fy,Fz,wx,wy,wz , R6 x = ocp.model.x u = ocp.model.u + q = x_ref[6:10] + q_ref = x[6:10] + q = q / cs.norm_2(q) + q_error = quat_mult_cs(q, cs.vertcat(q_ref[0], -q_ref[1], -q_ref[2], -q_ref[3])) + q_error = q_error * cs.sign(q_error[0]) + x_error = x[0:3] - x_ref[0:3] x_error = cs.vertcat(x_error, x[3:6] - x_ref[3:6]) - x_error = cs.vertcat(x_error, 1 - (x[6:10].T @ x_ref[6:10])**2) + x_error = cs.vertcat(x_error, q_error) u_error = u - u_ref # Control error ocp.model.p = cs.vertcat(x_ref, u_ref) @@ -107,31 +117,33 @@ def setup(self, x0, N_horizon, Tf): ocp.model.cost_y_expr_0 = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) - ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr_e = x_error ocp.cost.yref_0 = np.zeros(ocp.model.cost_y_expr_0.shape[0]) + ocp.cost.yref_0[6] = 1 ocp.cost.yref = np.zeros(ocp.model.cost_y_expr.shape[0]) + ocp.cost.yref[6] = 1 ocp.cost.yref_e = np.zeros(ocp.model.cost_y_expr_e.shape[0]) + ocp.cost.yref_e[6] = 1 # Initialize parameters p_0 = np.concatenate((x0, np.zeros(nu))) # First step is error 0 since x_ref = x0 ocp.parameter_values = p_0 # set constraints on U - ocp.constraints.lbu = np.array([-Fmax, -Fmax, -Fmax, -wmax, -wmax, -wmax]) - ocp.constraints.ubu = np.array([+Fmax, +Fmax, +Fmax, wmax, wmax, wmax]) + ocp.constraints.lbu = np.array([-Fmax, -Fmax, 0, 0, 0, -wmax]) + ocp.constraints.ubu = np.array([+Fmax, +Fmax, 0, 0, 0, wmax]) ocp.constraints.idxbu = np.array([0, 1, 2, 3, 4, 5]) # set constraints on X - ocp.constraints.lbx = np.array([-5, -5, -5, -1, -1, -1]) - ocp.constraints.ubx = np.array([+5, +5, +5, +1, +1, +1]) + ocp.constraints.lbx = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5]) + ocp.constraints.ubx = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5]) ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5]) # set constraints on X at the end of the horizon - ocp.constraints.lbx_e = np.array([-5, -5, -5, -1, -1, -1]) - ocp.constraints.ubx_e = np.array([+5, +5, +5, +1, +1, +1]) - ocp.constraints.idxbx_e = np.array([0, 1, 2, 3, 4, 5]) + ocp.constraints.lbx_e = np.array([0.3, -1.28, -5, -0.5, -0.5, -0.5]) + ocp.constraints.ubx_e = np.array([+3.8, +1.44, +5, +0.5, +0.5, +0.5]) + ocp.constraints.idxbx_e = ocp.constraints.idxbx # To constrain quaternion states, add indices 6–9 to idxbx/idxbx_e and set their bounds in lbx/ubx. # Usually not needed. Valid quaternions stay in [-1, 1], and drift is better fixed by renormalising. diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py index 5503772..1eb03f4 100644 --- a/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py +++ b/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py @@ -35,12 +35,13 @@ import numpy as np import casadi as cs import os +from px4_mpc.utils.rotations import quat_error_v_cs class SpacecraftWrenchMPC(): def __init__(self, model): self.model = model self.Tf = 5.0 - self.N = 49 + self.N = 29 self.x0 = np.array([0.01, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) @@ -73,11 +74,12 @@ def setup(self, x0, N_horizon, Tf): ocp.solver_options.N_horizon = N_horizon # set cost - Q_mat = [8e2, 8e2, 8e2, - 7e1, 7e1, 7e1, - 8e4, + Q_mat = [1e0, 1e0, 1e0, + 1e1, 1e1, 1e1, + 1e1, 1e1, 1e1, + 1e1, 1e1, 1e1] + R_mat = [5e-1, 5e-1, 5e-1, 1e1, 1e1, 1e1] - R_mat = [2e1, 2e1, 2e1, 20e1, 20e1, 20e1] ocp.cost.W_0 = np.diag(Q_mat + R_mat) ocp.cost.W = np.diag(Q_mat + R_mat) @@ -93,9 +95,11 @@ def setup(self, x0, N_horizon, Tf): x = ocp.model.x u = ocp.model.u + q_error_v = quat_error_v_cs(x[6:10], x_ref[6:10]) + x_error = x[0:3] - x_ref[0:3] x_error = cs.vertcat(x_error, x[3:6] - x_ref[3:6]) - x_error = cs.vertcat(x_error, 1 - (x[6:10].T @ x_ref[6:10])**2) + x_error = cs.vertcat(x_error, q_error_v) x_error = cs.vertcat(x_error, x[10:13] - x_ref[10:13]) u_error = u - u_ref @@ -108,7 +112,6 @@ def setup(self, x0, N_horizon, Tf): ocp.model.cost_y_expr_0 = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) - ocp.model.cost_y_expr = cs.vertcat(x_error, u_error) ocp.model.cost_y_expr_e = x_error ocp.cost.yref_0 = np.zeros(ocp.model.cost_y_expr_0.shape[0]) @@ -120,19 +123,19 @@ def setup(self, x0, N_horizon, Tf): ocp.parameter_values = p_0 # set constraints on U - ocp.constraints.lbu = np.array([-Fmax, -Fmax, -Fmax, -Tmax, -Tmax, -Tmax]) - ocp.constraints.ubu = np.array([+Fmax, +Fmax, +Fmax, +Tmax, +Tmax, +Tmax]) + ocp.constraints.lbu = np.array([-Fmax, -Fmax, -0, -0, -0, -Tmax]) + ocp.constraints.ubu = np.array([+Fmax, +Fmax, +0, +0, +0, +Tmax]) ocp.constraints.idxbu = np.array([0, 1, 2, 3, 4, 5]) # set constraints on X - ocp.constraints.lbx = np.array([-5, -5, -5, -1, -1, -1, -1, -1, -1]) - ocp.constraints.ubx = np.array([+5, +5, +5, +1, +1, +1, +1, +1, +1]) - ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 10, 11, 12]) + ocp.constraints.lbx = np.array([0.3, -1.28, -0.5, -0.5, -1]) + ocp.constraints.ubx = np.array([+3.8, +1.44, +0.5, +0.5, +1]) + ocp.constraints.idxbx = np.array([0, 1, 3, 4, 12]) # set constraints on X at the end of the horizon - ocp.constraints.lbx_e = np.array([-5, -5, -5, -1, -1, -1, -1, -1, -1]) - ocp.constraints.ubx_e = np.array([+5, +5, +5, +1, +1, +1, +1, +1, +1]) - ocp.constraints.idxbx_e = np.array([0, 1, 2, 3, 4, 5, 10, 11, 12]) + ocp.constraints.lbx_e = np.array([0.3, -1.28, -0.5, -0.5, -1]) + ocp.constraints.ubx_e = np.array([+3.8, +1.44, +0.5, +0.5, +1]) + ocp.constraints.idxbx_e = ocp.constraints.idxbx # To constrain quaternion states, add indices 6–9 to idxbx/idxbx_e and set their bounds in lbx/ubx. # Usually not needed. Valid quaternions stay in [-1, 1], and drift is better fixed by renormalising. diff --git a/px4_mpc/px4_mpc/launch/mpc_quadrotor_launch.py b/px4_mpc/px4_mpc/launch/mpc_quadrotor_launch.py index a1f06c5..a7fa394 100644 --- a/px4_mpc/px4_mpc/launch/mpc_quadrotor_launch.py +++ b/px4_mpc/px4_mpc/launch/mpc_quadrotor_launch.py @@ -52,18 +52,18 @@ def generate_launch_description(): description='Namespace for all nodes' ) - setpoint_from_rviz_arg = DeclareLaunchArgument( - 'setpoint_from_rviz', + use_rviz_arg = DeclareLaunchArgument( + 'use_rviz', default_value='true', description='Publish setpoint pose via rviz' ) namespace = LaunchConfiguration('namespace') - setpoint_from_rviz = LaunchConfiguration('setpoint_from_rviz') + use_rviz = LaunchConfiguration('use_rviz') return LaunchDescription([ namespace_arg, - setpoint_from_rviz_arg, + use_rviz_arg, Node( package='px4_mpc', namespace=namespace, @@ -73,7 +73,7 @@ def generate_launch_description(): emulate_tty=True, parameters=[ {'namespace': namespace}, - {'setpoint_from_rviz': setpoint_from_rviz} + {'use_rviz': use_rviz} ] ), Node( @@ -84,7 +84,7 @@ def generate_launch_description(): output='screen', emulate_tty=True, parameters=[{'namespace': namespace}], - condition=IfCondition(setpoint_from_rviz) + condition=IfCondition(use_rviz) ), # Node( # package='micro_ros_agent', @@ -98,7 +98,7 @@ def generate_launch_description(): executable='visualizer', name='visualizer', parameters=[{'namespace': namespace}], - condition=IfCondition(setpoint_from_rviz) + condition=IfCondition(use_rviz) ), OpaqueFunction(function=launch_setup), ]) @@ -136,6 +136,6 @@ def launch_setup(context, *args, **kwargs): executable='rviz2', name='rviz2', arguments=['-d', patched_config], - condition=IfCondition(LaunchConfiguration('setpoint_from_rviz')) + condition=IfCondition(LaunchConfiguration('use_rviz')) ) ] diff --git a/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py b/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py index 5130cdf..f7e74c8 100644 --- a/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py +++ b/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py @@ -39,7 +39,9 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import PythonExpression from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from ament_index_python.packages import get_package_share_directory import os import tempfile @@ -58,59 +60,106 @@ def generate_launch_description(): description='Namespace for all nodes' ) - setpoint_from_rviz_arg = DeclareLaunchArgument( - 'setpoint_from_rviz', + rviz_mode_arg = DeclareLaunchArgument( + 'rviz_mode', + default_value='setpoint', + description='RViz and setpoint mode (off, viz, setpoint)' + ) + px4_uses_ned_arg = DeclareLaunchArgument( + 'px4_uses_ned', default_value='true', - description='Publish setpoint pose via rviz' + description='PX4 uses NED frame (default: true) or ENU frame (false)' + ) + camera_arg = DeclareLaunchArgument( + 'camera', + default_value='false', + description='Enable camera' ) mode = LaunchConfiguration('mode') namespace = LaunchConfiguration('namespace') - setpoint_from_rviz = LaunchConfiguration('setpoint_from_rviz') + rviz_mode = LaunchConfiguration('rviz_mode') + px4_uses_ned = LaunchConfiguration('px4_uses_ned') + camera = LaunchConfiguration('camera') - return LaunchDescription([ - mode_arg, - namespace_arg, - setpoint_from_rviz_arg, - Node( - package='px4_mpc', - namespace=namespace, - executable='mpc_spacecraft', - name='mpc_spacecraft', - output='screen', - emulate_tty=True, - parameters=[ - {'mode': mode}, - {'setpoint_from_rviz': setpoint_from_rviz} - ] - ), - Node( - package='px4_mpc', - namespace=namespace, - executable='rviz_pos_marker', - name='rviz_pos_marker', - output='screen', - emulate_tty=True, - condition=IfCondition(setpoint_from_rviz) - ), - Node( - package='px4_mpc', - namespace=namespace, - executable='test_setpoints', - name='test_setpoints', - output='screen', - emulate_tty=True, - condition=UnlessCondition(setpoint_from_rviz) - ), - Node( - package='px4_offboard', - namespace=namespace, - executable='visualizer', - name='visualizer', - condition=IfCondition(setpoint_from_rviz) - ), - OpaqueFunction(function=launch_setup), - ]) + rviz_enabled = PythonExpression(["'", rviz_mode, "' in ['viz', 'setpoint']"]) + rviz_setpoint_mode = PythonExpression(["'", rviz_mode, "' == 'setpoint'"]) + + ld = LaunchDescription() + + ld.add_action(mode_arg) + ld.add_action(namespace_arg) + ld.add_action(rviz_mode_arg) + ld.add_action(px4_uses_ned_arg) + ld.add_action(camera_arg) + + ld.add_action(Node( + package='px4_mpc', + namespace=namespace, + executable='mpc_spacecraft', + name='mpc_spacecraft', + output='screen', + emulate_tty=True, + parameters=[ + {'mode': mode}, + {'use_rviz': ParameterValue(rviz_setpoint_mode, value_type=bool)}, + {'px4_uses_ned': px4_uses_ned}, + {'camera': camera} + ] + )) + + ld.add_action(Node( + package='px4_mpc', + namespace=namespace, + executable='rviz_pos_marker', + name='rviz_pos_marker', + output='screen', + emulate_tty=True, + condition=IfCondition(rviz_setpoint_mode) + )) + + ld.add_action(Node( + package='px4_mpc', + namespace=namespace, + executable='test_setpoints', + name='test_setpoints', + output='screen', + emulate_tty=True, + condition=UnlessCondition(rviz_setpoint_mode) + )) + + ld.add_action(Node( + package='px4_mpc', + namespace=namespace, + executable='visualizer', + name='visualizer', + parameters=[ + {'px4_uses_ned': px4_uses_ned}, + {'camera': camera} + ], + condition=IfCondition(rviz_enabled) + )) + + ld.add_action(OpaqueFunction(function=launch_setup)) + + # ros2 launch realsense2_camera rs_launch.py publish_tf:=true + ld.add_action(Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_world_to_inertial', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'inertial'], + condition=IfCondition(camera) + )) + + ld.add_action(Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_world_to_camera', + arguments=['2', '1.9', '2.3', '0.3010647', '0.3013046', '-0.6395013', '0.6400107', 'map', 'camera_link'], # camera 2 + condition=IfCondition(camera) + )) + + return ld def patch_rviz_config(original_config_path, namespace): """ @@ -134,6 +183,16 @@ def launch_setup(context, *args, **kwargs): """ Function to set up the launch context and patch the RViz configuration. """ + rviz_mode = LaunchConfiguration('rviz_mode').perform(context) + valid_modes = {'off', 'viz', 'setpoint'} + if rviz_mode not in valid_modes: + raise ValueError( + f"Invalid rviz_mode '{rviz_mode}'. Expected one of: {sorted(valid_modes)}" + ) + + if rviz_mode == 'off': + return [] + namespace = LaunchConfiguration('namespace').perform(context) rviz_config_path = os.path.join(get_package_share_directory('px4_mpc'), 'config.rviz') patched_config = patch_rviz_config(rviz_config_path, namespace) @@ -145,6 +204,5 @@ def launch_setup(context, *args, **kwargs): executable='rviz2', name='rviz2', arguments=['-d', patched_config], - condition=IfCondition(LaunchConfiguration('setpoint_from_rviz')) ) ] \ No newline at end of file diff --git a/px4_mpc/px4_mpc/models/spacecraft_direct_allocation_model.py b/px4_mpc/px4_mpc/models/spacecraft_direct_allocation_model.py index bfb7d6c..14d1c8f 100644 --- a/px4_mpc/px4_mpc/models/spacecraft_direct_allocation_model.py +++ b/px4_mpc/px4_mpc/models/spacecraft_direct_allocation_model.py @@ -34,6 +34,7 @@ from acados_template import AcadosModel import casadi as cs import numpy as np +import px4_mpc.utils.rotations as R class SpacecraftDirectAllocationModel(): def __init__(self): @@ -46,27 +47,6 @@ def __init__(self): self.torque_arm_length = 0.12 def get_acados_model(self) -> AcadosModel: - def skew_symmetric(v): - return cs.vertcat(cs.horzcat(0, -v[0], -v[1], -v[2]), - cs.horzcat(v[0], 0, v[2], -v[1]), - cs.horzcat(v[1], -v[2], 0, v[0]), - cs.horzcat(v[2], v[1], -v[0], 0)) - - def q_to_rot_mat(q): - qw, qx, qy, qz = q[0], q[1], q[2], q[3] - - rot_mat = cs.vertcat( - cs.horzcat(1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qw * qz), 2 * (qx * qz + qw * qy)), - cs.horzcat(2 * (qx * qy + qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qw * qx)), - cs.horzcat(2 * (qx * qz - qw * qy), 2 * (qy * qz + qw * qx), 1 - 2 * (qx ** 2 + qy ** 2))) - - return rot_mat - - def v_dot_q(v, q): - rot_mat = q_to_rot_mat(q) - - return cs.mtimes(rot_mat, v) - model = AcadosModel() # set up states & controls @@ -106,12 +86,13 @@ def v_dot_q(v, q): xdot = cs.vertcat(p_dot, v_dot, q_dot, w_dot) - a_thrust = v_dot_q(F, q)/self.mass + q_normalized = q / cs.norm_2(q) + a_thrust = R.v_dot_q_cs(F, q_normalized)/self.mass # dynamics f_expl = cs.vertcat(v, a_thrust, - 1 / 2 * cs.mtimes(skew_symmetric(w), q), + 1 / 2 * cs.mtimes(R.skew_symmetric_cs(w), q_normalized), np.linalg.inv(self.inertia) @ (tau - cs.cross(w, self.inertia @ w)) ) diff --git a/px4_mpc/px4_mpc/models/spacecraft_propeller_model.py b/px4_mpc/px4_mpc/models/spacecraft_propeller_model.py new file mode 100644 index 0000000..bf6e50a --- /dev/null +++ b/px4_mpc/px4_mpc/models/spacecraft_propeller_model.py @@ -0,0 +1,121 @@ +############################################################################ +# +# Copyright (C) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +from acados_template import AcadosModel +import casadi as cs +import numpy as np +import px4_mpc.utils.rotations as R + +class SpacecraftPropellerModel(): + def __init__(self): + self.name = 'spacecraft_propeller_model' + + # constants + self.mass = 17.8 + self.inertia = np.diag([0.315]*3) + self.min_thrust = -1.5 + self.max_thrust = 1.5 + self.torque_arm_length = 0.105 + + def get_acados_model(self) -> AcadosModel: + model = AcadosModel() + + # set up states & controls + p = cs.MX.sym('p', 3) + v = cs.MX.sym('v', 3) + q = cs.MX.sym('q', 4) + w = cs.MX.sym('w', 3) + + x = cs.vertcat(p, v, q, w) + + u = cs.MX.sym('u', 4) + D_mat = cs.MX.zeros(2, 4) + D_mat[0, 0] = -1 + D_mat[0, 1] = 1 + D_mat[1, 2] = -1 + D_mat[1, 3] = 1 + + # L mat + L_mat = cs.MX.zeros(1, 4) + L_mat[0, 0] = 1 + L_mat[0, 1] = 1 + L_mat[0, 2] = 1 + L_mat[0, 3] = 1 + L_mat = L_mat * self.torque_arm_length + + F_2d = cs.mtimes(D_mat, u) + tau_1d = cs.mtimes(L_mat, u) + + F = cs.vertcat(F_2d[0, 0], F_2d[1, 0], 0.0) + tau = cs.vertcat(0.0, 0.0, tau_1d) + + # xdot + p_dot = cs.MX.sym('p_dot', 3) + v_dot = cs.MX.sym('v_dot', 3) + q_dot = cs.MX.sym('q_dot', 4) + w_dot = cs.MX.sym('w_dot', 3) + + xdot = cs.vertcat(p_dot, v_dot, q_dot, w_dot) + + q_normalized = q / cs.norm_2(q) + a_thrust = R.v_dot_q_cs(F, q_normalized)/self.mass + + # dynamics with planar constraints: v_z = 0, w_x = 0, w_y = 0 + # Enforce v[2] = 0, w[0] = 0, w[1] = 0 + v_planar = cs.vertcat(v[0], v[1], 0.0) + w_planar = cs.vertcat(0.0, 0.0, w[2]) + + # Only propagate planar velocities and yaw + a_thrust_planar = cs.vertcat(a_thrust[0], a_thrust[1], 0.0) + w_dot_planar = cs.vertcat(0.0, 0.0, (1/self.inertia[2,2]) * (tau[2] - 0.0)) + + # Quaternion derivative only for planar rotation (about z) + q_dot_planar = 1 / 2 * cs.mtimes(R.skew_symmetric_cs(w_planar), q_normalized) + + f_expl = cs.vertcat( + v_planar, # p_dot + a_thrust_planar, # v_dot + q_dot_planar, # q_dot + w_dot_planar # w_dot + ) + + f_impl = xdot - f_expl + + model.f_impl_expr = f_impl + model.f_expl_expr = f_expl + model.x = x + model.xdot = xdot + model.u = u + model.name = self.name + + return model diff --git a/px4_mpc/px4_mpc/models/spacecraft_rate_model.py b/px4_mpc/px4_mpc/models/spacecraft_rate_model.py index 08415c4..8d49d83 100644 --- a/px4_mpc/px4_mpc/models/spacecraft_rate_model.py +++ b/px4_mpc/px4_mpc/models/spacecraft_rate_model.py @@ -33,6 +33,7 @@ from acados_template import AcadosModel import casadi as cs +import px4_mpc.utils.rotations as R class SpacecraftRateModel(): def __init__(self): @@ -41,30 +42,9 @@ def __init__(self): # constants self.mass = 17.8 self.max_thrust = 2 * 1.5 - self.max_rate = 4 * 0.12 * 1.5 + self.max_rate = 1 def get_acados_model(self) -> AcadosModel: - def skew_symmetric(v): - return cs.vertcat(cs.horzcat(0, -v[0], -v[1], -v[2]), - cs.horzcat(v[0], 0, v[2], -v[1]), - cs.horzcat(v[1], -v[2], 0, v[0]), - cs.horzcat(v[2], v[1], -v[0], 0)) - - def q_to_rot_mat(q): - qw, qx, qy, qz = q[0], q[1], q[2], q[3] - - rot_mat = cs.vertcat( - cs.horzcat(1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qw * qz), 2 * (qx * qz + qw * qy)), - cs.horzcat(2 * (qx * qy + qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qw * qx)), - cs.horzcat(2 * (qx * qz - qw * qy), 2 * (qy * qz + qw * qx), 1 - 2 * (qx ** 2 + qy ** 2))) - - return rot_mat - - def v_dot_q(v, q): - rot_mat = q_to_rot_mat(q) - - return cs.mtimes(rot_mat, v) - model = AcadosModel() # set up states & controls @@ -85,12 +65,13 @@ def v_dot_q(v, q): xdot = cs.vertcat(p_dot, v_dot, q_dot) - a_thrust = v_dot_q(F, q)/self.mass + q_normalized = q / cs.norm_2(q) + a_thrust = R.v_dot_q_cs(F, q_normalized)/self.mass # dynamics f_expl = cs.vertcat(v, a_thrust, - 1 / 2 * cs.mtimes(skew_symmetric(w), q) + 1 / 2 * cs.mtimes(R.skew_symmetric_cs(w), q_normalized) ) f_impl = xdot - f_expl diff --git a/px4_mpc/px4_mpc/models/spacecraft_wrench_model.py b/px4_mpc/px4_mpc/models/spacecraft_wrench_model.py index 5a2ac0a..a8b8d4f 100644 --- a/px4_mpc/px4_mpc/models/spacecraft_wrench_model.py +++ b/px4_mpc/px4_mpc/models/spacecraft_wrench_model.py @@ -34,6 +34,7 @@ from acados_template import AcadosModel import casadi as ca import numpy as np +import px4_mpc.utils.rotations as R class SpacecraftWrenchModel(): def __init__(self): @@ -42,8 +43,8 @@ def __init__(self): # constants self.mass = 17.8 self.inertia = np.diag([0.315]*3) - self.max_thrust = 2 * 1.5 - self.max_torque = 4 * 0.12 * 1.5 + self.max_thrust = 2 * 1.5 * 2/3 + self.max_torque = 4 * 0.12 * 1.5 * 1/3 # set linearized symbolic matrices # functions: 12x12, 12x6 wrt error state @@ -52,27 +53,6 @@ def __init__(self): self.A, self.B = self.sym_linearization() def create_model(self): - def skew_symmetric(v): - return ca.vertcat(ca.horzcat(0, -v[0], -v[1], -v[2]), - ca.horzcat(v[0], 0, v[2], -v[1]), - ca.horzcat(v[1], -v[2], 0, v[0]), - ca.horzcat(v[2], v[1], -v[0], 0)) - - def q_to_rot_mat(q): - qw, qx, qy, qz = q[0], q[1], q[2], q[3] - - rot_mat = ca.vertcat( - ca.horzcat(1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qw * qz), 2 * (qx * qz + qw * qy)), - ca.horzcat(2 * (qx * qy + qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qw * qx)), - ca.horzcat(2 * (qx * qz - qw * qy), 2 * (qy * qz + qw * qx), 1 - 2 * (qx ** 2 + qy ** 2))) - - return rot_mat - - def v_dot_q(v, q): - rot_mat = q_to_rot_mat(q) - - return ca.mtimes(rot_mat, v) - # set up states & controls p = ca.MX.sym('p', 3) v = ca.MX.sym('v', 3) @@ -95,8 +75,8 @@ def v_dot_q(v, q): # dynamics self.f_expl = ca.vertcat(v, - v_dot_q(F, q) / self.mass, - 1.0 / 2 * ca.mtimes(skew_symmetric(w), q), + R.v_dot_q_cs(F, q) / self.mass, + 1.0 / 2 * ca.mtimes(R.skew_symmetric_cs(w), q), ca.inv(self.inertia) @ (tau - ca.cross(w, self.inertia @ w)) ) self.dynamics = ca.Function('f', [self.x, self.u], [self.f_expl]) diff --git a/px4_mpc/px4_mpc/mpc_quadrotor.py b/px4_mpc/px4_mpc/mpc_quadrotor.py index 21b1c34..6dda685 100644 --- a/px4_mpc/px4_mpc/mpc_quadrotor.py +++ b/px4_mpc/px4_mpc/mpc_quadrotor.py @@ -66,7 +66,7 @@ def __init__(self): self.namespace_prefix = f'/{self.namespace}' if self.namespace else '' # Get setpoint from rviz (true/false) - self.setpoint_from_rviz = self.declare_parameter('setpoint_from_rviz', False).value + self.use_rviz = self.declare_parameter('use_rviz', False).value # QoS profiles qos_profile_pub = QoSProfile( @@ -110,7 +110,7 @@ def __init__(self): self.vehicle_local_position_callback, qos_profile_sub) - if self.setpoint_from_rviz: + if self.use_rviz: self.set_pose_srv = self.create_service( SetPose, 'set_pose', diff --git a/px4_mpc/px4_mpc/mpc_spacecraft.py b/px4_mpc/px4_mpc/mpc_spacecraft.py index 963285f..ce081ac 100755 --- a/px4_mpc/px4_mpc/mpc_spacecraft.py +++ b/px4_mpc/px4_mpc/mpc_spacecraft.py @@ -42,9 +42,9 @@ from rclpy.clock import Clock from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy +from std_msgs.msg import Float32MultiArray from nav_msgs.msg import Path, Odometry from geometry_msgs.msg import PoseStamped, Vector3Stamped -from visualization_msgs.msg import Marker from px4_msgs.msg import OffboardControlMode from px4_msgs.msg import VehicleStatus @@ -69,29 +69,27 @@ def __init__(self): # Get mode; rate, wrench, direct_allocation self.mode = self.declare_parameter('mode', 'wrench').value self.sitl = self.declare_parameter('sitl', False).value + self.use_ned = self.declare_parameter('px4_uses_ned', True).value # Get setpoint from rviz (true/false) - self.setpoint_from_rviz = self.declare_parameter('setpoint_from_rviz', False).value + self.use_rviz = self.declare_parameter('use_rviz', False).value - # QoS profiles - qos_profile_pub = QoSProfile( - reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - history=QoSHistoryPolicy.KEEP_LAST, - depth=0 - ) + # Camera mode (true/false) + self.camera = self.declare_parameter('camera', False).value - qos_profile_sub = QoSProfile( + # QoS profile + qos_profile = QoSProfile( reliability=QoSReliabilityPolicy.BEST_EFFORT, - durability=QoSDurabilityPolicy.VOLATILE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, history=QoSHistoryPolicy.KEEP_LAST, - depth=0 + depth=1 ) # Setup publishers and subscribers - self.set_publishers_subscribers(qos_profile_pub, qos_profile_sub) + self.set_publishers_subscribers(qos_profile) timer_period = 0.1 # seconds + timer_period = 0.05 if self.mode == 'propeller' else timer_period self.timer = self.create_timer(timer_period, self.cmdloop_callback) self.nav_state = VehicleStatus.NAVIGATION_STATE_MAX @@ -119,11 +117,15 @@ def __init__(self): from px4_mpc.controllers.spacecraft_direct_allocation_mpc import SpacecraftDirectAllocationMPC self.model = SpacecraftDirectAllocationModel() self.mpc = SpacecraftDirectAllocationMPC(self.model) + elif self.mode == 'propeller': + from px4_mpc.models.spacecraft_propeller_model import SpacecraftPropellerModel + from px4_mpc.controllers.spacecraft_propeller_mpc import SpacecraftPropellerMPC + self.model = SpacecraftPropellerModel() + self.mpc = SpacecraftPropellerMPC(self.model) self.vehicle_attitude = np.array([1.0, 0.0, 0.0, 0.0]) self.vehicle_local_position = np.array([0.0, 0.0, 0.0]) self.vehicle_angular_velocity = np.array([0.0, 0.0, 0.0]) - self.vehicle_angular_velocity = np.array([0.0, 0.0, 0.0]) self.vehicle_local_velocity = np.array([0.0, 0.0, 0.0]) self.setpoint_position = np.array([1.0, 0.0, 0.0]) self.setpoint_attitude = np.array([1.0, 0.0, 0.0, 0.0]) @@ -134,46 +136,35 @@ def __init__(self): self.vehicle_angular_velocity_timestamp = -np.inf self.vehicle_status_timestamp = -np.inf - def set_publishers_subscribers(self, qos_profile_pub, qos_profile_sub): + def set_publishers_subscribers(self, qos_profile): # Subscribe to both using the same callback # - depending on PX4 version, one or the other will be used, but not both - self.status_sub_v1 = self.create_subscription( - VehicleStatus, - 'fmu/out/vehicle_status_v1', - self.vehicle_status_callback, - qos_profile_sub) self.status_sub = self.create_subscription( VehicleStatus, - 'fmu/out/vehicle_status', + 'fmu/out/vehicle_status_v4', self.vehicle_status_callback, - qos_profile_sub) - + qos_profile) self.attitude_sub = self.create_subscription( VehicleAttitude, 'fmu/out/vehicle_attitude', self.vehicle_attitude_callback, - qos_profile_sub) + qos_profile) self.angular_vel_sub = self.create_subscription( VehicleAngularVelocity, 'fmu/out/vehicle_angular_velocity', self.vehicle_angular_velocity_callback, - qos_profile_sub) - self.local_position_sub = self.create_subscription( - VehicleLocalPosition, - 'fmu/out/vehicle_local_position', - self.vehicle_local_position_callback, - qos_profile_sub) + qos_profile) self.local_position_sub = self.create_subscription( VehicleLocalPosition, 'fmu/out/vehicle_local_position_v1', self.vehicle_local_position_callback, - qos_profile_sub) + qos_profile) - if self.setpoint_from_rviz: + if self.use_rviz: self.set_pose_srv = self.create_service( SetPose, 'set_pose', - self.add_set_pos_callback + self.add_set_pose_callback ) else: self.setpoint_pose_sub = self.create_subscription( @@ -186,113 +177,109 @@ def set_publishers_subscribers(self, qos_profile_pub, qos_profile_sub): self.publisher_offboard_mode = self.create_publisher( OffboardControlMode, 'fmu/in/offboard_control_mode', - qos_profile_pub) + qos_profile) self.publisher_rates_setpoint = self.create_publisher( VehicleRatesSetpoint, 'fmu/in/vehicle_rates_setpoint', - qos_profile_pub) + qos_profile) self.publisher_direct_actuator = self.create_publisher( ActuatorMotors, 'fmu/in/actuator_motors', - qos_profile_pub) + qos_profile) self.publisher_thrust_setpoint = self.create_publisher( VehicleThrustSetpoint, 'fmu/in/vehicle_thrust_setpoint', - qos_profile_pub) + qos_profile) self.publisher_torque_setpoint = self.create_publisher( VehicleTorqueSetpoint, 'fmu/in/vehicle_torque_setpoint', - qos_profile_pub) + qos_profile) + self.publisher_propeller_setpoint = self.create_publisher( + Float32MultiArray, + 'prop_plate/external_motor_cmd', + 10) self.predicted_path_pub = self.create_publisher( Path, 'px4_mpc/predicted_path', 10) self.reference_pub = self.create_publisher( - Marker, + PoseStamped, 'px4_mpc/reference', 10) if self.mode == 'offset_free_wrench': self.disturbance_rotation_pub = self.create_publisher( Vector3Stamped, 'px4_mpc/translation_d_hat', - qos_profile_pub) + qos_profile) self.disturbance_translation_pub = self.create_publisher( Vector3Stamped, 'px4_mpc/attitude_d_hat', - qos_profile_pub) + qos_profile) if self.sitl: - qos_profile_pub_sitl = QoSProfile( - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, - history=QoSHistoryPolicy.KEEP_LAST, - depth=0 - ) self.odom_pub = self.create_publisher( Odometry, - f'{self.namespace_prefix}/odom', - qos_profile_pub_sitl) - self.sitl_pose_pub = self.create_publisher( - PoseStamped, - f'{self.namespace_prefix}/pose', - qos_profile_pub_sitl) + 'odom', + qos_profile) return def vehicle_attitude_callback(self, msg): - # NED-> ENU transformation - # Receives quaternion in NED frame as (qw, qx, qy, qz) - self.vehicle_attitude_timestamp = Clock().now().nanoseconds / 1e9 - q_enu = 1/np.sqrt(2) * np.array([msg.q[0] + msg.q[3], msg.q[1] + msg.q[2], msg.q[1] - msg.q[2], msg.q[0] - msg.q[3]]) - q_enu /= np.linalg.norm(q_enu) - self.vehicle_attitude = q_enu.astype(float) + # Store message arrival time in ROS clock domain for validity checking + self.vehicle_attitude_timestamp = self.get_clock().now().nanoseconds / 1e9 + + if self.use_ned: + # NED-> ENU transformation + # Receives quaternion in NED frame as (qw, qx, qy, qz) + q_enu = 1/np.sqrt(2) * np.array([msg.q[0] + msg.q[3], msg.q[1] + msg.q[2], msg.q[1] - msg.q[2], msg.q[0] - msg.q[3]]) + q_enu /= np.linalg.norm(q_enu) + self.vehicle_attitude = q_enu.astype(float) + else: + q = np.array([msg.q[0], msg.q[1], msg.q[2], msg.q[3]]) + q /= np.linalg.norm(q) + self.vehicle_attitude = q.astype(float) def vehicle_local_position_callback(self, msg): - # NED-> ENU transformation - self.vehicle_local_position_timestamp = Clock().now().nanoseconds / 1e9 - self.vehicle_local_position[0] = msg.y - self.vehicle_local_position[1] = msg.x - self.vehicle_local_position[2] = -msg.z - self.vehicle_local_velocity[0] = msg.vy - self.vehicle_local_velocity[1] = msg.vx - self.vehicle_local_velocity[2] = -msg.vz + # Store message arrival time in ROS clock domain for validity checking + self.vehicle_local_position_timestamp = self.get_clock().now().nanoseconds / 1e9 + + if self.use_ned: + # NED-> ENU transformation + self.vehicle_local_position[0] = msg.y + self.vehicle_local_position[1] = msg.x + self.vehicle_local_position[2] = -msg.z + self.vehicle_local_velocity[0] = msg.vy + self.vehicle_local_velocity[1] = msg.vx + self.vehicle_local_velocity[2] = -msg.vz + else: + self.vehicle_local_position[0] = msg.x + self.vehicle_local_position[1] = msg.y + self.vehicle_local_position[2] = msg.z + self.vehicle_local_velocity[0] = msg.vx + self.vehicle_local_velocity[1] = msg.vy + self.vehicle_local_velocity[2] = msg.vz def vehicle_angular_velocity_callback(self, msg): - # NED-> ENU transformation - self.vehicle_angular_velocity_timestamp = Clock().now().nanoseconds / 1e9 - self.vehicle_angular_velocity[0] = msg.xyz[0] - self.vehicle_angular_velocity[1] = -msg.xyz[1] - self.vehicle_angular_velocity[2] = -msg.xyz[2] + # Store message arrival time in ROS clock domain for validity checking + self.vehicle_angular_velocity_timestamp = self.get_clock().now().nanoseconds / 1e9 + + if self.use_ned: + # NED-> ENU transformation + self.vehicle_angular_velocity[0] = msg.xyz[0] + self.vehicle_angular_velocity[1] = -msg.xyz[1] + self.vehicle_angular_velocity[2] = -msg.xyz[2] + else: + self.vehicle_angular_velocity[0] = msg.xyz[0] + self.vehicle_angular_velocity[1] = msg.xyz[1] + self.vehicle_angular_velocity[2] = msg.xyz[2] def vehicle_status_callback(self, msg): - # print("NAV_STATUS: ", msg.nav_state) - # print(" - offboard status: ", VehicleStatus.NAVIGATION_STATE_OFFBOARD) - self.vehicle_status_timestamp = Clock().now().nanoseconds / 1e9 + # Store message arrival time in ROS clock domain for validity checking + self.vehicle_status_timestamp = self.get_clock().now().nanoseconds / 1e9 self.nav_state = msg.nav_state - def publish_reference(self, pub, reference): - msg = Marker() - msg.action = Marker.ADD - msg.header.frame_id = "map" - # msg.header.stamp = Clock().now().nanoseconds / 1000 - msg.ns = "arrow" - msg.id = 1 - msg.type = Marker.SPHERE - msg.scale.x = 0.5 - msg.scale.y = 0.5 - msg.scale.z = 0.5 - msg.color.r = 1.0 - msg.color.g = 0.0 - msg.color.b = 0.0 - msg.color.a = 1.0 - msg.pose.position.x = reference[0] - msg.pose.position.y = reference[1] - msg.pose.position.z = reference[2] - msg.pose.orientation.w = 1.0 - msg.pose.orientation.x = 0.0 - msg.pose.orientation.y = 0.0 - msg.pose.orientation.z = 0.0 - + def publish_reference(self, pub, reference, attitude): + msg = self.vector2PoseMsg('map', reference, attitude) pub.publish(msg) def publish_rate_setpoint(self, u_pred): @@ -304,7 +291,7 @@ def publish_rate_setpoint(self, u_pred): F_cmd *= F_scaling rates_setpoint_msg = VehicleRatesSetpoint() - rates_setpoint_msg.timestamp = int(Clock().now().nanoseconds / 1000) + rates_setpoint_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) rates_setpoint_msg.roll = float(w_cmd[0]) rates_setpoint_msg.pitch = -float(w_cmd[1]) rates_setpoint_msg.yaw = -float(w_cmd[2]) @@ -314,34 +301,35 @@ def publish_rate_setpoint(self, u_pred): self.publisher_rates_setpoint.publish(rates_setpoint_msg) def publish_wrench_setpoint(self, u_pred): - # u_pred is [Fx, Fy, Tz]] in FLU frame - + # u_pred is [Fx, Fy, Fz, Tx, Ty, Tz]] in FLU frame # The PX4 uses normalized wrench input. Scaling with respect to the maximum force and torque. F_scaling = 1/(2 * 1.5) T_scaling = 1/(4 * 0.12 * 1.5) - u_pred[0, 0] *= F_scaling - u_pred[0, 1] *= F_scaling - u_pred[0, 2] *= T_scaling + u_pred[0, :3] *= F_scaling + u_pred[0, 3:6] *= T_scaling + + timestamp = int(self.get_clock().now().nanoseconds / 1000) thrust_outputs_msg = VehicleThrustSetpoint() - thrust_outputs_msg.timestamp = int(Clock().now().nanoseconds / 1000) + thrust_outputs_msg.timestamp = timestamp torque_outputs_msg = VehicleTorqueSetpoint() - torque_outputs_msg.timestamp = int(Clock().now().nanoseconds / 1000) + torque_outputs_msg.timestamp = timestamp - eps_x = 0.0 - eps_y = 0.0 - eps_tau = 0.00 - - thrust_outputs_msg.xyz = [u_pred[0, 0] - eps_x, -u_pred[0, 1] - eps_y, -0.0] - torque_outputs_msg.xyz = [0.0, 0.0, -u_pred[0, 5] - eps_tau] + if self.use_ned: + # FLU -> FRD transformation + thrust_outputs_msg.xyz = [u_pred[0, 0], -u_pred[0, 1], -u_pred[0, 2]] + torque_outputs_msg.xyz = [u_pred[0, 3], -u_pred[0, 4], -u_pred[0, 5]] + else: + thrust_outputs_msg.xyz = [u_pred[0, 0], u_pred[0, 1], u_pred[0, 2]] + torque_outputs_msg.xyz = [u_pred[0, 3], u_pred[0, 4], u_pred[0, 5]] self.publisher_thrust_setpoint.publish(thrust_outputs_msg) self.publisher_torque_setpoint.publish(torque_outputs_msg) def publish_direct_actuator_setpoint(self, u_pred): actuator_outputs_msg = ActuatorMotors() - actuator_outputs_msg.timestamp = int(Clock().now().nanoseconds / 1000) + actuator_outputs_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) # Normalize thrust values w.r.t. max thrust thrust = u_pred[0, :] / self.model.max_thrust @@ -355,6 +343,13 @@ def publish_direct_actuator_setpoint(self, u_pred): actuator_outputs_msg.control[:len(thrust_command)] = thrust_command self.publisher_direct_actuator.publish(actuator_outputs_msg) + def publish_propeller_setpoint(self, u_pred): + propeller_outputs_msg = Float32MultiArray() + thrust_command = u_pred[0, :] + thrust_command = np.clip(np.array(thrust_command, dtype=np.float32), self.model.min_thrust, self.model.max_thrust) + propeller_outputs_msg.data = thrust_command.tolist() + self.publisher_propeller_setpoint.publish(propeller_outputs_msg) + def publish_disturbance_estimate(self, d_hat): disturbance_msg = Vector3Stamped() disturbance_msg.header.stamp = Clock().now().to_msg() @@ -374,7 +369,7 @@ def publish_sitl_odometry(self): msg = Odometry() msg.header.frame_id = "mocap" msg.child_frame_id = "base_link" - msg.header.stamp = Clock().now().to_msg() + msg.header.stamp = self.get_clock().now().to_msg() msg.pose.pose.position.x = self.vehicle_local_position[0] msg.pose.pose.position.y = self.vehicle_local_position[1] msg.pose.pose.position.z = self.vehicle_local_position[2] @@ -404,20 +399,24 @@ def publish_sitl_odometry(self): return def check_data_validity(self): - current_time = Clock().now().nanoseconds / 1e9 + ret_val = True + current_time = self.get_clock().now().nanoseconds / 1e9 # Check if the data is valid based on the timestamps - if (current_time - self.vehicle_attitude_timestamp > DATA_VALIDITY_STREAM or - current_time - self.vehicle_local_position_timestamp > DATA_VALIDITY_STREAM or - current_time - self.vehicle_angular_velocity_timestamp > DATA_VALIDITY_STREAM): - self.get_logger().warn("Vehicle attitude, position, or angular velocity data is too old. Skipping offboard control...", throttle_duration_sec=1.0) - return False - + if (current_time - self.vehicle_attitude_timestamp > DATA_VALIDITY_STREAM): + self.get_logger().warn("Vehicle attitude data is too old. Skipping offboard control...") + ret_val = False + if (current_time - self.vehicle_local_position_timestamp > DATA_VALIDITY_STREAM): + self.get_logger().warn("Vehicle position data is too old. Skipping offboard control...") + ret_val = False + if (current_time - self.vehicle_angular_velocity_timestamp > DATA_VALIDITY_STREAM): + self.get_logger().warn("Vehicle angular velocity data is too old. Skipping offboard control...") + ret_val = False if (current_time - self.vehicle_status_timestamp > DATA_VALIDITY_STATUS): - self.get_logger().warn("Vehicle status data is too old. Skipping offboard control...", throttle_duration_sec=1.0) - return False + self.get_logger().warn("Vehicle status data is too old. Skipping offboard control...") + ret_val = False - return True + return ret_val def cmdloop_callback(self): @@ -431,7 +430,7 @@ def cmdloop_callback(self): # Publish offboard control modes offboard_msg = OffboardControlMode() - offboard_msg.timestamp = int(Clock().now().nanoseconds / 1000) + offboard_msg.timestamp = int(self.get_clock().now().nanoseconds / 1000) offboard_msg.position = False offboard_msg.velocity = False offboard_msg.acceleration = False @@ -440,7 +439,7 @@ def cmdloop_callback(self): offboard_msg.direct_actuator = False if self.mode == 'rate': offboard_msg.body_rate = True - elif self.mode == 'direct_allocation': + elif self.mode == 'direct_allocation' or self.mode == 'propeller': offboard_msg.direct_actuator = True elif self.mode == 'wrench' or self.mode == 'offset_free_wrench' or self.mode == 'lqr_wrench': offboard_msg.thrust_and_torque = True @@ -501,7 +500,7 @@ def cmdloop_callback(self): np.zeros(3), # velocity self.setpoint_attitude[0:], # attitude np.zeros(3)), axis=0) # angular velocity - elif self.mode == 'direct_allocation': + elif self.mode == 'direct_allocation' or self.mode == 'propeller': x0 = np.array([self.vehicle_local_position[0], self.vehicle_local_position[1], self.vehicle_local_position[2], @@ -541,7 +540,11 @@ def cmdloop_callback(self): predicted_path_msg.header = predicted_pose_msg.header predicted_path_msg.poses.append(predicted_pose_msg) self.predicted_path_pub.publish(predicted_path_msg) - self.publish_reference(self.reference_pub, self.setpoint_position) + self.publish_reference( + self.reference_pub, + self.setpoint_position, + self.setpoint_attitude, + ) if self.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD: if self.mode == 'rate': @@ -550,8 +553,10 @@ def cmdloop_callback(self): self.publish_direct_actuator_setpoint(u_pred) elif self.mode == 'wrench' or self.mode == 'offset_free_wrench' or self.mode == 'lqr_wrench': self.publish_wrench_setpoint(u_pred) + elif self.mode == 'propeller': + self.publish_propeller_setpoint(u_pred) - def add_set_pos_callback(self, request, response): + def add_set_pose_callback(self, request, response): self.setpoint_position[0] = request.pose.position.x self.setpoint_position[1] = request.pose.position.y self.setpoint_position[2] = request.pose.position.z @@ -580,7 +585,7 @@ def vector2PoseMsg(self, frame_id, position, attitude): pose_msg.pose.orientation.z = attitude[3] pose_msg.pose.position.x = float(position[0]) pose_msg.pose.position.y = float(position[1]) - pose_msg.pose.position.z = float(position[2]) + pose_msg.pose.position.z = float(position[2]) if not self.camera else 0.0 return pose_msg diff --git a/px4_mpc/px4_mpc/rviz_pos_marker.py b/px4_mpc/px4_mpc/rviz_pos_marker.py index b1a8df5..caeb5f6 100644 --- a/px4_mpc/px4_mpc/rviz_pos_marker.py +++ b/px4_mpc/px4_mpc/rviz_pos_marker.py @@ -58,9 +58,9 @@ def makeBox(msg): marker = Marker() marker.type = Marker.SPHERE - marker.scale.x = msg.scale * 0.45 - marker.scale.y = msg.scale * 0.45 - marker.scale.z = msg.scale * 0.45 + marker.scale.x = msg.scale * 0.4 + marker.scale.y = msg.scale * 0.4 + marker.scale.z = msg.scale * 0.4 marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 @@ -90,10 +90,8 @@ def make6DofMarker(server, menu_handler, process_feedback, fixed, interaction_mo int_marker = InteractiveMarker() int_marker.header.frame_id = 'map' int_marker.pose.position = position - int_marker.scale = 1.0 - + int_marker.scale = 0.3 int_marker.name = 'simple_6dof' - int_marker.description = 'Simple 6-DOF Control' # insert a box makeBoxControl(int_marker) @@ -101,7 +99,6 @@ def make6DofMarker(server, menu_handler, process_feedback, fixed, interaction_mo if fixed: int_marker.name += '_fixed' - int_marker.description += '\n(fixed orientation)' if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { @@ -110,10 +107,6 @@ def make6DofMarker(server, menu_handler, process_feedback, fixed, interaction_mo InteractiveMarkerControl.MOVE_ROTATE_3D: 'MOVE_ROTATE_3D' } int_marker.name += '_' + control_modes_dict[interaction_mode] - int_marker.description = '3D Control' - if show_6dof: - int_marker.description += ' + 6-DOF controls' - int_marker.description += '\n' + control_modes_dict[interaction_mode] if show_6dof: for axis, name in [(1.0, 'move_x'), (2.0, 'move_y'), (3.0, 'move_z')]: diff --git a/px4_mpc/px4_mpc/test/test_setpoints.py b/px4_mpc/px4_mpc/test/test_setpoints.py index f39620e..aaa106b 100755 --- a/px4_mpc/px4_mpc/test/test_setpoints.py +++ b/px4_mpc/px4_mpc/test/test_setpoints.py @@ -14,7 +14,7 @@ def __init__(self): self.sitl = self.declare_parameter('hardware', False).value self.namespace_prefix = f'/{self.namespace}' if self.namespace else '' - self.publisher_ = self.create_publisher(PoseStamped, f'{self.namespace_prefix}/px4_mpc/setpoint_pose', 10) + self.publisher_ = self.create_publisher(PoseStamped, f'px4_mpc/setpoint_pose', 10) self.timer_period = 0.01 # seconds time.sleep(5) # Give time for all inits... self.counter = 0 @@ -30,12 +30,19 @@ def __init__(self): ] else: self.setpoints = [ - (0.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), - (1.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), - (1.4, -0.8, 0.0, 0.0, 0.0, 0.383, 0.924), - (1.4, 0.2, 0.0, 0.0, 0.0, 0.707, 0.707), - (0.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), + (0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0), + (2.06, 1.0, 0.0, 0.0, 0.0, 0.383, 0.924), + (3.0, 0.5, 0.0, 0.0, 0.0, 0.383, 0.924), + (1.75, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707), + (0.75, 0.0, 0.0, 0.0, 0.0, 0.383, 0.924), ] + # self.setpoints = [ + # (0.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), + # (1.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), + # (1.4, -0.8, 0.0, 0.0, 0.0, 0.383, 0.924), + # (1.4, 0.2, 0.0, 0.0, 0.0, 0.707, 0.707), + # (0.4, -0.8, 0.0, 0.0, 0.0, 0.0, 1.0), + # ] self.index = -1 self.timer = self.create_timer(self.timer_period, self.timer_callback) @@ -56,7 +63,7 @@ def timer_callback(self): pose.pose.orientation.w = q[3] self.publisher_.publish(pose) - if self.counter % 2000 == 0: + if self.counter % 1600 == 0: self.index = (self.index + 1) % len(self.setpoints) print(f"Publishing setpoint {self.index}: {self.setpoints[self.index]}") self.counter += 1 diff --git a/px4_mpc/px4_mpc/utils/__init__.py b/px4_mpc/px4_mpc/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/px4_mpc/px4_mpc/utils/rotations.py b/px4_mpc/px4_mpc/utils/rotations.py new file mode 100644 index 0000000..8df3f3e --- /dev/null +++ b/px4_mpc/px4_mpc/utils/rotations.py @@ -0,0 +1,70 @@ + +import casadi as cs +import numpy as np + +def skew_symmetric_cs(v): + return cs.vertcat(cs.horzcat(0, -v[0], -v[1], -v[2]), + cs.horzcat(v[0], 0, v[2], -v[1]), + cs.horzcat(v[1], -v[2], 0, v[0]), + cs.horzcat(v[2], v[1], -v[0], 0)) + +def skew_symmetric_np(v): + return np.array([[0, -v[0], -v[1], -v[2]], + [v[0], 0, v[2], -v[1]], + [v[1], -v[2], 0, v[0]], + [v[2], v[1], -v[0], 0]]) + +def q_to_rot_mat_cs(q): + qw, qx, qy, qz = q[0], q[1], q[2], q[3] + + rot_mat = cs.vertcat( + cs.horzcat(1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qw * qz), 2 * (qx * qz + qw * qy)), + cs.horzcat(2 * (qx * qy + qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qw * qx)), + cs.horzcat(2 * (qx * qz - qw * qy), 2 * (qy * qz + qw * qx), 1 - 2 * (qx ** 2 + qy ** 2))) + return rot_mat + +def q_to_rot_mat_np(q): + qw, qx, qy, qz = q[0], q[1], q[2], q[3] + + rot_mat = np.array([ + [1 - 2 * (qy ** 2 + qz ** 2), 2 * (qx * qy - qw * qz), 2 * (qx * qz + qw * qy)], + [2 * (qx * qy + qw * qz), 1 - 2 * (qx ** 2 + qz ** 2), 2 * (qy * qz - qw * qx)], + [2 * (qx * qz - qw * qy), 2 * (qy * qz + qw * qx), 1 - 2 * (qx ** 2 + qy ** 2)] + ]) + return rot_mat + +def quat_mult_cs(q1, q2): + return cs.vertcat( + q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3], + q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2], + q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1], + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0] + ) + +def quat_mult_np(q1, q2): + return np.array([ + q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3], + q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2], + q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1], + q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0] + ]) + +def v_dot_q_cs(v, q): + rot_mat = q_to_rot_mat_cs(q) + + return cs.mtimes(rot_mat, v) + +def v_dot_q_np(v, q): + rot_mat = q_to_rot_mat_np(q) + + return np.dot(rot_mat, v) + +def quat_error_v_cs(q, q_ref): + q = q / (cs.norm_2(q) + 1e-8) # Normalize to avoid numerical issues + q_ref = cs.sign(cs.dot(q, q_ref)) * q_ref # Ensure q_ref has the same sign as q to avoid discontinuity + q_error_v = cs.vertcat( + q_ref[0]*q[1] - q_ref[1]*q[0] - q_ref[2]*q[3] + q_ref[3]*q[2], + q_ref[0]*q[2] + q_ref[1]*q[3] - q_ref[2]*q[0] - q_ref[3]*q[1], + q_ref[0]*q[3] - q_ref[1]*q[2] + q_ref[2]*q[1] - q_ref[3]*q[0] + ) + return q_error_v \ No newline at end of file diff --git a/px4_mpc/px4_mpc/visualizer.py b/px4_mpc/px4_mpc/visualizer.py new file mode 100644 index 0000000..39e4c8c --- /dev/null +++ b/px4_mpc/px4_mpc/visualizer.py @@ -0,0 +1,260 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +__author__ = "Jaeyoung Lim" +__contact__ = "jalim@ethz.ch" + +import numpy as np + +import rclpy +from rclpy.node import Node +from rclpy.clock import Clock +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy + +from px4_msgs.msg import VehicleAttitude +from px4_msgs.msg import VehicleLocalPosition +from px4_msgs.msg import TrajectorySetpoint +from geometry_msgs.msg import PoseStamped, Point +from nav_msgs.msg import Path +from visualization_msgs.msg import Marker + +class PX4Visualizer(Node): + def __init__(self): + super().__init__("visualizer") + + self.px4_uses_ned = self.declare_parameter('px4_uses_ned', True).value + self.camera = self.declare_parameter('camera', False).value + + # QoS profile + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 + ) + + self.attitude_sub = self.create_subscription( + VehicleAttitude, + f"fmu/out/vehicle_attitude", + self.vehicle_attitude_callback, + qos_profile, + ) + self.local_position_sub = self.create_subscription( + VehicleLocalPosition, + f"fmu/out/vehicle_local_position", + self.vehicle_local_position_callback, + qos_profile, + ) + self.local_position_sub = self.create_subscription( + VehicleLocalPosition, + f"fmu/out/vehicle_local_position_v1", + self.vehicle_local_position_callback, + qos_profile, + ) + self.setpoint_sub = self.create_subscription( + TrajectorySetpoint, + f"fmu/in/trajectory_setpoint", + self.trajectory_setpoint_callback, + qos_profile, + ) + + self.vehicle_pose_pub = self.create_publisher( + PoseStamped, f"px4_visualizer/vehicle_pose", 10 + ) + self.vehicle_vel_pub = self.create_publisher( + Marker, f"px4_visualizer/vehicle_velocity", 10 + ) + self.vehicle_path_pub = self.create_publisher( + Path, f"px4_visualizer/vehicle_path", 10 + ) + self.setpoint_path_pub = self.create_publisher( + Path, f"px4_visualizer/setpoint_path", 10 + ) + + self.vehicle_attitude = np.array([1.0, 0.0, 0.0, 0.0]) + self.vehicle_local_position = np.array([0.0, 0.0, 0.0]) + self.vehicle_local_velocity = np.array([0.0, 0.0, 0.0]) + self.setpoint_position = np.array([0.0, 0.0, 0.0]) + self.vehicle_path_msg = Path() + self.setpoint_path_msg = Path() + + # trail size + self.trail_size = 1000 + + # time stamp for the last local position update received on ROS2 topic + self.last_local_pos_update = 0.0 + # time after which existing path is cleared upon receiving new + # local position ROS2 message + self.declare_parameter("path_clearing_timeout", -1.0) + + timer_period = 0.05 # seconds + self.timer = self.create_timer(timer_period, self.cmdloop_callback) + + def vector2PoseMsg(self, frame_id, position, attitude): + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = frame_id + pose_msg.pose.orientation.w = attitude[0] + pose_msg.pose.orientation.x = attitude[1] + pose_msg.pose.orientation.y = attitude[2] + pose_msg.pose.orientation.z = attitude[3] + pose_msg.pose.position.x = position[0] + pose_msg.pose.position.y = position[1] + pose_msg.pose.position.z = position[2] if not self.camera else 0.0 + return pose_msg + + def vehicle_attitude_callback(self, msg): + if self.px4_uses_ned: + # NED-> ENU transformation + # Receives quaternion in NED frame as (qw, qx, qy, qz) + q_enu = 1/np.sqrt(2) * np.array([msg.q[0] + msg.q[3], msg.q[1] + msg.q[2], msg.q[1] - msg.q[2], msg.q[0] - msg.q[3]]) + q_enu /= np.linalg.norm(q_enu) + self.vehicle_attitude = q_enu.astype(float) + else: + self.vehicle_attitude[0] = msg.q[0] + self.vehicle_attitude[1] = msg.q[1] + self.vehicle_attitude[2] = msg.q[2] + self.vehicle_attitude[3] = msg.q[3] + + def vehicle_local_position_callback(self, msg): + path_clearing_timeout = ( + self.get_parameter("path_clearing_timeout") + .get_parameter_value() + .double_value + ) + if path_clearing_timeout >= 0 and ( + (self.get_clock().now() / 1e9 - self.last_local_pos_update) + > path_clearing_timeout + ): + self.vehicle_path_msg.poses.clear() + self.last_local_pos_update = Clock().now().nanoseconds / 1e9 + + if self.px4_uses_ned: + # TODO: handle NED->ENU transformation + self.vehicle_local_position[0] = msg.y + self.vehicle_local_position[1] = msg.x + self.vehicle_local_position[2] = -msg.z + self.vehicle_local_velocity[0] = msg.vy + self.vehicle_local_velocity[1] = msg.vx + self.vehicle_local_velocity[2] = -msg.vz + else: + self.vehicle_local_position[0] = msg.x + self.vehicle_local_position[1] = msg.y + self.vehicle_local_position[2] = msg.z + self.vehicle_local_velocity[0] = msg.vx + self.vehicle_local_velocity[1] = msg.vy + self.vehicle_local_velocity[2] = msg.vz + + def trajectory_setpoint_callback(self, msg): + if self.px4_uses_ned: + self.setpoint_position[0] = msg.position[1] + self.setpoint_position[1] = msg.position[0] + self.setpoint_position[2] = -msg.position[2] + else: + self.setpoint_position[0] = msg.position[0] + self.setpoint_position[1] = msg.position[1] + self.setpoint_position[2] = msg.position[2] + + def create_arrow_marker(self, id, tail, vector): + msg = Marker() + msg.action = Marker.ADD + msg.header.frame_id = "map" + # msg.header.stamp = Clock().now().nanoseconds / 1000 + msg.ns = "arrow" + msg.id = id + msg.type = Marker.ARROW + msg.scale.x = 0.1*0.3 + msg.scale.y = 0.2*0.3 + msg.scale.z = 0.0 + msg.color.r = 0.5 + msg.color.g = 0.5 + msg.color.b = 0.0 + msg.color.a = 1.0 + dt = 0.3 + tail_point = Point() + tail_point.x = tail[0] + tail_point.y = tail[1] + tail_point.z = tail[2] + head_point = Point() + head_point.x = tail[0] + dt * vector[0] + head_point.y = tail[1] + dt * vector[1] + head_point.z = tail[2] + dt * vector[2] + msg.points = [tail_point, head_point] + return msg + + def append_vehicle_path(self, msg): + self.vehicle_path_msg.poses.append(msg) + if len(self.vehicle_path_msg.poses) > self.trail_size: + del self.vehicle_path_msg.poses[0] + + def append_setpoint_path(self, msg): + self.setpoint_path_msg.poses.append(msg) + if len(self.setpoint_path_msg.poses) > self.trail_size: + del self.setpoint_path_msg.poses[0] + + def cmdloop_callback(self): + vehicle_pose_msg = self.vector2PoseMsg( + "map", self.vehicle_local_position, self.vehicle_attitude + ) + self.vehicle_pose_pub.publish(vehicle_pose_msg) + + # Publish time history of the vehicle path + self.vehicle_path_msg.header = vehicle_pose_msg.header + self.append_vehicle_path(vehicle_pose_msg) + self.vehicle_path_pub.publish(self.vehicle_path_msg) + + # Publish time history of the vehicle path + setpoint_pose_msg = self.vector2PoseMsg("odom", self.vehicle_local_position, self.vehicle_attitude) + self.setpoint_path_msg.header = setpoint_pose_msg.header + self.append_setpoint_path(setpoint_pose_msg) + self.setpoint_path_pub.publish(self.setpoint_path_msg) + + # Publish arrow markers for velocity + velocity_msg = self.create_arrow_marker(1, self.vehicle_local_position, self.vehicle_local_velocity) + self.vehicle_vel_pub.publish(velocity_msg) + + +def main(args=None): + rclpy.init(args=args) + + px4_visualizer = PX4Visualizer() + + rclpy.spin(px4_visualizer) + + px4_visualizer.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/px4_mpc/setup.py b/px4_mpc/setup.py index a510041..444fda2 100644 --- a/px4_mpc/setup.py +++ b/px4_mpc/setup.py @@ -28,6 +28,7 @@ 'mpc_spacecraft = px4_mpc.mpc_spacecraft:main', 'test_setpoints = px4_mpc.test.test_setpoints:main', 'rviz_pos_marker = px4_mpc.rviz_pos_marker:main', + 'visualizer = px4_mpc.visualizer:main', ], }, )