Update fixedwing dynamics (#22)

* add fixedwing sim and reformat

Signed-off-by: wsribunm <worawissribunma@gmail.com>

* update fixedwing model

Signed-off-by: wsribunm <worawissribunma@gmail.com>

* edit param for nvp sim

Signed-off-by: wsribunm <worawissribunma@gmail.com>

* fix fixedwing sim

Signed-off-by: wsribunm <worawissribunma@gmail.com>

* reformat

Signed-off-by: wsribunm <worawissribunma@gmail.com>

* remove orig file

Signed-off-by: wsribunm <worawissribunma@gmail.com>

---------

Signed-off-by: wsribunm <worawissribunma@gmail.com>

Author: wsribunma

config/nvp_sim.config.rviz

@@ -1,5 +1,4 @@
 <launch>
-	<node pkg="cyecca" exec="fixedwing_sim.py" output="screen"/>
-	<node pkg="joy" exec="joy_node"/>
-</launch>	
-<!-- vi: ts=4 sw=4 et-->
+    <node pkg="cyecca" exec="fixedwing_sim.py" output="screen"/>
+    <node pkg="joy" exec="joy_node"/>
+</launch>

cyecca/models/fixedwing.py

@@ -0,0 +1,6 @@
+<launch>
+	<node pkg="rviz2" exec="rviz2" args="-d $(env HOME)/ros2_ws/src/cyecca/config/nvp_sim.config.rviz">
+		<param name="use_sim_time" value="true"/>
+	</node>
+</launch>	
+<!-- vi: ts=4 sw=4 et-->

cyecca/models/fixedwing.py.orig

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
-from cyecca.models import fixedwing
+from cyecca.models import fixedwing, lookupTableFixedwing
 
+# from cyecca.models.lookupTableFixedwing import build_tables
 import casadi as ca
 import numpy as np
 
@@ -9,12 +10,13 @@
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 
-from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped
+from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped, Point
 
 from rosgraph_msgs.msg import Clock
 from nav_msgs.msg import Odometry, Path
 from sensor_msgs.msg import Joy
 from tf2_ros import TransformBroadcaster
+from visualization_msgs.msg import Marker
 
 
 class Simulator(Node):
@@ -31,6 +33,12 @@ def __init__(self, x0=None, p=None):
         self.pub_pose = self.create_publisher(PoseWithCovarianceStamped, "pose", 1)
         self.pub_clock = self.create_publisher(Clock, "clock", 1)
         self.pub_odom = self.create_publisher(Odometry, "odom", 1)
+        self.pub_lift = self.create_publisher(Marker, "lift", 1)
+        self.pub_drag = self.create_publisher(Marker, "drag", 1)
+        self.pub_weight = self.create_publisher(Marker, "weight", 1)
+        self.pub_thrust = self.create_publisher(Marker, "thrust", 1)
+        self.pub_side_force = self.create_publisher(Marker, "side_force", 1)
+        self.pub_vel_b = self.create_publisher(Marker, "vel_b", 1)
 
         self.tf_broadcaster = TransformBroadcaster(self)
 
@@ -54,18 +62,34 @@ def __init__(self, x0=None, p=None):
         # ----------------------------------------------
         self.input_mode = "manual"
 
+        # -------------------------------------------------------
+        #  Lookup Table
+        # -------------------------------------------------------
+
+        self.lookup_tab = lookupTableFixedwing.build_tables()
+        self.coeff_data = {
+            "CL": 0.0,
+            "CD": 0.0,
+            "Cy": 0.0,
+            "Cl": 0.0,
+            "Cm": 0.0,
+            "Cn": 0.0,
+            "Cmdr": 0.0,
+            "Cmda": 0.0,
+        }
         # -------------------------------------------------------
         # Dynamics
         # ----------------------------------------------
         dynamics = fixedwing
-        self.model = dynamics.derive_model()
+        self.model = dynamics.derive_model(coeff_data=self.coeff_data)
         self.x0_dict = self.model["x0_defaults"]
         if x0 is not None:
             for k in x0.keys():
                 if not k in self.x0_dict.keys():
                     raise KeyError(k)
                 self.x0_dict[k] = x0[k]
         self.p_dict = self.model["p_defaults"]
+        # print(self.p_dict)
         if p is not None:
             for k in p.keys():
                 if not k in self.p_dict.keys():
@@ -74,9 +98,14 @@ def __init__(self, x0=None, p=None):
 
         # init state (x), param(p), and input(u)
         self.state = np.array(list(self.x0_dict.values()), dtype=float)
-        self.p = np.array(list(self.p_dict.values()), dtype=float)
+        self.p = np.array(
+            [
+                self.p_dict[str(self.model["p"][i])]
+                for i in range(self.model["p"].shape[0])
+            ],
+            dtype=float,
+        )
         self.u = np.zeros(4, dtype=float)
-
         # start main loop on timer
         self.system_clock = rclpy.clock.Clock(
             clock_type=rclpy.clock.ClockType.SYSTEM_TIME
@@ -86,6 +115,7 @@ def __init__(self, x0=None, p=None):
             callback=self.timer_callback,
             clock=self.system_clock,
         )
+        self.Info = None
 
     # Manual Joy AETR
     def joy_callback(self, msg: Joy):
@@ -114,7 +144,7 @@ def auto_joy_callback(self, msg: Joy):
         self.input_auto = ca.vertcat(
             msg.axes[0],  # thrust
             msg.axes[1],  # aileron
-            msg.axes[2],  # elevator
+            -msg.axes[2],  # elevator -- positive = elevator down (pitch up)
             msg.axes[3],  # rudder
         )  # TAER
 
@@ -139,9 +169,9 @@ def update_controller(self):
         elif self.input_mode == "auto":
             self.u = ca.vertcat(  # TAER mode
                 float(self.input_auto[0]),
-                float(self.input_auto[1]),  # scaled roll moment from rudder
+                float(self.input_auto[1]),
                 float(self.input_auto[2]),
-                float(self.input_auto[3]),  # Rudder directly affects yaw for nvp
+                float(self.input_auto[3]),
             )
         else:
             self.get_logger().info("unhandled mode: %s" % self.input_mode)
@@ -151,14 +181,78 @@ def integrate_simulation(self):
         """
         Integrate the simulation one step and calculate measurements
         """
+        RAD2DEG = 180 / ca.pi
+
         try:
+            # Grab from Look Up Table
+            if self.Info == None:
+                self.Info = {}
+                self.Info["alpha"] = 0.0
+                self.Info["beta"] = 0.0
+                self.Info["ail"] = 0.0
+                self.Info["elev"] = 0.0
+                self.Info["rud"] = 0.0
+
+            else:
+                self.coeff_data["CD"] = -1 * self.lookup_tab["Cx"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["elev"] * RAD2DEG
+                )
+                self.coeff_data["Cmdr"] = -1 * self.lookup_tab["DnDr"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["beta"] * RAD2DEG / 2
+                )
+                self.coeff_data["Cmda"] = -1 * self.lookup_tab["DlDa"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["beta"] * RAD2DEG / 2
+                )
+                self.coeff_data["CL"] = self.lookup_tab["Cz"](
+                    self.Info["alpha"] * RAD2DEG,
+                    self.Info["beta"] * RAD2DEG,
+                    self.Info["elev"] * RAD2DEG,
+                )
+                self.coeff_data["Cy"] = self.lookup_tab["Cy"](
+                    self.Info["beta"] * RAD2DEG,
+                    self.Info["ail"] * RAD2DEG,
+                    self.Info["rud"] * RAD2DEG,
+                )
+                self.coeff_data["Cl"] = -1 * self.lookup_tab["Cl"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["beta"] * RAD2DEG
+                )
+                self.coeff_data["Cm"] = -1 * self.lookup_tab["Cm"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["elev"] * RAD2DEG
+                )
+                self.coeff_data["Cn"] = -1 * self.lookup_tab["Cn"](
+                    self.Info["alpha"] * RAD2DEG, self.Info["beta"] * RAD2DEG
+                )
+
+                dynamics = fixedwing
+                # print("Cm", self.coeff_data["Cm"])
+                self.model = dynamics.derive_model(coeff_data=self.coeff_data)
+
             # opts = {"abstol": 1e-9,"reltol":1e-9,"fsens_err_con": True,"calc_ic":True,"calc_icB":True}
+            opts = {"abstol": 1e-2, "reltol": 1e-6, "fsens_err_con": True}
+            xdot = self.model["f"](self.state, self.u, self.p)
+
+            # RK4
+            # f_int = ca.integrator(
+            #     "test", "rk", self.model["dae"], {"t0": self.t, "tf": self.t + self.dt}
+            # )
+            # res = f_int(x0=self.state, z0=ca.DM.zeros(self.model["z"].size1()), p=self.p, u=self.u)
+
+            # IDAS
+            # f_int = ca.integrator(
+            #     "test", "idas", self.model["dae"], {"t0": self.t, "tf": self.t + self.dt, **opts}
+            # )
+            # res = f_int(x0=self.state, z0=ca.DM.zeros(self.model["z"].size1()), p=self.p, u=self.u)
+
+            # #CVODES
             f_int = ca.integrator(
-                "test", "cvodes", self.model["dae"], self.t, self.t + self.dt
+                "test", "cvodes", self.model["dae"], self.t, self.t + self.dt, opts
             )
             res = f_int(x0=self.state, z0=0.0, p=self.p, u=self.u)
+
         except RuntimeError as e:
             print(e)
+            xdot = self.model["f"](x=self.state, u=self.u, p=self.p)
+            print(xdot)
             raise e
 
         x1 = np.array(res["xf"]).reshape(-1)
@@ -170,6 +264,7 @@ def integrate_simulation(self):
         # store states and measurements
         # ---------------------------------------------------------------------
         self.state = np.array(res["xf"]).reshape(-1)
+        self.Info = self.model["Info"](x=self.state, u=self.u, p=self.p)
 
         self.publish_state()
 
@@ -222,6 +317,64 @@ def publish_state(self):
         tf.transform.rotation.z = qz
         self.tf_broadcaster.sendTransform(tf)
 
+        # ------------------------------------
+        # publish tf2 transform vector lift
+        # ------------------------------------
+        def vector(v, name, color, scale):
+            marker = Marker()
+            marker.header.frame_id = "base_link"
+            marker.ns = name
+            marker.id = 0
+            marker.type = Marker.ARROW
+            marker.action = Marker.ADD
+            marker.header.stamp = msg_clock.clock
+            marker.pose.position.x = 0.0
+            marker.pose.position.y = 0.0
+            marker.pose.position.z = 0.0
+            marker.pose.orientation.w = 1.0
+            marker.pose.orientation.x = 0.0
+            marker.pose.orientation.y = 0.0
+            marker.pose.orientation.z = 0.0
+            marker.scale.x = 0.05
+            marker.scale.y = 0.1
+            marker.scale.z = 0.1
+            marker.color.r = color[0]
+            marker.color.g = color[1]
+            marker.color.b = color[2]
+            marker.color.a = color[3]
+
+            p1 = Point()
+            p1.x = 0.0
+            p1.y = 0.0
+            p1.z = 0.0
+            p2 = Point()
+            p2.x = float(v[0] * scale)
+            p2.y = float(v[1] * scale)
+            p2.z = float(v[2] * scale)
+            marker.points.append(p1)
+            marker.points.append(p2)
+            return marker
+
+        self.pub_lift.publish(
+            vector(self.Info["L_b"], "lift", [1.0, 0.0, 0.0, 1.0], 1.0)
+        )
+        self.pub_drag.publish(
+            vector(self.Info["D_b"], "drag", [0.0, 0.0, 1.0, 1.0], 1.0)
+        )
+        self.pub_weight.publish(
+            vector(self.Info["W_b"], "weight", [0.0, 1.0, 0.0, 1.0], 1.0)
+        )
+        self.pub_thrust.publish(
+            vector(self.Info["T_b"], "thrust", [1.0, 0.0, 1.0, 1.0], 1.0)
+        )
+        self.pub_side_force.publish(
+            vector(self.Info["S_b"], "side_force", [1.0, 1.0, 0.0, 1.0], 1.0)
+        )
+
+        self.pub_vel_b.publish(
+            vector(self.Info["v_b"], "vel_b", [0.0, 1.0, 1.0, 1.0], 0.1)
+        )
+        print("alpha", self.Info["alpha"], "CL", self.Info["CL"], "CD", self.Info["CD"])
         # ------------------------------------
         # publish pose with covariance stamped
         # ------------------------------------