Kinematics dynamics/six dof rigid body

src/CMakeLists.txt

@@ -577,7 +577,20 @@ if(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "")
   generate_package_targets("${EXTERNAL_MODULE_TARGETS}" "${AllLibs}" "ExternalModules")
 endif(NOT "${EXTERNAL_MODULES_PATH}" STREQUAL "")
 
-# The quotations around DYN_TARGETS informs function that it is a list
+# Remove the following .i files from the list so they aren't configured as build targets
+list(REMOVE_ITEM
+        DYN_TARGETS
+        "simulation/dynamics/KinematicsArchitecture/Assembly.i"
+        "simulation/dynamics/KinematicsArchitecture/Frame.i"
+        "simulation/dynamics/KinematicsArchitecture/Hinge.i"
+        "simulation/dynamics/KinematicsArchitecture/Joint.i"
+        "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.i"
+        "simulation/dynamics/KinematicsArchitecture/Part.i"
+        "simulation/dynamics/KinematicsArchitecture/Point.i"
+        "simulation/dynamics/KinematicsArchitecture/Tensor.i"
+        "simulation/dynamics/KinematicsArchitecture/Vector.i"
+        "simulation/dynamics/KinematicsArchitecture/DynamicsEngine.i")
+
 generate_package_targets("${DYN_TARGETS}" "dynamicsLib;${ARCHITECTURE_LIBS}" "simulation")
 generate_package_targets("${ENV_TARGETS}" "environmentLib;${ARCHITECTURE_LIBS};${library_dependencies}" "simulation")
 generate_package_targets("${DATA_TARGETS}" "onboardDataHandlingLib;${ARCHITECTURE_LIBS};" "simulation")

src/prototype/DynamicsArchitecture/Actuator.py

@@ -0,0 +1,5 @@
+
+class Actuator:
+    def __init__(self):
+        self.force = None
+        self.torque = None

src/prototype/DynamicsArchitecture/Base.py

@@ -0,0 +1,13 @@
+
+
+class Base:
+    def __init__(self):
+        # States
+        self.sigma_BN = None
+        self.omega_BN = None
+        self.omegaDot_BN = None
+
+        # Storage
+        self.r_BN = None
+        self.rDot_BN = None
+        self.rDDot_BN = None

src/prototype/DynamicsArchitecture/Categorizer.py

@@ -0,0 +1,168 @@
+import math
+
+
+class Node:
+    def __init__(self, tag):
+        self.tag = tag
+        self.adjacentNodeList = []
+        self.visited = False
+
+    def __str__(self):
+        string = "Node " + self.tag + " has connections "
+        for nodeInfo in self.adjacentNodeList:
+            string = string + " " + nodeInfo[0].tag + f"-{nodeInfo[1]}"
+        return string
+
+
+def addEdge(node1, node2, weight):
+    node1.adjacentNodeList.append([node2, weight])
+    node2.adjacentNodeList.append([node1, weight])
+
+
+def findBase(nodeList):
+    for node in nodeList:
+        node.visited = False
+
+    for node in nodeList:
+        print("### Checking node " + node.tag + " ###")
+        if checkBase(node):
+            for nodeNew in nodeList:
+                nodeNew.visited = False
+            return node
+        for nodeNew in nodeList:
+            nodeNew.visited = False
+
+
+def checkBase(node):
+    node.visited = True
+    for adjNodeInfo in node.adjacentNodeList:
+        result = adjNodeInfo[1] + search(adjNodeInfo, node)
+        if result > 3:
+            print("Error: found a long chain")
+            return False
+        if math.isnan(result):
+            return False
+    return True
+
+
+def search(nodeInfo, parentNode):
+    print(nodeInfo[0].tag)
+    nodeInfo[0].visited = True
+
+    numConnections = len(nodeInfo[0].adjacentNodeList)
+    if numConnections > 2:
+        print("ERROR: found a fork")
+        return math.nan
+    elif numConnections == 1:
+        return 0
+
+    for adjNodeInfo in nodeInfo[0].adjacentNodeList:
+        if adjNodeInfo[0].tag == parentNode.tag:
+            continue
+        if adjNodeInfo[0].visited:
+            print("ERROR: found a loop")
+            return math.nan
+
+        return adjNodeInfo[1] + search(adjNodeInfo, nodeInfo[0])
+
+
+def fork():
+    # Make the following graph
+    #           A
+    #  	       / \
+    #         1   1
+    # 	     /     \
+    # 	    D       B
+    #      / \       \
+    #     2   1       1
+    #    /     \       \
+    #   G       E       C
+    #            \
+    #             1
+    #              \
+    #               F
+    nodeList = [Node("A"), Node("B"), Node("C"), Node("D"), Node("E"), Node("F"), Node("G")]
+    addEdge(nodeList[0], nodeList[1], 1)
+    addEdge(nodeList[1], nodeList[2], 1)
+    addEdge(nodeList[0], nodeList[3], 1)
+    addEdge(nodeList[3], nodeList[4], 1)
+    addEdge(nodeList[4], nodeList[5], 1)
+    addEdge(nodeList[3], nodeList[6], 2)
+
+    print("### Graph structure ###")
+    for node in nodeList:
+        print(node)
+
+    return nodeList
+
+
+def longChain():
+    # Make the following graph
+    # A
+    #  \
+    #   2
+    # 	 \
+    # 	  B
+    #      \
+    #       1
+    #        \
+    #         C
+    #          \
+    #           2
+    #            \
+    #             D
+    nodeList = [Node("A"), Node("B"), Node("C"), Node("D")]
+    addEdge(nodeList[0], nodeList[1], 2)
+    addEdge(nodeList[1], nodeList[2], 1)
+    addEdge(nodeList[2], nodeList[3], 2)
+
+    print("### Graph structure ###")
+    for node in nodeList:
+        print(node)
+
+    return nodeList
+
+
+def loop():
+    # Make the following graph
+    # A - 1 - B
+    # |       |
+    # 1       1
+    # |       |
+    # C - 1 - D
+    #          \
+    #           1
+    #            \
+    #             E
+    nodeList = [Node("A"), Node("B"), Node("C"), Node("D"), Node("E")]
+    addEdge(nodeList[0], nodeList[1], 1)
+    addEdge(nodeList[0], nodeList[2], 1)
+    addEdge(nodeList[1], nodeList[3], 1)
+    addEdge(nodeList[2], nodeList[3], 1)
+    addEdge(nodeList[3], nodeList[4], 1)
+
+    print("### Graph structure ###")
+    for node in nodeList:
+        print(node)
+
+    return nodeList
+
+
+def Graph(tag):
+    if tag == "fork":
+        return fork()
+    elif tag == "longChain":
+        return longChain()
+    elif tag == "loop":
+        return loop()
+
+
+if __name__ == "__main__":
+    # Choose the graph to analyze
+    graph = Graph("fork")  # fork, longChain, loop
+
+    baseNode = findBase(graph)
+    if baseNode is None:
+        print("### No Base node found ###")
+    else:
+        print("### Base node is " + baseNode.tag + " ###")

src/prototype/DynamicsArchitecture/DynamicsEngine.py

@@ -0,0 +1,18 @@
+class DynamicsEngine:
+    def __init__(self):
+        self.systemList = []
+
+    def categorizeSystem(self):
+        return
+
+    def propagateStates(self):
+        # NEED TO FIGURE OUT SYSTEM INTERACTION
+
+        # Loop through all systems
+        for system in self.systemList:
+            system.propagateDynamics()
+
+        # where to update kinematics?
+
+    def setEnvironment(self):
+        return

src/prototype/DynamicsArchitecture/External Disturbance.py

@@ -0,0 +1,5 @@
+
+class ExternalDisturbance:
+    def __init__(self):
+        self.force = None
+        self.torque = None

src/prototype/DynamicsArchitecture/Subsystem.py

@@ -0,0 +1,21 @@
+
+
+class Subsystem:
+    def __init__(self):
+        self.states = None
+
+        self.a = None
+        self.b = None
+        self.c = None
+
+    def updateEffectorMassProps(self):
+        return
+
+    def updateContributions(self):
+        return
+
+    def updateEnergyMomContributions(self):
+        return
+
+    def computeDerivatives(self):
+        return

src/prototype/DynamicsArchitecture/System.py

@@ -0,0 +1,18 @@
+class System:
+    def __init__(self):
+        self.massSC = None
+        self.ISC = None
+        self.r_CN = None
+        self.rDot_CN = None
+        self.r_CN = None
+        self.rDot_CB = None
+
+        self.base = None
+        self.subsytemList = []
+
+    def propagateDynamics(self):
+        for subsystem in self.subsytemList:
+            subsystem.updateEffectorMassProps()
+            subsystem.computeDerivatives()
+            subsystem.updateContributions()
+            subsystem.updateEnergyMomContributions()

src/prototype/KinematicsArchitecture/Assembly.py

@@ -0,0 +1,9 @@
+
+class Assembly:
+    def __init__(self):
+        self.initialFrame = None
+        self.finalFrame = None
+
+    def makeAssembly(self, partList, jointList):
+        return
+

src/prototype/KinematicsArchitecture/AttitudeParameterization.py

@@ -0,0 +1,48 @@
+from utilities import RigidBodyKinematics as rbk
+
+class DCM:
+    def __init__(self, dcm):
+        self.dcm = dcm
+
+    def toMRP(self):
+        return MRP(rbk.C2MRP(self.dcm))
+
+
+class Euler321:
+    def __init__(self, euler):
+        self.euler = euler
+
+    def toMRP(self):
+        return MRP(rbk.euler3212MRP(self.euler))
+
+
+class PRV:
+    def __init__(self, prv):
+        self.prv = prv
+
+    def toMRP(self):
+        return MRP(rbk.PRV2MRP(self.prv))
+
+
+class Quaternion:
+    def __init__(self, quaternion):
+        self.quaternion = quaternion
+
+    def toMRP(self):
+        return MRP(rbk.EP2MRP(self.quaternion))
+
+
+class CRP:
+    def __init__(self, crp):
+        self.crp = crp
+
+    def toMRP(self):
+        return MRP(rbk.gibbs2MRP(self.crp))
+
+
+class MRP:
+    def __init__(self, mrp):
+        self.mrp = mrp
+
+    def toMRP(self):
+        return self

src/prototype/KinematicsArchitecture/Frame.py

@@ -0,0 +1,21 @@
+from prototype.KinematicsArchitecture.KinematicsEngine import *
+from prototype.KinematicsArchitecture.Vector import Vector
+from prototype.KinematicsArchitecture.AttitudeParameterization import MRP
+
+
+class Frame:
+    def __init__(self, attitude_SP, omega_SP_S, omegaDot_SP_S, r_SP_P, rDot_SP_P, rDDot_SP_P, parentFrame):
+        # Create non-initialized fields
+        self.parentFrame = parentFrame
+        # self.inertial = self.parentFrame.parentFrame is None
+
+        # Initialize variables from constructor
+        self.r_SP_P = Vector(r_SP_P, self.parentFrame)
+        self.rDot_SP_P = Vector(rDot_SP_P, self.parentFrame)
+        self.rDDot_SP_P = Vector(rDDot_SP_P, self.parentFrame)
+        self.sigma_SP = attitude_SP.toMRP()
+        self.omega_SP_S = Vector(omega_SP_S, self)
+        self.omegaDot_SP_S = Vector(omegaDot_SP_S, self)
+
+    def addParentFrame(self, parentFrame):
+        self.parentFrame = parentFrame