c4dynamics 2.0.3__py3-none-any.whl

c4dynamics/detectors/yolo3_tf.py

@@ -0,0 +1,124 @@
+from tensorflow.keras.models import load_model
+from tensorflow import image, expand_dims,  split, reshape, shape, concat
+from tensorflow.image import combined_non_max_suppression
+
+# https://saturncloud.io/blog/how-to-resolve-python-kernel-dies-on-jupyter-notebook-with-tensorflow-2/
+# from .detector_utils import * 
+import numpy as np
+import os
+
+
+MODEL_SIZE                = (416, 416, 3)
+NUM_OF_CLASSES            = 80
+MAX_OUTPUT_SIZE           = 40
+MAX_OUTPUT_SIZE_PER_CLASS = 20
+IOU_THRESHOLD             = 0.5
+CONFIDENCE_THRESHOLD      = 0.5
+
+
+class yolo():
+    
+    '''
+    The yolo3_detector class is a wrapper for object detection using the YOLO 
+    (You Only Look Once) model. 
+
+    1. __init__(self, height=0, width=0): 
+        The class constructor initializes the YOLO model by loading it 
+        from the specified model path. 
+        It also loads the class names for the detected objects. 
+        The height and width parameters are used to set the dimensions of the video frame.
+
+    2. measure(self, frame): 
+        This method takes a frame as input and performs object detection 
+        on it using the YOLO model. 
+        It returns the bounding box coordinates of the detected objects 
+        scaled according to the frame dimensions.
+
+        performs the actual object detection on the given frame. 
+        It preprocesses the frame, resizes it to match the model's input size, 
+        and passes it through the YOLO model to obtain the bounding box coordinates, 
+        scores, class predictions, and the number of detected objects.
+
+
+
+
+    The yolo3_detector class encapsulates the functionality of object detection using the YOLO model, 
+    providing methods to perform detection, extract measurements, 
+    and visualize the results on the frame. The class works in conjunction 
+    with the YOLO model, which is loaded externally and used for the actual detection process.
+
+
+    '''
+    
+    
+    def __init__(self, model, height = 0, width = 0):
+        self.model = model # load_model(modelpath, compile = False)
+
+        with open(os.path.join(os.getcwd(), 'c4dynamics', 'resources', 'detectors', 'yolo', 'v3', 'coco.names'), 'r') as f:
+            self.class_names = f.read().splitlines()
+
+        self.width = width
+        self.height = height
+
+
+
+
+        
+    def detect(self, frame, t, outfile):
+        # frame (1080, 1920, 3), resized_frame tf([1, 416, 416, 3])
+        resized_frame = expand_dims(frame, 0)
+        resized_frame = image.resize(resized_frame, (MODEL_SIZE[0], MODEL_SIZE[1]))
+        
+        detections = self.model.predict(resized_frame, verbose = 0).squeeze()
+        # detections shape: (1, 10647, 85)
+        
+
+        xcenter = detections[:, 0].copy()
+        # top_left_x = center_x - width / 2.0
+        detections[:, 0] = xcenter - detections[:, 2] / 2.0  # top_left_x        
+        # bottom_right_x = center_x + width / 2.0
+        detections[:, 2] = xcenter + detections[:, 2] / 2.0  # bottom_right_x
+        ycenter = detections[:, 1].copy()
+        # top_left_y = center_y - height / 2.0
+        detections[:, 1] = ycenter - detections[:, 3] / 2.0  # top_left_y
+        # bottom_right_y = center_y + height / 2.0
+        detections[:, 3] = ycenter + detections[:, 3] / 2.0  # bottom_right_y
+
+
+        bbox, confs, class_probs = split(detections, [4, 1, -1], axis = -1)
+        bbox = bbox / MODEL_SIZE[0]
+        scores = confs * class_probs
+
+
+        boxes, _, _, Nvalid = combined_non_max_suppression(
+                            boxes = reshape(bbox, (1, -1, 1, 4)) # reshapedbox 
+                                , scores = expand_dims(scores, axis = 0)  # reshape(scores, (1, -1, 1, 4)) # reshape(scores, (shape(scores)[0], -1, shape(scores)[-1]))
+                                    , max_output_size_per_class = MAX_OUTPUT_SIZE_PER_CLASS
+                                        , max_total_size = MAX_OUTPUT_SIZE
+                                            , iou_threshold = IOU_THRESHOLD
+                                                , score_threshold = CONFIDENCE_THRESHOLD)
+
+        # Extract class indices with highest confidence
+        class_indices = np.argmax(class_probs, axis = -1)
+
+        # Extract class labels using the class_indices and class_names
+        class_labels = [self.class_names[i] for i in class_indices]
+
+
+        N = Nvalid[0].numpy()
+        boxout = boxes[0][:N].numpy()
+        
+        # 
+        # translate the normalized diagnonal of the bounding box to the size of the recorded frame:
+        ## 
+        boxout[:, [0, 2]] *= self.width # x1
+        # boxout[:, 2] = boxout[:, 2] * self.width # x2
+        
+        boxout[:, [1, 3]] *= self.height # y1 
+        # boxout[:, 3] = boxout[:, 3] * self.height # y2 
+        # log
+        # normalized box out coordinates 
+        
+        # Return both bounding boxes and object classifications
+        return boxout, class_labels[:N]
+        

c4dynamics/eqm/__init__.py

@@ -0,0 +1,324 @@
+'''
+
+Background Material [MI]_
+-------------------------
+
+Introduction
+^^^^^^^^^^^^
+
+Motion models for points (particles) and rigid bodies in space and time are based on mathematical
+equations. 
+
+Three degrees of freedom models employ translational
+equations of motion.
+Six degrees of freedom models 
+incorporate both translational 
+and rotational equations of motion. 
+
+The inputs to the equations of motion are the 
+forces and moments acting on the body; 
+yielding body accelerations as outputs.
+
+
+Nomenclature and Convention
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Typically, the forces and moments on a body are
+resolved into components in the body coordinate system. 
+Fig-1 shows the components of
+force, moment velocity, and angular rate of a body
+resolved in the body coordinate system. 
+The six projections
+of the linear and angular velocity vectors on the moving
+body frame axes are the six degrees of freedom. 
+The nomenclature and conventions for positive directions
+are as shown in Fig-1 and in the following Table:
+
+
+.. figure:: /_architecture/rigidbody.svg
+   
+   Fig-1: Forces, velocities, moments, and angular rates in body reference frame 
+
+
+
+.. list-table::
+   :widths: 10 20 20 20 20 20 20 
+   :header-rows: 1
+
+   * - Axis
+     - Force along axis
+     - Moment about axis
+     - Linear velocity
+     - Angular displacement 
+     - Angular velocity 
+     - Moment of Inertia
+   * - :math:`x_b`
+     - :math:`{F_x}_b`
+     - :math:`L`
+     - :math:`u`
+     - :math:`\\varphi`
+     - :math:`p`
+     - :math:`I_{xx}`
+   * - :math:`y_b`
+     - :math:`{F_y}_b`
+     - :math:`M`
+     - :math:`v`
+     - :math:`\\theta`
+     - :math:`q`
+     - :math:`I_{yy}`
+   * - :math:`z_b`
+     - :math:`{F_z}_b`
+     - :math:`N`
+     - :math:`w`
+     - :math:`\\psi`
+     - :math:`r`
+     - :math:`I_{zz}`
+
+
+The position of the mass center of the body is given by
+its Cartesian coordinates expressed in an inertial frame of
+reference, such as the fixed-earth frame :math:`(x, y, z)`. 
+
+The body's angular orientation is defined by three rotations :math:`(\\psi, \\theta, \\varphi)` 
+relative to the inertial frame of reference. 
+These are
+called Euler rotations, and the order of the successive rotations
+is important. 
+Starting with the body coordinate frame
+aligned with the earth coordinate frame, the adopted order here is 3-2-1, i.e.: 
+
+(1) Rotate the body frame about the :math:`z_b` axis through the heading angle :math:`\\psi`, 
+(2) Rotate about the :math:`y_b` axis through the pitch angle :math:`\\theta`, and 
+(3) Rotate about the :math:`x_b` axis through the roll angle :math:`\\varphi`
+
+The total inertial velocity :math:`V` has components :math:`u, v`, and :math:`w` on the body frame axes, 
+and :math:`(v_x, v_y, v_z)` on the earth-frame axes.
+
+
+Newton's Second Law 
+^^^^^^^^^^^^^^^^^^^
+
+Newton’s second law of motion establishes the foundational 
+equation governing the relationship among 
+force, mass, and acceleration.
+ 
+
+in the context of Newton's second law, the force :math:`(F)` 
+acting on an object is the derivative of its momentum :math:`(m \\cdot v)` 
+with respect to time :math:`(t)`:
+
+.. math:: 
+   F = {d(m \\cdot v) \\over dt}
+
+Where:
+
+- :math:`F` is the total force acting on the object
+- :math:`m` is the mass of the object
+- :math:`v` is the velocity 
+- :math:`t` is time 
+
+This equation yields the final form of the equations of linear motion.
+In the final form, acceleration is represented by the rate of change of the velocity:
+
+.. math::
+   F = m \\cdot \\dot{v}
+
+Where:
+
+- :math:`F` is the total force acting on the object
+- :math:`m` is the mass of the object
+- :math:`\\dot{v}` is the acceleration of the object
+
+
+A direct extension of Newton's second law to rotational motion 
+reveals that the moment of force (torque) on a body 
+about a given axis equals the time rate of change of the 
+angular momentum of the paricle about that axis. 
+
+
+.. math::
+   M = {dh \\over dt} 
+
+Where:
+
+- :math:`M` is the total moment (torque) acting on the object
+- :math:`h` is the angular momentum vector of the object
+
+
+
+Hence, the final form of the equations of angular motion is given by: 
+
+.. math::
+   M = [I] \\cdot \\dot{\\omega}
+
+Where:
+
+- :math:`M` is the total moment (torque) acting on the object
+
+- :math:`[I]` is the inertia matrix of the body relative to the axis of rotation
+
+- :math:`\\dot{\\omega}` is the absolute angular acceleration vector of the body
+
+
+
+   
+Translational Equations of Motion
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The basis of the translational equation of motion was introduced 
+above. 
+The usual procedure used to solve this
+equation is to sum the external forces
+:math:`F` acting on the body, express them in an 
+inertial frame, and substitute :math:`F` into 
+the equation.
+Once the acceleration, namely the forces 
+divided by the mass, is expressed in inertial coordinates, 
+it is integrated twice to yield the
+translational displacement. 
+
+
+.. math::
+   dx = v_x
+
+   dy = v_y
+
+   dz = v_z
+
+   dv_x = {F[0] \\over m}
+
+   dv_y = {F[1] \\over m}
+
+   dv_z = {F[2] \\over m}
+
+Where:
+
+- :math:`dx, dy, dz` are the changes in position in the :math:`x, y, z` inertial directions, respectively  
+- :math:`dv_x, dv_y, dv_z` are the changes in velocity in the :math:`x, y, z` inertial directions, respectively 
+- :math:`v_x, v_y, v_z` are the velocities in the :math:`x, y, z` inertial directions, respectively
+- :math:`f[0], f[1], f[2]` are the input force components in the :math:`x, y, z` inertial directions, respectively
+- :math:`m` is the mass of the body.
+
+
+These equations describe the dynamics of a datapoint in three-dimensional space (**3DOF**). 
+Which is 
+the rate of change of position 
+:math:`(x, y, z)` with respect to time equals to the velocity, 
+and the rate of change of velocity 
+:math:`(v_x, v_y, v_z)` with respect to time
+equals to the force divided by the mass :math:`(m)`.
+
+
+
+
+Rotational Equations of Motion
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+As mentioned earlier, the rotational analog
+of Newton's law describes the relationship between torque, 
+moment of inertia, and angular acceleration. 
+We also saw that a double integration on the translational 
+acceleration produces the change of the body in position.
+
+However, the angular accelerations
+are typically expressed with respect to a body frame and 
+must be adjusted in order to produce the attitude of the
+body. 
+For that purpose we introduced the euler angles (see Nomenclature and Conventions)
+which describe the 
+body attitude with respect to an inertial frame of reference.
+
+The orientation of the body reference frame is specified by the three
+Euler angles, :math:`\\psi, \\theta, \\varphi`. 
+
+As a rigid body changes its orientation
+in space, the Euler angles change. 
+The rates of change
+of the Euler angles are related to the angular rates :math:`(p, q, r)` of the
+body frame.
+
+The rate of change of the Euler angles together with the rotational analog 
+of Newton's law provide the set of differential equations 
+that 
+describe the equations governing the motion of a rigid body: 
+
+
+.. math::
+
+   d\\varphi = p + (q \\cdot sin(\\varphi) + r \\cdot cos(\\varphi)) \\cdot tan(\\theta)
+
+   d\\theta = q \\cdot cos(\\varphi) - r \\cdot sin(\\varphi)
+   
+   d\\psi = {q \\cdot sin(\\varphi) + r \\cdot cos(\\phi) \\over cos(\\theta)}
+
+   dp = {M[0] - q \\cdot r \\cdot (I_{zz} - I_{yy}) \\over I_{xx}}
+
+   dq = {M[1] - p \\cdot r \\cdot (I_{xx} - I_{zz}) \\over I_{yy}}
+
+   dr = {M[2] - p \\cdot q \\cdot (I_{yy} - I_{xx}) \\over I_{zz}}
+
+Where: 
+
+- :math:`d\\varphi, d\\theta, d\\psi` are the changes in Euler roll, Euler pitch, and Euler yaw angles, respectively 
+- :math:`dp, dq, dr` are the changes in body roll rate, pitch rate, and yaw rate, respectively
+- :math:`\\varphi, \\theta, \\psi` are the Euler roll, Euler pitch, and uler yaw angles, respectively 
+- :math:`p, q, r` are the body roll rate, pitch rate, and yaw rate, respcetively
+- :math:`M[0], M[1], M[2]` are the input moment of force components about the :math:`x, y, z` in the body direction, respectively
+- :math:`I_{xx}, I_{yy}, I_{zz}` are the moments of inertia about the :math:`x, y,` and :math:`z` in body direction, respectively
+
+
+These equations describe the angular dynamics of a rigid body. 
+Together with the equations that describe the translational 
+motion of the body they form the six-dimensional motion in space (**6DOF**). 
+
+
+References
+----------
+
+.. [MI] 17 July 1995, "Missile Flight Simulation, Part One, Surface-to-Air Missiles", 
+         Ch 4 In: Military Handbook. 1995, MIL-HDBK-1211(MI)
+
+
+         
+Examples
+--------
+
+For examples, see the various functions.
+
+'''
+import sys, os 
+sys.path.append('.')
+
+
+from c4dynamics.eqm.derivs import * 
+from c4dynamics.eqm.integrate import * 
+
+
+
+
+if __name__ == "__main__":
+
+  import doctest, contextlib
+  from c4dynamics import IgnoreOutputChecker, cprint
+  
+  # Register the custom OutputChecker
+  doctest.OutputChecker = IgnoreOutputChecker
+
+  tofile = False 
+  optionflags = doctest.FAIL_FAST
+
+  if tofile: 
+    with open(os.path.join('tests', '_out', 'output.txt'), 'w') as f:
+      with contextlib.redirect_stdout(f), contextlib.redirect_stderr(f):
+        result = doctest.testmod(optionflags = optionflags) 
+  else: 
+    result = doctest.testmod(optionflags = optionflags)
+
+  if result.failed == 0:
+    cprint(os.path.basename(__file__) + ": all tests passed!", 'g')
+  else:
+    print(f"{result.failed}")
+
+
+
+

c4dynamics/eqm/derivs.py

@@ -0,0 +1,212 @@
+from typing import Union
+import numpy as np
+import sys 
+sys.path.append('.')
+from c4dynamics.utils.math import * 
+from c4dynamics import datapoint, rigidbody  
+
+def eqm3(dp: 'datapoint', F: Union[np.ndarray, list]) -> np.ndarray:
+  '''
+    Translational motion derivatives.
+
+    These equations represent a set of first-order ordinary 
+    differential equations (ODEs) that describe the motion 
+    of a datapoint in three-dimensional space under the influence 
+    of external forces. 
+    
+    Parameters
+    ----------
+    dp : :class:`datapoint <c4dynamics.states.lib.datapoint.datapoint>`
+        C4dynamics' datapoint object for which the equations of motion are calculated. 
+
+    F : array_like
+        Force vector :math:`[F_x, F_y, F_z]`
+   
+    Returns
+    -------
+    out : numpy.array 
+        :math:`[dx, dy, dz, dv_x, dv_y, dv_z]`
+        6 derivatives of the equations of motion, 3 position derivatives, 
+        and 3 velocity derivatives.  
+
+    Examples
+    --------
+
+    Import required packages:
+
+    .. code:: 
+
+      >>> import c4dynamics as c4d
+      >>> from matplotlib import pyplot as plt 
+      >>> import numpy as np 
+
+
+    .. code:: 
+
+      >>> dp = c4d.datapoint()           
+      >>> dp.mass = 10                    # mass 10kg     # doctest: +IGNORE_OUTPUT
+      >>> F  = [0, 0, c4d.g_ms2]          # g_ms2 = 9.8m/s^2
+      >>> c4d.eqm.eqm3(dp, F)             # doctest: +NUMPY_FORMAT
+      array([0  0  0  0  0  0.980665])
+
+    
+    Euler integration on the equations of motion of 
+    mass in a free fall:
+
+    
+    .. code:: 
+
+      >>> h0 = 10000
+      >>> pt = c4d.datapoint(z = 10000)
+      >>> while pt.z > 0:
+      ...   pt.store()
+      ...   dx = c4d.eqm.eqm3(pt, [0, 0, -c4d.g_ms2])
+      ...   pt.X += dx  # (dt = 1)
+      >>> pt.plot('z')
+      >>> # comapre to anayltic solution 
+      >>> t = np.arange(len(pt.data('t')))
+      >>> z = h0 - .5 * c4d.g_ms2 * t**2 
+      >>> plt.gca().plot(t[z > 0], z[z > 0], 'c', linewidth = 1) # doctest: +IGNORE_OUTPUT
+
+    .. figure:: /_examples/eqm/eqm3.png
+
+  '''
+  
+  dx = dp.vx
+  dy = dp.vy
+  dz = dp.vz
+
+  dvx = F[0] / dp.mass
+  dvy = F[1] / dp.mass
+  dvz = F[2] / dp.mass
+
+  return np.array([dx, dy, dz, dvx, dvy, dvz])
+
+
+def eqm6(rb: 'rigidbody', F: Union[np.ndarray, list], M: Union[np.ndarray, list]) -> np.ndarray:
+  '''
+    Translational and angular motion derivatives. 
+        
+    A set of first-order ordinary 
+    differential equations (ODEs) that describe the motion 
+    of a rigid body in three-dimensional space under the influence 
+    of external forces and moments. 
+
+    Parameters
+    ----------
+    rb : :class:`rigidbody <c4dynamics.states.lib.rigidbody.rigidbody>` 
+        C4dynamics' rigidbody object for which the 
+        equations of motion are calculated on. 
+    F : array_like
+        Force vector :math:`[F_x, F_y, F_z]`  
+    M : array_like
+        Moments vector :math:`[M_x, M_y, M_z]`
+        
+    Returns
+    -------
+    out : numpy.array 
+        :math:`[dx, dy, dz, dv_x, dv_y, dv_z, d\\varphi, d\\theta, d\\psi, dp, dq, dr]`
+        
+        12 total derivatives; 6 of translational motion, 6 of rotational motion.  
+
+    Examples
+    --------
+    Euler integration on the equations of motion of 
+    a stick fixed at one edge:
+
+    (mass: 0.5 kg, moment of inertia about y: 0.4 kg*m^2, 
+    Length: 1m, initial Euler pitch angle: 80° (converted to radians))
+
+    
+    Import required packages:
+
+    .. code:: 
+
+      >>> import c4dynamics as c4d
+      >>> import numpy as np 
+
+    Settings and initial conditions 
+      
+    .. code:: 
+
+      >>> dt = 0.5e-3 
+      >>> t = np.arange(0, 10, dt) 
+      >>> length =  1  # metter 
+      >>> rb = c4d.rigidbody(theta = 80 * c4d.d2r)
+      >>> rb.mass = 0.5 # kg
+      >>> rb.I = [0, 0.4, 0]
+
+      
+    Main loop: 
+    
+    .. code:: 
+
+      >>> for ti in t: 
+      ...    rb.store(ti)
+      ...    tau_g = -rb.mass * c4d.g_ms2 * length / 2 * c4d.cos(rb.theta)
+      ...    dx = c4d.eqm.eqm6(rb, np.zeros(3), [0, tau_g, 0])
+      ...    rb.X += dx * dt 
+      >>> rb.plot('theta')
+
+    .. figure:: /_examples/eqm/eqm3.png
+
+  '''
+
+  ixx, iyy, izz = rb.I
+  #
+  # translational motion derivatives
+  ##
+  dx = rb.vx
+  dy = rb.vy
+  dz = rb.vz
+
+  dvx = F[0] / rb.mass 
+  dvy = F[1] / rb.mass
+  dvz = F[2] / rb.mass
+  
+  # 
+  # euler angles derivatives
+  ## 
+  dphi   = (rb.q * sin(rb.phi) + rb.r * cos(rb.phi)) * tan(rb.theta) + rb.p
+  dtheta =  rb.q * cos(rb.phi) - rb.r * sin(rb.phi)
+  dpsi   = (rb.q * sin(rb.phi) + rb.r * cos(rb.phi)) / cos(rb.theta)
+
+  # 
+  # angular motion derivatives 
+  ## 
+  # dp     = (lA - q * r * (izz - iyy)) / ixx
+  dp = 0 if ixx == 0 else (M[0] - rb.q * rb.r * (izz - iyy)) / ixx
+  dq = 0 if iyy == 0 else (M[1] - rb.p * rb.r * (ixx - izz)) / iyy
+  dr = 0 if izz == 0 else (M[2] - rb.p * rb.q * (iyy - ixx)) / izz
+
+  #       0   1   2   3    4    5    6     7       8     9   10  11 
+  return np.array([dx, dy, dz, dvx, dvy, dvz, dphi, dtheta, dpsi, dp, dq, dr])
+
+
+
+
+
+if __name__ == "__main__":
+
+  import doctest, contextlib, os
+  from c4dynamics import IgnoreOutputChecker, cprint
+  
+  # Register the custom OutputChecker
+  doctest.OutputChecker = IgnoreOutputChecker
+
+  tofile = False 
+  optionflags = doctest.FAIL_FAST
+
+  if tofile: 
+    with open(os.path.join('tests', '_out', 'output.txt'), 'w') as f:
+      with contextlib.redirect_stdout(f), contextlib.redirect_stderr(f):
+        result = doctest.testmod(optionflags = optionflags) 
+  else: 
+    result = doctest.testmod(optionflags = optionflags)
+
+  if result.failed == 0:
+    cprint(os.path.basename(__file__) + ": all tests passed!", 'g')
+  else:
+    print(f"{result.failed}")
+
+