Custom namespaces for spacecraft mpc (DISCOWER#19)

E-Krantz · web-flow · commit cda0b13dc38d · 2024-12-16T16:55:49.000+01:00
* Added custom namespace argument

* Updated README for namespace argument

* Updated mpc parameters
diff --git a/.gitignore b/.gitignore
@@ -1,4 +1,6 @@
 build/*
+install/*
+log/*
 dist/*
 __pycache__/
 poetry.lock
diff --git a/README.md b/README.md
@@ -39,3 +39,8 @@ In order to launch the mpc quadrotor in a ros2 launchfile,
 ```
 ros2 launch px4_mpc mpc_quadrotor_launch.py 
 ```
+
+Use a custom namespace
+```
+ros2 launch px4_mpc mpc_spacecraft_launch.py namespace:=<namespace> 
+```
diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_direct_allocation_mpc.py
@@ -39,7 +39,7 @@
 class SpacecraftDirectAllocationMPC():
     def __init__(self, model):
         self.model = model
-        self.Tf = 1.0
+        self.Tf = 5.0
         self.N = 50
 
         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])
@@ -65,12 +65,12 @@ def setup(self, x0, N_horizon, Tf):
         ocp.dims.N = N_horizon
 
         # set cost
-        Q_mat = np.diag([3e1, 3e1, 3e1,
-                         2e1, 2e1, 2e2,
-                         1e2, 1e2, 1e2, 1e2,
-                         1e1, 1e1, 1e1])
+        Q_mat = np.diag([5e1, 5e1, 5e1,
+                         2e3, 2e3, 2e3,
+                         5e2, 5e2, 5e2, 5e2,
+                         3e2, 3e2, 3e2])
         Q_e = 20 * Q_mat
-        R_mat = np.diag([0.5e1] * 4)
+        R_mat = np.diag([1e1] * 4)
 
         ocp.cost.cost_type = 'NONLINEAR_LS'
         ocp.cost.cost_type_e = 'NONLINEAR_LS'
@@ -87,11 +87,14 @@ def setup(self, x0, N_horizon, Tf):
         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])
+        # ocp.constraints.lbx = np.array([0, -1.54, -10 -0.05, -0.05, -10, -1.01, -1.01, -1.01, -1.01, -10, -10, -10])
+        # ocp.constraints.ubx = np.array([4, 1.5,    10, 0.05,  0.05,  10,  1.01,  1.01,  1.01,  1.01,  10,  10, 10])
+        # ocp.constraints.idxbx = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12])
 
         ocp.constraints.x0 = x0
 
         # set options
-        ocp.solver_options.qp_solver = 'FULL_CONDENSING_DAQP' # FULL_CONDENSING_QPOASES
+        ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
         # 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'
diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_rate_mpc.py
@@ -39,7 +39,7 @@
 class SpacecraftRateMPC():
     def __init__(self, model):
         self.model = model
-        self.Tf = 1.0
+        self.Tf = 5.0
         self.N = 50
 
         self.x0 = np.array([0.01, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0])
@@ -66,11 +66,9 @@ def setup(self, x0, N_horizon, Tf):
         ocp.dims.N = N_horizon
 
         # set cost
-        Q_mat = 2*np.diag([10e1, 10e1, 10e1, 100e1, 100e1, 100e1, 1.0, 1.0, 1.0, 1.0])
-        Q_e = 2*np.diag([30e2, 30e2, 30e2, 100e2, 100e2, 100e2, 10.0, 10.0, 10.0, 10.0])
-        R_mat = 2*np.diag([1e1, 1e1, 1e1, 1e1, 1e1, 1e1])
-
-        # TODO: How do you add terminal costs?
+        Q_mat = np.diag([5e1, 5e1, 5e1, 2e3, 2e3, 2e3, 5e2, 5e2, 5e2, 5e2])
+        Q_e = 20 * Q_mat
+        R_mat = 2*np.diag([1e1, 1e1, 1e1, 3e2, 3e2, 3e2])
 
         # the 'EXTERNAL' cost type can be used to define general cost terms
         # NOTE: This leads to additional (exact) hessian contributions when using GAUSS_NEWTON hessian.
@@ -91,8 +89,8 @@ def setup(self, x0, N_horizon, Tf):
         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])
 
         # set constraints
-        ocp.constraints.lbu = np.array([-Fmax, -Fmax, -Fmax, -wmax, -wmax, -0.5*wmax])
-        ocp.constraints.ubu = np.array([+Fmax, +Fmax, +Fmax, wmax, wmax, 0.5*wmax])
+        ocp.constraints.lbu = np.array([-Fmax, -Fmax, -Fmax, -wmax, -wmax, -wmax])
+        ocp.constraints.ubu = np.array([+Fmax, +Fmax, +Fmax, wmax, wmax, wmax])
         ocp.constraints.idxbu = np.array([0, 1, 2, 3, 4, 5])
 
         ocp.constraints.x0 = x0
diff --git a/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py b/px4_mpc/px4_mpc/controllers/spacecraft_wrench_mpc.py
@@ -39,7 +39,7 @@
 class SpacecraftWrenchMPC():
     def __init__(self, model):
         self.model = model
-        self.Tf = 1.0
+        self.Tf = 5.0
         self.N = 50
 
         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])
@@ -66,9 +66,12 @@ def setup(self, x0, N_horizon, Tf):
         ocp.dims.N = N_horizon
 
         # set cost
-        Q_mat = 2*np.diag([10e1, 10e1, 10e1, 1e1, 1e1, 1e1, 1e1, 1e1, 1e1, 0.0, 0.1, 0.1, 0.1])
-        Q_e   = 2*np.diag([30e2, 30e2, 30e2, 1e2, 1e2, 1e2, 0.0, 0.0, 0.0, 0.0, 1e2, 1e2, 1e2])
-        R_mat = 2*np.diag([1e1, 1e1, 5e0])
+        Q_mat = np.diag([5e1, 5e1, 5e1,
+                         2e3, 2e3, 2e3,
+                         5e2, 5e2, 5e2, 5e2,
+                         3e2, 3e2, 3e2])
+        Q_e = 20 * Q_mat
+        R_mat = 2*np.diag([1e1, 1e1, 1e1])
 
         ocp.cost.cost_type = 'NONLINEAR_LS'
         ocp.cost.cost_type_e = 'NONLINEAR_LS'
diff --git a/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py b/px4_mpc/px4_mpc/launch/mpc_spacecraft_launch.py
@@ -36,29 +36,49 @@
 __contact__ = "padr@kth.se, jalim@ethz.ch"
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from ament_index_python.packages import get_package_share_directory
 import os
 
 
 def generate_launch_description():
+    # Declare namespace argument
+    namespace_arg = DeclareLaunchArgument(
+        'namespace',
+        default_value='',  # Default namespace is empty
+        description='Namespace for all nodes'
+    )
+
+    # Get and process the namespace value
+    namespace = LaunchConfiguration('namespace')
+
     return LaunchDescription([
+        namespace_arg,
+
         Node(
             package='px4_mpc',
-            namespace='px4_mpc',
+            namespace=namespace,
             executable='mpc_spacecraft',
             name='mpc_spacecraft',
             output='screen',
             emulate_tty=True,
-            parameters=[{'mode': 'wrench'}], # rate/wrench/direct_allocation
+            parameters=[
+                {'mode': 'direct_allocation'},
+                {'namespace': namespace}
+            ]
         ),
         Node(
             package='px4_mpc',
-            namespace='px4_mpc',
+            namespace=namespace,
             executable='rviz_pos_marker',
             name='rviz_pos_marker',
             output='screen',
             emulate_tty=True,
+            parameters=[
+                {'namespace': namespace}
+            ]
         ),
         # Node(
         #     package='micro_ros_agent',
@@ -68,15 +88,18 @@ def generate_launch_description():
         #     output='screen'),
         Node(
             package='px4_offboard',
-            namespace='px4_offboard',
+            namespace=namespace,
             executable='visualizer',
-            name='visualizer'
+            name='visualizer',
+            parameters=[
+                {'namespace': namespace}
+            ]
         ),
         Node(
             package='rviz2',
             namespace='',
             executable='rviz2',
             name='rviz2',
-            arguments=['-d', [os.path.join(get_package_share_directory('px4_mpc'), 'config.rviz')]]
-        )
+            arguments=['-d', os.path.join(get_package_share_directory('px4_mpc'), 'config.rviz')],
+        ),
     ])
diff --git a/px4_mpc/px4_mpc/models/spacecraft_rate_model.py b/px4_mpc/px4_mpc/models/spacecraft_rate_model.py
@@ -39,8 +39,8 @@ def __init__(self):
         self.name = 'spacecraft_rate_model'
 
         # constants
-        self.mass = 15.
-        self.max_thrust = 1.
+        self.mass = 15.0
+        self.max_thrust = 1.5
         self.max_rate = 0.5
 
     def get_acados_model(self) -> AcadosModel:
diff --git a/px4_mpc/px4_mpc/mpc_spacecraft.py b/px4_mpc/px4_mpc/mpc_spacecraft.py
@@ -79,54 +79,62 @@ def vector2PoseMsg(frame_id, position, attitude):
 class SpacecraftMPC(Node):
 
     def __init__(self):
-        super().__init__('minimal_publisher')
-        qos_profile = QoSProfile(
-            reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
-            durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
-            history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
-            depth=1
+        super().__init__('spacecraft_mpc')
+
+        # Declare and retrieve the namespace parameter
+        self.declare_parameter('namespace', '')  # Default to empty namespace
+        self.namespace = self.get_parameter('namespace').value
+        self.namespace_prefix = f'/{self.namespace}' if self.namespace else ''
+
+        # QoS profiles
+        qos_profile_pub = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=0
+        )
+
+        qos_profile_sub = QoSProfile(
+            reliability=QoSReliabilityPolicy.BEST_EFFORT,
+            durability=QoSDurabilityPolicy.VOLATILE,
+            history=QoSHistoryPolicy.KEEP_LAST,
+            depth=0
         )
 
         # Get mode; rate, wrench, direct_allocation
         self.mode = self.declare_parameter('mode', 'rate').value
 
         self.status_sub = self.create_subscription(
             VehicleStatus,
-            '/fmu/out/vehicle_status',
+            f'{self.namespace_prefix}/fmu/out/vehicle_status',
             self.vehicle_status_callback,
-            qos_profile)
+            qos_profile_sub)
 
         self.attitude_sub = self.create_subscription(
             VehicleAttitude,
-            '/fmu/out/vehicle_attitude',
+            f'{self.namespace_prefix}/fmu/out/vehicle_attitude',
             self.vehicle_attitude_callback,
-            qos_profile)
-        self.angular_vel_sub = self.create_subscription(
-            VehicleAngularVelocity,
-            '/fmu/out/vehicle_angular_velocity',
-            self.vehicle_angular_velocity_callback,
-            qos_profile)
+            qos_profile_sub)
         self.angular_vel_sub = self.create_subscription(
             VehicleAngularVelocity,
-            '/fmu/out/vehicle_angular_velocity',
+            f'{self.namespace_prefix}/fmu/out/vehicle_angular_velocity',
             self.vehicle_angular_velocity_callback,
-            qos_profile)
+            qos_profile_sub)
         self.local_position_sub = self.create_subscription(
             VehicleLocalPosition,
-            '/fmu/out/vehicle_local_position',
+            f'{self.namespace_prefix}/fmu/out/vehicle_local_position',
             self.vehicle_local_position_callback,
-            qos_profile)
+            qos_profile_sub)
 
-        self.set_pose_srv = self.create_service(SetPose, '/set_pose', self.add_set_pos_callback)
+        self.set_pose_srv = self.create_service(SetPose, f'{self.namespace_prefix}/set_pose', self.add_set_pos_callback)
 
-        self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
-        self.publisher_rates_setpoint = self.create_publisher(VehicleRatesSetpoint, '/fmu/in/vehicle_rates_setpoint', qos_profile)
-        self.publisher_direct_actuator = self.create_publisher(ActuatorMotors, '/fmu/in/actuator_motors', qos_profile)
-        self.publisher_thrust_setpoint = self.create_publisher(VehicleThrustSetpoint, '/fmu/in/vehicle_thrust_setpoint', qos_profile)
-        self.publisher_torque_setpoint = self.create_publisher(VehicleTorqueSetpoint, '/fmu/in/vehicle_torque_setpoint', qos_profile)
-        self.predicted_path_pub = self.create_publisher(Path, '/px4_mpc/predicted_path', 10)
-        self.reference_pub = self.create_publisher(Marker, "/px4_mpc/reference", 10)
-        self.reference_pub = self.create_publisher(Marker, "/px4_mpc/reference", 10)
+        self.publisher_offboard_mode = self.create_publisher(OffboardControlMode, f'{self.namespace_prefix}/fmu/in/offboard_control_mode', qos_profile_pub)
+        self.publisher_rates_setpoint = self.create_publisher(VehicleRatesSetpoint, f'{self.namespace_prefix}/fmu/in/vehicle_rates_setpoint', qos_profile_pub)
+        self.publisher_direct_actuator = self.create_publisher(ActuatorMotors, f'{self.namespace_prefix}/fmu/in/actuator_motors', qos_profile_pub)
+        self.publisher_thrust_setpoint = self.create_publisher(VehicleThrustSetpoint, f'{self.namespace_prefix}/fmu/in/vehicle_thrust_setpoint', qos_profile_pub)
+        self.publisher_torque_setpoint = self.create_publisher(VehicleTorqueSetpoint, f'{self.namespace_prefix}/fmu/in/vehicle_torque_setpoint', qos_profile_pub)
+        self.predicted_path_pub = self.create_publisher(Path, f'{self.namespace_prefix}/px4_mpc/predicted_path', 10)
+        self.reference_pub = self.create_publisher(Marker, f"{self.namespace_prefix}/px4_mpc/reference", 10)
 
         timer_period = 0.02  # seconds
         self.timer = self.create_timer(timer_period, self.cmdloop_callback)
@@ -218,7 +226,7 @@ def publish_reference(self, pub, reference):
     def publish_rate_setpoint(self, u_pred):
         thrust_rates = u_pred[0, :]
         # Hover thrust = 0.73
-        thrust_command = thrust_rates[0:3] * 0.07  # NOTE: Tune in thrust multiplier
+        thrust_command = thrust_rates[0:3]
         rates_setpoint_msg = VehicleRatesSetpoint()
         rates_setpoint_msg.timestamp = int(Clock().now().nanoseconds / 1000)
         rates_setpoint_msg.roll  = float(thrust_rates[3])
diff --git a/px4_mpc/px4_mpc/rviz_pos_marker.py b/px4_mpc/px4_mpc/rviz_pos_marker.py
@@ -158,7 +158,13 @@ class MinimalClientAsync(Node):
 
     def __init__(self):
         super().__init__('minimal_client_async')
-        self.cli = self.create_client(SetPose, '/set_pose')
+
+        # Declare and retrieve the namespace parameter
+        self.declare_parameter('namespace', '')  # Default to empty namespace
+        self.namespace = self.get_parameter('namespace').value
+        self.namespace_prefix = f'/{self.namespace}' if self.namespace else ''
+
+        self.cli = self.create_client(SetPose, f'{self.namespace_prefix}/set_pose')
         while not self.cli.wait_for_service(timeout_sec=1.0):
             self.get_logger().info('service not available, waiting again...')
         self.req = SetPose.Request()
