diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 183629789d..992efbe811 100644 --- a/src/CMakeLists.txt +++ b/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") diff --git a/src/prototype/DynamicsArchitecture/Actuator.py b/src/prototype/DynamicsArchitecture/Actuator.py new file mode 100644 index 0000000000..36ca9fb06a --- /dev/null +++ b/src/prototype/DynamicsArchitecture/Actuator.py @@ -0,0 +1,5 @@ + +class Actuator: + def __init__(self): + self.force = None + self.torque = None diff --git a/src/prototype/DynamicsArchitecture/Base.py b/src/prototype/DynamicsArchitecture/Base.py new file mode 100644 index 0000000000..6c3965107b --- /dev/null +++ b/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 diff --git a/src/prototype/DynamicsArchitecture/Categorizer.py b/src/prototype/DynamicsArchitecture/Categorizer.py new file mode 100644 index 0000000000..943a63906a --- /dev/null +++ b/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 + " ###") diff --git a/src/prototype/DynamicsArchitecture/DynamicsEngine.py b/src/prototype/DynamicsArchitecture/DynamicsEngine.py new file mode 100644 index 0000000000..7b9fbe6dff --- /dev/null +++ b/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 diff --git a/src/prototype/DynamicsArchitecture/External Disturbance.py b/src/prototype/DynamicsArchitecture/External Disturbance.py new file mode 100644 index 0000000000..d0065d1258 --- /dev/null +++ b/src/prototype/DynamicsArchitecture/External Disturbance.py @@ -0,0 +1,5 @@ + +class ExternalDisturbance: + def __init__(self): + self.force = None + self.torque = None diff --git a/src/prototype/DynamicsArchitecture/Subsystem.py b/src/prototype/DynamicsArchitecture/Subsystem.py new file mode 100644 index 0000000000..2feb5e0c60 --- /dev/null +++ b/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 diff --git a/src/prototype/DynamicsArchitecture/System.py b/src/prototype/DynamicsArchitecture/System.py new file mode 100644 index 0000000000..d3e2d41722 --- /dev/null +++ b/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() diff --git a/src/prototype/KinematicsArchitecture/Assembly.py b/src/prototype/KinematicsArchitecture/Assembly.py new file mode 100644 index 0000000000..fb7cc37e24 --- /dev/null +++ b/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 + diff --git a/src/prototype/KinematicsArchitecture/AttitudeParameterization.py b/src/prototype/KinematicsArchitecture/AttitudeParameterization.py new file mode 100644 index 0000000000..db8f523474 --- /dev/null +++ b/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 diff --git a/src/prototype/KinematicsArchitecture/Frame.py b/src/prototype/KinematicsArchitecture/Frame.py new file mode 100644 index 0000000000..e681135018 --- /dev/null +++ b/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 diff --git a/src/prototype/KinematicsArchitecture/Graph.py b/src/prototype/KinematicsArchitecture/Graph.py new file mode 100644 index 0000000000..02ddfd72a7 --- /dev/null +++ b/src/prototype/KinematicsArchitecture/Graph.py @@ -0,0 +1,95 @@ +class Graph: + def __init__(self): + return + + +class Node: + def __init__(self, parentNode, frame): + self.childrenNodeList = [] + self.parentNode = parentNode + if parentNode is not None: + self.parentNode.childrenNodeList.append(self) + + self.frame = frame + + +# Helper function to find the LCA +def findLCA(path1, path2): + ind = 0 + # Now iterate over the path list found. + while ind < len(path1) and ind < len(path2): + # If there is a mismatch break the loop. + if path1[ind] != path2[ind]: + break + ind += 1 + + # Return the node encountered just before + # the mismatch. + return path1[ind-1:], path2[ind-1:] + + +def findPath(node, frame, path): + path.append(node.frame) + + if node.frame == frame: + return True + + for childNode in node.childrenNodeList: + if findPath(childNode, frame, path): + return True + + path.pop() + + # Returning false if nothing worked out. + return False + + +def findPathAlternate(frame, path): + newFrame = frame + while newFrame is not None: + path.append(newFrame) + newFrame = newFrame.parentFrame + + path.reverse() + + +if __name__ == "__main__": + # Making the following tree + # 1 + # /|\ + # / | \ + # 2 3 4 + # / \ \ + # / \ \ + # 5 6 7 + # /|\ + # / | \ + # 8 9 10 + node1 = Node(None, 1) + node2 = Node(node1, 2) + node3 = Node(node1, 3) + node4 = Node(node1, 4) + node5 = Node(node2, 5) + node6 = Node(node2, 6) + node7 = Node(node4, 7) + node8 = Node(node6, 8) + node9 = Node(node6, 9) + node10 = Node(node6, 10) + + # Choose the nodes + frame1 = 5 + frame2 = 8 + + # Find the path + path1 = [] + path2 = [] + findPath(node1, frame1, path1) + findPath(node1, frame2, path2) + print(f'Path from node {frame1} to base node: {path1}') + print(f'Path from node {frame2} to base node: {path2}') + + # Find the common ancestor between two frames and return path to it + path2LCA1, path2LCA2 = findLCA(path1, path2) + print(f'Paths to least common ancestor between node {frame1} and {frame2}:') + print(path2LCA1) + print(path2LCA2) diff --git a/src/prototype/KinematicsArchitecture/Hinge.py b/src/prototype/KinematicsArchitecture/Hinge.py new file mode 100644 index 0000000000..20db350b7c --- /dev/null +++ b/src/prototype/KinematicsArchitecture/Hinge.py @@ -0,0 +1,34 @@ +from prototype.KinematicsArchitecture.KinematicsEngine import * +from prototype.KinematicsArchitecture.Frame import Frame +from prototype.KinematicsArchitecture.AttitudeParameterization import PRV +import numpy as np + + +class Hinge: + # Initialize the class variables + def __init__(self, kinematicsEngine, equilibriumFrame, spinAxis, thetaInit, thetaDotInit, k, c): + # Initialize variables from constructor + self.equilibriumFrame = equilibriumFrame + self.spinAxis = spinAxis + self.theta = thetaInit + self.thetaDot = thetaDotInit + self.k = k + self.c = c + self.actuator = None + + # Set the current frame + # WE NEED TO ADD THIS TO THE LIST OF FRAMES + self.currentFrame = Frame(PRV(self.theta * self.spinAxis), self.thetaDot * self.spinAxis, np.array([0, 0, 0]), + np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([0, 0, 0]), self.equilibriumFrame) + kinematicsEngine.frameList.append(self.currentFrame) + + def updateKinematicsStates(self, theta, thetaDot): + self.theta = theta + self.thetaDot = thetaDot + + def updateFrameStates(self): + self.currentFrame.sigma_CP = PRV(self.theta * self.spinAxis).toMRP() + self.currentFrame.omega_CP_C = self.thetaDot * self.spinAxis + + def connectActuator(self, actuator): + self.actuator = actuator diff --git a/src/prototype/KinematicsArchitecture/Joint.py b/src/prototype/KinematicsArchitecture/Joint.py new file mode 100644 index 0000000000..3286f29e60 --- /dev/null +++ b/src/prototype/KinematicsArchitecture/Joint.py @@ -0,0 +1,25 @@ +from prototype.KinematicsArchitecture.Hinge import Hinge + + +class Joint: + def __init__(self): + self.lowerFrame = None + self.upperFrame = None + + def connectPartToLowerConnectionPoint(self, part): + self.lowerFrame.addParentFrame(part.frame) + + def connectPartToUpperConnectionPoint(self, part): + part.frame.addParentFrame(self.upperFrame) + + +class Joint1D(Joint): + def __init__(self, kinematicsEngine, equilibriumFrame, spinAxis, thetaInit, thetaDotInit, k, c): + super().__init__() + + # Create the frame + self.hinge = Hinge(kinematicsEngine, equilibriumFrame, spinAxis, thetaInit, thetaDotInit, k, c) + + # Populate lower and upper frames + self.lowerFrame = self.hinge.equilibriumFrame + self.upperFrame = self.hinge.currentFrame diff --git a/src/prototype/KinematicsArchitecture/KinematicsEngine.py b/src/prototype/KinematicsArchitecture/KinematicsEngine.py new file mode 100644 index 0000000000..3642c62dbf --- /dev/null +++ b/src/prototype/KinematicsArchitecture/KinematicsEngine.py @@ -0,0 +1,110 @@ +from prototype.KinematicsArchitecture.Frame import Frame +from prototype.KinematicsArchitecture.Part import Part +from prototype.KinematicsArchitecture.AttitudeParameterization import MRP +from prototype.KinematicsArchitecture.Joint import Joint1D +from prototype.KinematicsArchitecture.Hinge import Hinge +from prototype.KinematicsArchitecture.Graph import findPathAlternate, findLCA + + +class KinematicsEngine: + def __init__(self): + self.frameList = [] + self.partList = [] + self.jointList = [] + self.assemblyList = [] + + self.rootFrame = Frame(MRP([0, 0, 0]), [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], None) + self.frameList.append(self.rootFrame) + + def createFrame(self, attitude_SP=MRP([0, 0, 0]), omega_SP_S=None, omegaDot_SP_S=None, + r_SP_P=None, rDot_SP_P=None, rDDot_SP_P=None, parentFrame=None): + # Default parameter handling + if r_SP_P is None: + r_SP_P = [0, 0, 0] + if rDot_SP_P is None: + rDot_SP_P = [0, 0, 0] + if rDDot_SP_P is None: + rDDot_SP_P = [0, 0, 0] + if omega_SP_S is None: + omega_SP_S = [0, 0, 0] + if omegaDot_SP_S is None: + omegaDot_SP_S = [0, 0, 0] + if parentFrame is None: + parentFrame = self.rootFrame + + # Create the frame + frame = Frame(attitude_SP, omega_SP_S, omegaDot_SP_S, r_SP_P, rDot_SP_P, rDDot_SP_P, parentFrame) + self.frameList.append(frame) + + return frame + + def createPart(self, attitude_SP=None, omega_SP_S=None, omegaDot_SP_S=None, + r_SP_P=None, rDot_SP_P=None, rDDot_SP_P=None, parentFrame=None, + r_ScP_P=None, m=0, I=None): + # Default parameter handling + if attitude_SP is None: + attitude_SP = MRP([0, 0, 0]) + if omega_SP_S is None: + omega_SP_S = [0, 0, 0] + if omegaDot_SP_S is None: + omegaDot_SP_S = [0, 0, 0] + if r_SP_P is None: + r_SP_P = [0, 0, 0] + if rDot_SP_P is None: + rDot_SP_P = [0, 0, 0] + if rDDot_SP_P is None: + rDDot_SP_P = [0, 0, 0] + if parentFrame is None: + parentFrame = self.createFrame() + if I is None: + I = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] + + # Create the frame and part + frame = self.createFrame(attitude_SP, omega_SP_S, omegaDot_SP_S, r_SP_P, rDot_SP_P, rDDot_SP_P, parentFrame) + part = Part(r_ScP_P, frame, m, I) + self.partList.append(part) + + return part + + def createJoint1D(self, spinAxis, equilibriumFrame=None, thetaInit=0, thetaDotInit=0, k=0, c=0): + if equilibriumFrame is None: + equilibriumFrame = self.createFrame() + + # Create the joint + joint = Joint1D(self, equilibriumFrame, spinAxis, thetaInit, thetaDotInit, k, c) + self.jointList.append(joint) + + return joint + + def connect2Upper(self, part, joint): + part.frame.addParentFrame(joint.upperFrame) + + def connect2Lower(self, part, joint): + joint.lowerFrame.addParentFrame(part.frame) + + def relativeKinematics(self, frame1, frame2): + # Find the absolute path for the frames of the two parts + path1 = [] + path2 = [] + findPathAlternate(frame1, path1) + findPathAlternate(frame2, path2) + + # Find the path to the least common ancestor for both part frames + pathToLCA1, pathToLCA2 = findLCA(path1, path2) + + # Loop through each path and calculate the relative kinematics + + # Return the result + return Frame(MRP([0, 0, 0]), [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0], None) + + def vectorTransportTheorem(self): + return + + def inertiaTransportTheorem(self): + return + + def attitudeKinematics(self): + return + + def updateKinematics(self): + return diff --git a/src/prototype/KinematicsArchitecture/Part.py b/src/prototype/KinematicsArchitecture/Part.py new file mode 100644 index 0000000000..961f5548fc --- /dev/null +++ b/src/prototype/KinematicsArchitecture/Part.py @@ -0,0 +1,13 @@ +from prototype.KinematicsArchitecture.KinematicsEngine import * +from prototype.KinematicsArchitecture.Frame import Frame +from prototype.KinematicsArchitecture.Vector import Vector, Tensor + + +class Part: + def __init__(self, r_ScP_P, frame, m, I): + # Update the frame + self.frame = frame + + self.m = m + self.I = Tensor(I, self.frame) + self.r_ScP_P = Vector(r_ScP_P, self.frame.parentFrame) diff --git a/src/prototype/KinematicsArchitecture/Vector.py b/src/prototype/KinematicsArchitecture/Vector.py new file mode 100644 index 0000000000..49eb562ef1 --- /dev/null +++ b/src/prototype/KinematicsArchitecture/Vector.py @@ -0,0 +1,13 @@ +from prototype.KinematicsArchitecture.KinematicsEngine import * + + +class Vector: + def __init__(self, vectorArray, frame): + self.vectorArray = vectorArray + self.frame = frame + + +class Tensor: + def __init__(self, tensorArray, frame): + self.tensorArray = tensorArray + self.frame = frame diff --git a/src/prototype/Space Vehicle.py b/src/prototype/Space Vehicle.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/prototype/scenarios/scenarioDynamics.py b/src/prototype/scenarios/scenarioDynamics.py new file mode 100644 index 0000000000..2247c6782c --- /dev/null +++ b/src/prototype/scenarios/scenarioDynamics.py @@ -0,0 +1 @@ +import numpy as np \ No newline at end of file diff --git a/src/prototype/scenarios/scenarioKinematics.py b/src/prototype/scenarios/scenarioKinematics.py new file mode 100644 index 0000000000..843764289c --- /dev/null +++ b/src/prototype/scenarios/scenarioKinematics.py @@ -0,0 +1,34 @@ +import numpy as np + +from prototype.KinematicsArchitecture.KinematicsEngine import KinematicsEngine +from prototype.KinematicsArchitecture.AttitudeParameterization import Quaternion + + +def run(): + # Create the kinematics engine + myKinematicsEngine = KinematicsEngine() + + # Define the inertial frame + inertialFrame = myKinematicsEngine.createFrame() + + # Define parts + beta = Quaternion([0, 0, 1, 0]) + myPart1 = myKinematicsEngine.createPart(attitude_SP=beta, parentFrame=inertialFrame) + myPart2 = myKinematicsEngine.createPart() + myPart3 = myKinematicsEngine.createPart(parentFrame=inertialFrame) + + # Define a 1D joint + spinAxis = np.array([1, 0, 0]) + myJoint = myKinematicsEngine.createJoint1D(spinAxis) + + # Connect the parts with the joint + myKinematicsEngine.connect2Lower(myPart1, myJoint) + myKinematicsEngine.connect2Upper(myPart2, myJoint) + + relativeFrame = myKinematicsEngine.relativeKinematics(myPart2.frame, myPart3.frame) + + breakpoint() + + +if __name__ == "__main__": + run() diff --git a/src/simulation/dynamics/KinematicsArchitecture/Assembly.cpp b/src/simulation/dynamics/KinematicsArchitecture/Assembly.cpp new file mode 100755 index 0000000000..c3f570e2c7 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Assembly.cpp @@ -0,0 +1,23 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Assembly.h" + +void Assembly::addPart(const std::shared_ptr& part) { + this->partList.push_back(part); +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Assembly.h b/src/simulation/dynamics/KinematicsArchitecture/Assembly.h new file mode 100755 index 0000000000..c5dcd620c3 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Assembly.h @@ -0,0 +1,59 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef ASSEMBLY_H +#define ASSEMBLY_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Joint.h" +#include "simulation/dynamics/KinematicsArchitecture/Part.h" +#include "simulation/dynamics/KinematicsArchitecture/Point.h" +#include "simulation/dynamics/KinematicsArchitecture/Tensor.h" +#include "simulation/dynamics/KinematicsArchitecture/Vector.h" + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" + +#include +#include + +/*! @brief basic Basilisk C++ module class */ +class Assembly { +public: + Assembly() = default; + ~Assembly() = default; + + void addPart(const std::shared_ptr& part); + + std::vector> assemblyList; + std::vector> partList; + std::vector> jointList; + + std::string tag; + std::shared_ptr frame; + double mass = 0.0; + InertiaTensor IPntSc; + InertiaTensor IPntS; + std::shared_ptr r_ScS; + std::shared_ptr CoMPoint = nullptr; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Assembly.i b/src/simulation/dynamics/KinematicsArchitecture/Assembly.i new file mode 100644 index 0000000000..a47e147d13 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Assembly.i @@ -0,0 +1,38 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Assembly.h" +%} + +%shared_ptr(Assembly) + +%include "simulation/dynamics/KinematicsArchitecture/Assembly.h" + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.cpp b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.cpp new file mode 100755 index 0000000000..5731346d01 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.cpp @@ -0,0 +1,19 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "DynamicsEngine.h" diff --git a/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h new file mode 100755 index 0000000000..833955904e --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h @@ -0,0 +1,45 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef DYNAMICS_ENGINE_H +#define DYNAMICS_ENGINE_H + +#include + +#include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h" +#include "simulation/dynamics/_GeneralModuleFiles/dynamicObject.h" + +class DynamicsEngine: public DynamicObject { +public: + explicit DynamicsEngine(std::shared_ptr kinematicsEngine) : kinematicsEngine(std::move(kinematicsEngine)) {}; + ~DynamicsEngine() override = default; + + std::shared_ptr kinematicsEngine; + std::shared_ptr inertialFrame; + + void UpdateState(uint64_t callTime) override = 0; + void equationsOfMotion(double t, double timeStep) override = 0; + void preIntegration(double callTime) override = 0; + void postIntegration(double callTime) override = 0; + + virtual void updateKinematics() = 0; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.i b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.i new file mode 100755 index 0000000000..f3a0f09455 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/DynamicsEngine.i @@ -0,0 +1,42 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%module DynamicsEngine +%{ + #include "simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h" + +%} + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" + +%include "sys_model.h" +%include "simulation/dynamics/_GeneralModuleFiles/stateData.h" +%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h" +%include "simulation/dynamics/_GeneralModuleFiles/dynamicObject.h" +%include "simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h" + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Frame.cpp b/src/simulation/dynamics/KinematicsArchitecture/Frame.cpp new file mode 100755 index 0000000000..8dd41687eb --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Frame.cpp @@ -0,0 +1,23 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Frame.h" + +std::shared_ptr Frame::getOriginPoint() const{ + return this->originPoint; +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Frame.h b/src/simulation/dynamics/KinematicsArchitecture/Frame.h new file mode 100755 index 0000000000..3d6e963284 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Frame.h @@ -0,0 +1,54 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef FRAME_H +#define FRAME_H + +#include "simulation/dynamics/KinematicsArchitecture/Point.h" +#include "simulation/dynamics/KinematicsArchitecture/Vector.h" + +#include "architecture/utilities/avsEigenMRP.h" +#include "architecture/utilities/avsEigenSupport.h" + +#include +#include +#include + +/*! @brief basic Basilisk C++ module class */ +class Frame { +public: + Frame() = default; + explicit Frame(std::shared_ptr parentFrame, std::shared_ptr point) : parentFrame(std::move(parentFrame)), originPoint(std::move(point)) {}; + ~Frame() = default; + + std::shared_ptr parentFrame = nullptr; + std::shared_ptr originPoint = nullptr; + std::string tag = "root"; + + std::shared_ptr sigma_SP = nullptr; + std::shared_ptr r_SP = nullptr; + + std::shared_ptr getOriginPoint() const; + + void setParentFrame(std::shared_ptr newParentFrame) {this->parentFrame = std::move(newParentFrame);}; + void updateFrame() {}; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Frame.i b/src/simulation/dynamics/KinematicsArchitecture/Frame.i new file mode 100755 index 0000000000..9a764ca850 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Frame.i @@ -0,0 +1,42 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Frame.h" +%} + +%shared_ptr(Frame) +%shared_ptr(Point) +%shared_ptr(Rotation) +%shared_ptr(Translation) + +%include "simulation/dynamics/KinematicsArchitecture/Frame.h" + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Hinge.cpp b/src/simulation/dynamics/KinematicsArchitecture/Hinge.cpp new file mode 100755 index 0000000000..ec33f54ad3 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Hinge.cpp @@ -0,0 +1,44 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Hinge.h" +#include "architecture/utilities/avsEigenSupport.h" + +#include + + +Hinge::Hinge(std::shared_ptr equilibriumFrame, std::shared_ptr currentFrame) + : equilibriumFrame(std::move(equilibriumFrame)), + currentFrame(std::move(currentFrame)) {} + + +void Hinge::updateKinematicStates(double newTheta, double newThetaDot) { + this->theta = newTheta; + this->thetaDot = newThetaDot; +} + + +void Hinge::updateFrameStates() { + // Update the attitude + Eigen::AngleAxisd prv(this->theta, this->spinAxis.getMatrix(this->equilibriumFrame)); + this->currentFrame->sigma_SP->setAttitude(eigenC2MRP(prv.toRotationMatrix().transpose())); + + // Update the angular velocity + this->currentFrame->sigma_SP->setAngularVelocity(this->thetaDot * this->spinAxis.getMatrix(this->equilibriumFrame), this->equilibriumFrame); +} + diff --git a/src/simulation/dynamics/KinematicsArchitecture/Hinge.h b/src/simulation/dynamics/KinematicsArchitecture/Hinge.h new file mode 100755 index 0000000000..20b006ff2b --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Hinge.h @@ -0,0 +1,50 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef HINGE_H +#define HINGE_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Vector.h" +#include + +/*! @brief basic Basilisk C++ module class */ +class Hinge { +public: + Hinge(std::shared_ptr equilibriumFrame, std::shared_ptr currentFrame); + ~Hinge() = default; + + std::shared_ptr equilibriumFrame; + std::shared_ptr currentFrame; + + UnitVector spinAxis; + + double theta = 0.0; + double thetaDot = 0.0; + double k = 0.0; + double c = 0.0; + + // @TODO: we need an actuator here + + void updateKinematicStates(double newTheta, double newThetaDot); + void updateFrameStates(); // call PRV rotation to update things + void connectActuator() {}; // uses actuator +}; + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Hinge.i b/src/simulation/dynamics/KinematicsArchitecture/Hinge.i new file mode 100644 index 0000000000..41287caa77 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Hinge.i @@ -0,0 +1,39 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" + +%include +%shared_ptr(Hinge) + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Hinge.h" +%} + +%include "simulation/dynamics/KinematicsArchitecture/Hinge.h" + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} \ No newline at end of file diff --git a/src/simulation/dynamics/KinematicsArchitecture/Joint.cpp b/src/simulation/dynamics/KinematicsArchitecture/Joint.cpp new file mode 100755 index 0000000000..a592e67a00 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Joint.cpp @@ -0,0 +1,42 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Joint.h" + + +RotaryOneDOF::RotaryOneDOF(std::shared_ptr hinge) { + n = 1; + + this->hingeVector.push_back(std::move(hinge)); + + this->lowerFrame = this->hingeVector[0]->equilibriumFrame; + this->upperFrame = this->hingeVector[0]->currentFrame; +} + + + +RotaryTwoDOF::RotaryTwoDOF(std::shared_ptr firstHinge, std::shared_ptr secondHinge) { + n = 2; + + this->hingeVector.push_back(std::move(firstHinge)); + this->hingeVector.push_back(std::move(secondHinge)); + + this->hingeVector[1]->equilibriumFrame->setParentFrame(this->hingeVector[0]->currentFrame); + this->lowerFrame = this->hingeVector[0]->equilibriumFrame; + this->upperFrame = this->hingeVector[1]->currentFrame; +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Joint.h b/src/simulation/dynamics/KinematicsArchitecture/Joint.h new file mode 100755 index 0000000000..7cd79984d6 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Joint.h @@ -0,0 +1,57 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef JOINT_H +#define JOINT_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Hinge.h" +#include +#include + +/*! @brief basic Basilisk C++ module class */ +class Joint { +public: + Joint() = default; + ~Joint() = default; + + std::shared_ptr lowerFrame; + std::shared_ptr upperFrame; + + std::vector> hingeVector; + int n; +}; + + +class RotaryOneDOF : public Joint { +public: + RotaryOneDOF() {this->n = 1; }; + explicit RotaryOneDOF(std::shared_ptr hinge); + ~RotaryOneDOF() = default; +}; + + +class RotaryTwoDOF : public Joint { +public: + RotaryTwoDOF() {this->n = 2; }; + RotaryTwoDOF(std::shared_ptr firstHinge, std::shared_ptr secondHinge); + ~RotaryTwoDOF() = default; +}; + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Joint.i b/src/simulation/dynamics/KinematicsArchitecture/Joint.i new file mode 100644 index 0000000000..1e94843a98 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Joint.i @@ -0,0 +1,46 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Joint.h" +%} + +%shared_ptr(Joint) +%shared_ptr(RotaryOneDOF) +%shared_ptr(RotaryTwoDOF) +%shared_ptr(Hinge) + +%include "simulation/dynamics/KinematicsArchitecture/Joint.h" + +namespace std { + %template(HingeVector) vector>; +} + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} \ No newline at end of file diff --git a/src/simulation/dynamics/KinematicsArchitecture/KinematicsArchitecture.i b/src/simulation/dynamics/KinematicsArchitecture/KinematicsArchitecture.i new file mode 100644 index 0000000000..ae2e17758b --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/KinematicsArchitecture.i @@ -0,0 +1,16 @@ +%module KinematicsArchitecture + +%include "std_string.i" +%include "std_vector.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" + +%include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.i" +%include "simulation/dynamics/KinematicsArchitecture/Assembly.i" +%include "simulation/dynamics/KinematicsArchitecture/Part.i" +%include "simulation/dynamics/KinematicsArchitecture/Point.i" +%include "simulation/dynamics/KinematicsArchitecture/Tensor.i" +%include "simulation/dynamics/KinematicsArchitecture/Vector.i" +%include "simulation/dynamics/KinematicsArchitecture/Frame.i" +%include "simulation/dynamics/KinematicsArchitecture/Hinge.i" +%include "simulation/dynamics/KinematicsArchitecture/Joint.i" diff --git a/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.cpp b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.cpp new file mode 100755 index 0000000000..1fa6fb680b --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.cpp @@ -0,0 +1,523 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "KinematicsEngine.h" +#include "architecture/utilities/avsEigenMRP.h" +#include "architecture/utilities/avsEigenSupport.h" + +KinematicsEngine::KinematicsEngine() { + auto originPoint = this->createPoint(); + this->rootFrame = std::make_shared(); + this->rootFrame->originPoint = originPoint; +} + +KinematicsEngine::~KinematicsEngine() { + this->frameList.clear(); + this->partList.clear(); + this->jointList.clear(); + this->pointList.clear(); + this->translationList.clear(); + this->rotationList.clear(); +} + +std::shared_ptr KinematicsEngine::createPoint() { + auto tempPoint = std::make_shared(); + this->pointList.push_back(tempPoint); + + return tempPoint; +} + +std::shared_ptr KinematicsEngine::createFrame() { + return createFrame(this->rootFrame); +} + +std::shared_ptr KinematicsEngine::createFrame(const std::shared_ptr& parentFrame) { + auto originPoint = this->createPoint(); + auto tempFrame = std::make_shared(parentFrame, originPoint); + auto transProperties = this->createTranslationProperties(originPoint, parentFrame->originPoint); + auto rotProperties = this->createRotationProperties(tempFrame, parentFrame); + + // Required before setting so that these containers are not nullptrs + tempFrame->r_SP = transProperties; + tempFrame->sigma_SP = rotProperties; + + auto zeroVector = Vector(Eigen::Vector3d::Zero(), parentFrame); + transProperties->setPosition(zeroVector); + transProperties->setVelocity(zeroVector, parentFrame); + transProperties->setAcceleration(zeroVector, parentFrame, parentFrame); + zeroVector.set(Eigen::Vector3d::Zero(), tempFrame); + rotProperties->setAngularVelocity(zeroVector); + rotProperties->setAngularAcceleration(zeroVector, tempFrame); + + this->frameList.push_back(tempFrame); + + return tempFrame; +} + +std::shared_ptr KinematicsEngine::createPart() { + return this->createPart(this->rootFrame); +} + +std::shared_ptr KinematicsEngine::createPart(const std::shared_ptr& parentFrame) { + auto CoMPoint = this->createPoint(); + auto tempFrame = this->createFrame(parentFrame); + auto transProperties = this->createTranslationProperties(CoMPoint, tempFrame->originPoint); + auto tempPart = std::make_shared(std::move(tempFrame)); // maybe add constructor for part with pos vec + auto tempInertia = this->createInertiaTensor(CoMPoint); + + tempPart->r_ScS = transProperties; + + auto zeroVector = Vector(Eigen::Vector3d::Zero(), parentFrame); + transProperties->setPosition(zeroVector); + transProperties->setVelocity(zeroVector, parentFrame); + transProperties->setAcceleration(zeroVector, parentFrame, parentFrame); + tempInertia.set(Eigen::Matrix3d::Zero(), tempPart->frame); + + tempPart->CoMPoint = CoMPoint; + tempPart->IPntSc_S = tempInertia; + + this->partList.push_back(tempPart); + + auto tempNode = std::make_shared(tempPart); + this->nodeList.push_back(tempNode); + + return tempPart; +} + +std::shared_ptr KinematicsEngine::createRotaryOneDOFJoint() { + auto equilibriumFrame = this->createFrame(); + auto currentFrame = this->createFrame(equilibriumFrame); + auto tempHinge = std::make_shared(equilibriumFrame, currentFrame); + + auto tempJoint = std::make_shared(tempHinge); + this->jointList.push_back(tempJoint); + + return tempJoint; +} + +std::shared_ptr KinematicsEngine::createRotaryTwoDOFJoint() { + auto firstEquilibriumFrame = this->createFrame(); + auto firstCurrentFrame = this->createFrame(firstEquilibriumFrame); + auto tempFirstHinge = std::make_shared(std::move(firstEquilibriumFrame), std::move(firstCurrentFrame)); + + auto secondEquilibriumFrame = this->createFrame(); + auto secondCurrentFrame = this->createFrame(secondEquilibriumFrame); + auto tempSecondHinge = std::make_shared(std::move(secondEquilibriumFrame), std::move(secondCurrentFrame)); + + auto tempJoint = std::make_shared(tempFirstHinge, tempSecondHinge); + this->jointList.push_back(tempJoint); + + return tempJoint; +} + +// We might want to overload this to get a frame as the nominal written/derivative instead of the root frame +std::shared_ptr KinematicsEngine::createTranslationProperties(const std::shared_ptr& headPoint, + const std::shared_ptr& tailPoint) { + auto translationProperties = std::make_shared(headPoint, tailPoint); + + auto zeroVector = Vector(Eigen::Vector3d::Zero(), this->rootFrame); + translationProperties->setPosition(zeroVector); + translationProperties->setVelocity(zeroVector, this->rootFrame); + translationProperties->setAcceleration(zeroVector, this->rootFrame, this->rootFrame); + + this->translationList.push_back(translationProperties); + + return translationProperties; +} + +// We might want to overload this to get a frame as the nominal written/derivative instead of the root frame +std::shared_ptr KinematicsEngine::createRotationProperties(const std::shared_ptr& upperFrame, + const std::shared_ptr& lowerFrame) { + auto rotationalProperties = std::make_shared(upperFrame, lowerFrame); + + auto zeroVector = Vector(Eigen::Vector3d::Zero(), this->rootFrame); + Eigen::MRPd zeroAttitude; + zeroAttitude = Eigen::Vector3d::Zero(); + rotationalProperties->setAttitude(zeroAttitude); + rotationalProperties->setAngularVelocity(zeroVector); + rotationalProperties->setAngularAcceleration(zeroVector, this->rootFrame); + + this->rotationList.push_back(rotationalProperties); + + return rotationalProperties; +} + +InertiaTensor KinematicsEngine::createInertiaTensor(const std::shared_ptr& point) { + return InertiaTensor(point); +} + +std::shared_ptr KinematicsEngine::createAssembly() { + return std::make_shared(); +} + +std::shared_ptr KinematicsEngine::findNode(const std::shared_ptr& part) { + for (const auto& node: this->nodeList) { + if (node->part == part) + return node; + } + // Add an error message here, this cannot happen + return nullptr; +} + +void KinematicsEngine::connect(const std::shared_ptr& lowerPart, + const std::shared_ptr& joint, + const std::shared_ptr& upperPart) { + joint->lowerFrame->setParentFrame(lowerPart->frame); + upperPart->frame->setParentFrame(joint->upperFrame); + + auto lowerNode = findNode(lowerPart); + auto upperNode = findNode(upperPart); + lowerNode->addEdge(upperNode, joint); + upperNode->addEdge(lowerNode, joint); +} + +void KinematicsEngine::writeOutputMessages(uint64_t callTime, std::shared_ptr inertialFrame) { + for (const auto& part: this->partList) { + // - Populate state output message + SCStatesMsgPayload stateOut; + stateOut = part->bodyStateOutMsg.zeroMsgPayload; + + // Get attitude and angular velocity + auto omega_BN = part->frame->sigma_SP->getAngularVelocity(); + auto sigma_BN = part->frame->sigma_SP->getAttitude(); // this conversion is so dumb + + // Get position and velocity from the center of mass + auto r_CN = part->frame->r_SP->getPosition() + part->r_ScS->getPosition(); + auto rDot_CN = part->frame->r_SP->getVelocity(inertialFrame) + + part->r_ScS->getVelocity(inertialFrame); + + eigenMatrixXd2CArray(r_CN.getMatrix(inertialFrame), stateOut.r_CN_N); + eigenMatrixXd2CArray(rDot_CN.getMatrix(inertialFrame), stateOut.v_CN_N); + eigenMatrixXd2CArray(eigenMRPd2Vector3d(sigma_BN), stateOut.sigma_BN); + eigenMatrixXd2CArray(omega_BN.getMatrix(part->frame), stateOut.omega_BN_B); + part->bodyStateOutMsg.write(&stateOut, 0, callTime); // need to figure out moduleID + } +} + + +void Node::addEdge(const std::shared_ptr& adjNode, const std::shared_ptr& joint) { + this->edgeList.emplace_back(adjNode, joint); +} + +std::vector > KinematicsEngine::findAbsolutePath(const std::shared_ptr& frame) { + auto intermediateFrame = frame; + std::vector> path {frame}; + + while(intermediateFrame->parentFrame != nullptr) { + intermediateFrame = intermediateFrame->parentFrame; + path.push_back(intermediateFrame); + } + std::reverse(path.begin(), path.end()); + + return path; +} + +std::pair>, std::vector>> KinematicsEngine::findPath2LCA(const std::shared_ptr& upperFrame, + const std::shared_ptr& lowerFrame) { + std::vector> path2LCA1 {}; + std::vector> path2LCA2 {}; + + auto absolutePath1 = findAbsolutePath(upperFrame); + auto absolutePath2 = findAbsolutePath(lowerFrame); + + int index = 0; + for (int i = 0; i < absolutePath1.size() && i < absolutePath2.size(); i++) { + if(absolutePath1.at(i) != absolutePath2.at(i)) { + break; + } + index++; + } + + absolutePath1.erase(absolutePath1.begin()); + absolutePath2.erase(absolutePath2.begin()); + + for (int j = index-1; j < absolutePath1.size(); j++) { + path2LCA1.push_back(absolutePath1.at(j)); + } + for (int k = index-1; k < absolutePath2.size(); k++) { + path2LCA2.push_back(absolutePath2.at(k)); + } + + return std::make_pair(path2LCA1, path2LCA2); +} + +Eigen::MRPd KinematicsEngine::findIntermediateAttitude(const std::vector>& path) { + Eigen::MRPd pulledMRP; + Eigen::Matrix3d transformDCM = Eigen::Matrix3d::Identity(); + + for (int i = 0; i < path.size(); i++) { + pulledMRP = path.at(i)->sigma_SP->getAttitude(); + transformDCM = pulledMRP.toRotationMatrix().transpose() * transformDCM; + } + + Eigen::MRPd relativeMRP = eigenC2MRP(transformDCM); + + return relativeMRP; +} + +Eigen::MRPd KinematicsEngine::findRelativeAttitude(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame) { + if (upperFrame != lowerFrame) { + auto [path2LCA1, path2LCA2] = findPath2LCA(upperFrame, lowerFrame); + + if((!path2LCA1.empty() && !path2LCA2.empty())) { + auto relativeMRP1 = findIntermediateAttitude(path2LCA1); + auto relativeMRP2 = findIntermediateAttitude(path2LCA2); + + Eigen::Matrix3d transformDCM = relativeMRP1.toRotationMatrix().transpose() * relativeMRP2.toRotationMatrix(); + Eigen::MRPd relativeMRP = eigenC2MRP(transformDCM); + + return relativeMRP; + } + else if (path2LCA1.empty()) { + Eigen::MRPd relativeMRP = findIntermediateAttitude(path2LCA2); // this is ridiculous + Eigen::Matrix3d dcm = relativeMRP.toRotationMatrix(); + return eigenC2MRP(dcm); + } + else { + return findIntermediateAttitude(path2LCA1); + } + } + else{ + Eigen::MRPd relativeMRP; + return relativeMRP.setIdentity(); // this is dumb + } +} + +Vector KinematicsEngine::findIntermediateAngularVelocity(const std::vector>& path) { + auto relativeAngularVelocity = path.at(0)->sigma_SP->getAngularVelocity(); + + for (int i = 1; i < path.size(); i++) { + relativeAngularVelocity += path.at(i)->sigma_SP->getAngularVelocity(); + } + + return relativeAngularVelocity; +} + +Vector KinematicsEngine::findRelativeAngularVelocity(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame) { + if (upperFrame != lowerFrame) { + auto [path2LCA1, path2LCA2] = findPath2LCA(upperFrame, lowerFrame); + + if (!path2LCA1.empty() && !path2LCA2.empty()) { + auto relativeAngVel1 = findIntermediateAngularVelocity(path2LCA1); + auto relativeAngVel2 = findIntermediateAngularVelocity(path2LCA2); + return relativeAngVel1 - relativeAngVel2; + } + else if (path2LCA1.empty()) { + return - findIntermediateAngularVelocity(path2LCA2); + } + else { + return findIntermediateAngularVelocity(path2LCA1); + } + } + else{ + return Vector(Eigen::Vector3d::Zero(), upperFrame); + } +} + +Vector KinematicsEngine::findIntermediateAngularAcceleration(const std::vector>& path, + const std::shared_ptr& derivFrame) { + auto relativeAngularAcceleration = Vector(Eigen::Vector3d::Zero(), path.at(0)->sigma_SP->getAngularVelocity().getWrittenFrame()); // we might need to change this, not happy with it + + for (auto const& Frame: path) { + relativeAngularAcceleration += Frame->sigma_SP->getAngularAcceleration(derivFrame); + } + + return relativeAngularAcceleration; +} + +Vector KinematicsEngine::findRelativeAngularAcceleration(const std::shared_ptr& upperFrame, + const std::shared_ptr& lowerFrame, + const std::shared_ptr& derivFrame) { + if (upperFrame != lowerFrame) { + auto [path2LCA1, path2LCA2] = findPath2LCA(upperFrame, lowerFrame); + + if (!path2LCA1.empty() && !path2LCA2.empty()) { + auto relativeAngAccel1 = findIntermediateAngularAcceleration(path2LCA1, derivFrame); + auto relativeAngAccel2 = findIntermediateAngularAcceleration(path2LCA2, derivFrame); + return relativeAngAccel1 - relativeAngAccel2; + } + else if (path2LCA1.empty()) { + return - findIntermediateAngularAcceleration(path2LCA2, derivFrame); + } + else { + return findIntermediateAngularAcceleration(path2LCA1, derivFrame); + } + } + else{ + return Vector(Eigen::Vector3d::Zero(), upperFrame); + } +} + +Vector KinematicsEngine::findRelativePosition(std::shared_ptr headPoint, + const std::shared_ptr& tailPoint) { + auto returnVector = Vector(Eigen::Vector3d::Zero(), this->rootFrame); + + if (headPoint != tailPoint) { + std::vector> visitedVectors = {}; + auto intermediateHeadPoint = headPoint; + + auto [path, val] = searchPointPath(headPoint, tailPoint, visitedVectors, visitedVectors, false); + std::vector> translationPath(path); + + for (const auto& container: translationPath) { + if (container->getHeadPoint() == intermediateHeadPoint) { + returnVector += container->getPosition(); + intermediateHeadPoint = container->getTailPoint(); + } else { + returnVector -= container->getPosition(); + intermediateHeadPoint = container->getHeadPoint(); + } + } + } + return returnVector; +} + +Vector KinematicsEngine::findRelativeVelocity(std::shared_ptr headPoint, + const std::shared_ptr& tailPoint, + const std::shared_ptr derivFrame) { + auto returnVector = Vector(Eigen::Vector3d::Zero(), this->rootFrame); + + if (headPoint != tailPoint) { + std::vector> visitedVectors = {}; + auto intermediateHeadPoint = headPoint; + + auto [path, val] = searchPointPath(headPoint, tailPoint, visitedVectors, visitedVectors, false); + std::vector> translationPath(path); + + for (const auto& container: translationPath) { + if (container->getHeadPoint() == intermediateHeadPoint) { + returnVector += container->getVelocity(derivFrame); + intermediateHeadPoint = container->getTailPoint(); + } else { + returnVector -= container->getVelocity(derivFrame); + intermediateHeadPoint = container->getHeadPoint(); + } + } + } + return returnVector; +} + +std::pair>, bool> KinematicsEngine::searchPointPath(std::shared_ptr headPoint, + const std::shared_ptr& tailPoint, std::vector> translationPath, + std::vector> visitedContainers, bool leafReached) { + std::vector> transPath(translationPath); + std::vector> commonContainers = {}; + + for (const auto& transContainer: translationList) { + if ((transContainer->getHeadPoint() == headPoint || + transContainer->getTailPoint() == headPoint) && + !(std::find(visitedContainers.begin(), visitedContainers.end(), transContainer) != + visitedContainers.end())) { + commonContainers.push_back(transContainer); + } + } + + if (commonContainers.empty()) { + leafReached = true; + } + + for (const auto& container: commonContainers) { + if (container->getHeadPoint() == headPoint) { + headPoint = container->getTailPoint(); + } else { + headPoint = container->getHeadPoint(); + } + + transPath.push_back(container); + visitedContainers.push_back(container); + + if (headPoint == tailPoint) { + return std::make_pair(transPath, false); + } + + auto [path, leaf] = searchPointPath(headPoint, tailPoint, transPath, visitedContainers, leafReached); + leafReached = leaf; + + transPath.clear(); + for (const auto& element: path) { + transPath.push_back(element); + } + + if (leafReached) { + transPath.pop_back(); + } + } + return std::make_pair(transPath, leafReached); +} + +InertiaTensor KinematicsEngine::parallelAxisTheorem(const std::shared_ptr& part, const std::shared_ptr& point) { + Vector relativePosition = findRelativePosition(part->IPntSc_S.getPoint() , point); + + Eigen::Matrix3d returnInertiaMatrix = part->IPntSc_S.getMatrix(relativePosition.getWrittenFrame()) + + (part->mass * eigenTilde(relativePosition.getMatrix(relativePosition.getWrittenFrame())) * eigenTilde(relativePosition.getMatrix(relativePosition.getWrittenFrame())).transpose()); + + InertiaTensor returnInertia = this->createInertiaTensor(point); + returnInertia.set(returnInertiaMatrix, relativePosition.getWrittenFrame()); + + return returnInertia; +} + +double KinematicsEngine::getAssemblyMass(const std::shared_ptr& assembly) { + double assemblyMass = 0.0; + for (auto const& part: assembly->partList) { + assemblyMass += part->mass; + } + + return assemblyMass; +} + +Vector KinematicsEngine::getAssemblyCOM(const std::shared_ptr& assembly, const std::shared_ptr& tailPoint) { + double assemblyMass = getAssemblyMass(assembly); + auto assemblyCOMPositionVector = Vector(Eigen::Vector3d::Zero(), nullptr); + auto intermediatePosVec1 = Vector(Eigen::Vector3d::Zero(), this->rootFrame); + Eigen::Vector3d intermediateVec; + + for (const auto& part: assembly->partList) { + intermediatePosVec1 = findRelativePosition(part->r_ScS->getHeadPoint(), tailPoint); + intermediatePosVec1 *= (part->mass / assemblyMass); + + if (assemblyCOMPositionVector.getWrittenFrame()) { + assemblyCOMPositionVector += intermediatePosVec1; + } + else{ + assemblyCOMPositionVector = intermediatePosVec1; + } + } + + return assemblyCOMPositionVector; +} + +InertiaTensor KinematicsEngine::getAssemblyInertia(const std::shared_ptr& assembly, const std::shared_ptr& point) { + auto intermediateInertia = Tensor(); + for (int i = 0; ipartList.size(); i++) { + auto partInertia = parallelAxisTheorem(assembly->partList.at(i), point); + + if (!(i == 0)) { + intermediateInertia += partInertia; + } + else{ + intermediateInertia.set(partInertia.getMatrix(partInertia.getWrittenFrame()), partInertia.getWrittenFrame()); + } + } + + auto assemblyInertia = InertiaTensor(point); + assemblyInertia.set(intermediateInertia.getMatrix(intermediateInertia.getWrittenFrame()), intermediateInertia.getWrittenFrame()); + + return assemblyInertia; +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h new file mode 100755 index 0000000000..1c630c4f09 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h @@ -0,0 +1,100 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef KINEMATICS_ENGINE_H +#define KINEMATICS_ENGINE_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Part.h" +#include "simulation/dynamics/KinematicsArchitecture/Joint.h" +#include "simulation/dynamics/KinematicsArchitecture/Point.h" +#include "simulation/dynamics/KinematicsArchitecture/Tensor.h" +#include "simulation/dynamics/KinematicsArchitecture/Assembly.h" +#include "simulation/dynamics/KinematicsArchitecture/Vector.h" +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/avsEigenMRP.h" +#include + +class Node { +public: + explicit Node(std::shared_ptr part) : part(std::move(part)) {}; + ~Node() = default; + + void addEdge(const std::shared_ptr& adjNode, const std::shared_ptr& joint); + + std::shared_ptr part; + std::vector, std::shared_ptr>> edgeList; + bool visited; +}; + + +class KinematicsEngine { +public: + KinematicsEngine(); + ~KinematicsEngine(); + + std::shared_ptr rootFrame; + std::vector> frameList; + std::vector> nodeList; + std::vector> partList; + std::vector> jointList; + std::vector> pointList; + std::vector> translationList; + std::vector> rotationList; + + std::shared_ptr createPoint(); + std::shared_ptr createFrame(); + std::shared_ptr createFrame(const std::shared_ptr& parentFrame); + std::shared_ptr createPart(); + std::shared_ptr createPart(const std::shared_ptr& parentFrame); + std::shared_ptr createRotaryOneDOFJoint(); + std::shared_ptr createRotaryTwoDOFJoint(); + std::shared_ptr createTranslationProperties(const std::shared_ptr& headPoint, const std::shared_ptr& tailPoint); + std::shared_ptr createRotationProperties(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame); + InertiaTensor createInertiaTensor(const std::shared_ptr& point); + std::shared_ptr createAssembly(); + + std::shared_ptr findNode(const std::shared_ptr& part); + void connect(const std::shared_ptr& lowerPart, const std::shared_ptr& joint, const std::shared_ptr& upperPart); + + static std::vector> findAbsolutePath(const std::shared_ptr& frame); + static std::pair>, std::vector>> findPath2LCA(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame); + static Eigen::MRPd findRelativeAttitude(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame); + static Vector findRelativeAngularVelocity(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame); + static Vector findRelativeAngularAcceleration(const std::shared_ptr& upperFrame, const std::shared_ptr& lowerFrame, const std::shared_ptr& derivFrame); + Vector findRelativePosition(std::shared_ptr headPoint, const std::shared_ptr& tailPoint); + Vector findRelativeVelocity(std::shared_ptr headPoint, const std::shared_ptr& tailPoint, const std::shared_ptr derivFrame); + InertiaTensor parallelAxisTheorem(const std::shared_ptr& part, const std::shared_ptr& point); + double getAssemblyMass(const std::shared_ptr& assembly); + Vector getAssemblyCOM(const std::shared_ptr& assembly, const std::shared_ptr& tailPoint); + InertiaTensor getAssemblyInertia(const std::shared_ptr& assembly, const std::shared_ptr& point); + + void writeOutputMessages(uint64_t callTime, std::shared_ptr inertialFrame); + +private: + static Eigen::MRPd findIntermediateAttitude(const std::vector>& path); + static Vector findIntermediateAngularVelocity(const std::vector>& path); + static Vector findIntermediateAngularAcceleration(const std::vector>& path, const std::shared_ptr& derivFrame); + std::pair>, bool> searchPointPath(std::shared_ptr headPoint, + const std::shared_ptr& tailPoint, + std::vector> translationPath, + std::vector> visitedContainers, bool leafReached); +}; + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.i b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.i new file mode 100755 index 0000000000..9111fad6a1 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/KinematicsEngine.i @@ -0,0 +1,62 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h" +%} + +%shared_ptr(Assembly) +%shared_ptr(Frame) +%shared_ptr(InertiaTensor) +%shared_ptr(Joint) +%shared_ptr(KinematicsEngine) +%shared_ptr(Node) +%shared_ptr(Part) +%shared_ptr(Point) +%shared_ptr(RotaryOneDOF) +%shared_ptr(RotaryTwoDOF) +%shared_ptr(Rotation) +%shared_ptr(Tensor) +%shared_ptr(Translation) +%shared_ptr(Vector) + +%include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h" + +namespace std { + %template(JointVector) vector>; + %template(PartVector) vector>; + %template(PointVector) vector>; + %template(TranslationVector) vector>; + %template(RotationVector) vector>; + %template(FrameVector) vector>; + %template(NodeVector) vector>; +} + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Part.cpp b/src/simulation/dynamics/KinematicsArchitecture/Part.cpp new file mode 100755 index 0000000000..983f91625e --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Part.cpp @@ -0,0 +1,24 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Part.h" + +#include + +Part::Part(std::shared_ptr frame) + : frame(std::move(frame)) {} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Part.h b/src/simulation/dynamics/KinematicsArchitecture/Part.h new file mode 100755 index 0000000000..2aa9753b80 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Part.h @@ -0,0 +1,48 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef PART_H +#define PART_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Point.h" +#include "simulation/dynamics/KinematicsArchitecture/Tensor.h" +#include "simulation/dynamics/KinematicsArchitecture/Vector.h" +#include "architecture/messaging/messaging.h" +#include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" + +#include + +/*! @brief basic Basilisk C++ module class */ +class Part { +public: + Part() = default; + explicit Part(std::shared_ptr frame); + ~Part() = default; + + double mass = 0.0; + InertiaTensor IPntSc_S; + std::shared_ptr r_ScS; + std::shared_ptr CoMPoint; + std::shared_ptr frame; + + Message bodyStateOutMsg; +}; + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Part.i b/src/simulation/dynamics/KinematicsArchitecture/Part.i new file mode 100644 index 0000000000..11c809d910 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Part.i @@ -0,0 +1,43 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Part.h" +%} + +%shared_ptr(Frame) +%shared_ptr(Part) +%shared_ptr(InertiaTensor) + +%include "simulation/dynamics/KinematicsArchitecture/Part.h" + +%include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" +struct SCStatesMsg_C; + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Point.cpp b/src/simulation/dynamics/KinematicsArchitecture/Point.cpp new file mode 100755 index 0000000000..9ec4f83236 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Point.cpp @@ -0,0 +1,19 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "Point.h" diff --git a/src/simulation/dynamics/KinematicsArchitecture/Point.h b/src/simulation/dynamics/KinematicsArchitecture/Point.h new file mode 100755 index 0000000000..19f0777757 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Point.h @@ -0,0 +1,35 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef POINT_H +#define POINT_H + +#include +#include + +/*! @brief basic Basilisk C++ module class */ +class Point { +public: + Point() = default; + ~Point() = default; + std::string tag; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Point.i b/src/simulation/dynamics/KinematicsArchitecture/Point.i new file mode 100644 index 0000000000..8688323101 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Point.i @@ -0,0 +1,38 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Point.h" +%} + +%shared_ptr(Point) + +%include "simulation/dynamics/KinematicsArchitecture/Point.h" + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.cpp b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.cpp new file mode 100755 index 0000000000..a047614732 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.cpp @@ -0,0 +1,144 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "SixDOFRigidBody.h" +#include "architecture/utilities/avsEigenMRP.h" +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/macroDefinitions.h" +#include "simulation/dynamics/_GeneralModuleFiles/svIntegratorRK4.h" +#include +#include + + +SixDOFRigidBody::SixDOFRigidBody(std::shared_ptr kinematicsEngine, std::shared_ptr inertialFrame, std::shared_ptr body) +: DynamicsEngine(std::move(kinematicsEngine)) { + // - Set integrator as RK4 by default + this->integrator = new svIntegratorRK4(this); + + this->timeBefore = 0.0; + + this->body = std::move(body); + this->baseFrame = this->body->frame; + this->inertialFrame = std::move(inertialFrame); + this->mass = this->body->mass; + this->IScPntC = this->body->IPntSc_S; +} + + +void SixDOFRigidBody::Reset(uint64_t CurrentSimNanos) { + this->initializeDynamics(); +} + +void SixDOFRigidBody::UpdateState(uint64_t callTime) { + this->integrateState(callTime * NANO2SEC); + this->writeOutputMessages(callTime); +} + +void SixDOFRigidBody::writeOutputMessages(uint64_t callTime) { + this->kinematicsEngine->writeOutputMessages(callTime, this->inertialFrame); +} + +void SixDOFRigidBody::initializeDynamics() { + this->positionState_N = this->dynManager.registerState(3, 1, this->nameOfBodyPosition); + this->velocityState_N = this->dynManager.registerState(3, 1, this->nameOfBodyVelocity); + this->sigmaState = this->dynManager.registerState(3, 1, this->nameOfBodySigma); + this->omegaState_B = this->dynManager.registerState(3, 1, this->nameOfBodyOmega); + + // Get attitude and angular velocity + auto sigma_BN = this->baseFrame->sigma_SP->getAttitude(); + auto omega_BN = this->baseFrame->sigma_SP->getAngularVelocity(); + + // Get position and velocity from the center of mass + auto r_CN = this->baseFrame->r_SP->getPosition() + this->body->r_ScS->getPosition(); + auto rDot_CN = this->baseFrame->r_SP->getVelocity(this->inertialFrame) + + this->body->r_ScS->getVelocity(this->inertialFrame); + + // Need to change this to account for center of mass offset + this->positionState_N->setState(r_CN.getMatrix(this->inertialFrame)); + this->velocityState_N->setState(rDot_CN.getMatrix(this->inertialFrame)); + this->sigmaState->setState(eigenMRPd2Vector3d(sigma_BN)); + this->omegaState_B->setState(omega_BN.getMatrix(this->baseFrame)); + + this->equationsOfMotion(0.0, 0.0); // sets the derivatives at t0 + this->updateKinematics(); + this->computeEnergyMomentum(0.0); +} + +void SixDOFRigidBody::preIntegration(double callTime) { + this->timeStep = callTime - this->timeBefore; +} + +void SixDOFRigidBody::equationsOfMotion(double t, double timeStep) { + this->updateKinematics(); + + // Grab the states + auto rDot_CN = Vector(this->velocityState_N->getState(), this->inertialFrame); + auto omega_BN = Vector(this->omegaState_B->getState(), this->baseFrame); + Eigen::MRPd sigma_BN; + sigma_BN = (Eigen::Vector3d)this->sigmaState->getState(); // this conversion is so dumb + + auto LPntC = Vector(Eigen::Vector3d::Zero(), this->baseFrame); + auto F = Vector(Eigen::Vector3d::Zero(), this->inertialFrame); + + auto omegaDot_BN = this->IScPntC.inverse() * (-omega_BN.cross(this->IScPntC * omega_BN) + LPntC); + auto rDDot_CN = F / this->mass; + + this->sigmaState->setDerivative(1.0/4.0 * sigma_BN.Bmat() * omega_BN.getMatrix(this->baseFrame)); + this->omegaState_B->setDerivative(omegaDot_BN.getMatrix(this->baseFrame)); + + this->positionState_N->setDerivative(rDot_CN.getMatrix(this->inertialFrame)); + this->velocityState_N->setDerivative(rDDot_CN.getMatrix(this->inertialFrame)); +} + +void SixDOFRigidBody::postIntegration(double callTime) { + this->timeBefore = callTime; + + // Switch MRPs + Eigen::Vector3d sigma_BN; + sigma_BN = (Eigen::Vector3d) this->sigmaState->getState(); + if (sigma_BN.norm() > 1) { + sigma_BN = - sigma_BN / (sigma_BN.dot(sigma_BN)); + this->sigmaState->setState(sigma_BN); + } + + this->updateKinematics(); + this->computeEnergyMomentum(callTime); +} + +void SixDOFRigidBody::computeEnergyMomentum(double time) { + auto omega_BN = Vector(this->omegaState_B->getState(), this->baseFrame); + + this->rotAngMomPntC_N = (this->IScPntC * omega_BN).getMatrix(this->inertialFrame); + this->rotEnergy = 1.0 / 2.0 * omega_BN.dot(this->IScPntC * omega_BN); +} + +void SixDOFRigidBody::updateKinematics() { + auto r_CN = Vector(this->positionState_N->getState(), this->inertialFrame); + auto rDot_CN = Vector(this->velocityState_N->getState(), this->inertialFrame); + auto omega_BN = Vector(this->omegaState_B->getState(), this->baseFrame); + Eigen::MRPd sigma_BN; + sigma_BN = (Eigen::Vector3d)this->sigmaState->getState(); // this conversion is so dumb + + auto r_BN = r_CN - this->body->r_ScS->getPosition(); + auto rDot_BN = rDot_CN - this->body->r_ScS->getVelocity(this->inertialFrame); + + this->baseFrame->r_SP->setPosition(r_BN); + this->baseFrame->r_SP->setVelocity(rDot_BN, this->inertialFrame); + this->baseFrame->sigma_SP->setAttitude(sigma_BN); + this->baseFrame->sigma_SP->setAngularVelocity(omega_BN); +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.h b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.h new file mode 100755 index 0000000000..71332b97b3 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.h @@ -0,0 +1,70 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef SIX_DOF_RIGID_BODY_H +#define SIX_DOF_RIGID_BODY_H + +#include + +#include "DynamicsEngine.h" +#include "simulation/dynamics/_GeneralModuleFiles/stateData.h" + +class SixDOFRigidBody: public DynamicsEngine { +public: + explicit SixDOFRigidBody(std::shared_ptr kinematicsEngine, std::shared_ptr inertialFrame, std::shared_ptr body); + + // SysModel functions + void Reset(uint64_t CurrentSimNanos) final; + void UpdateState(uint64_t callTime) final; + + // DynamicObject functions + void equationsOfMotion(double t, double timeStep) final; + void preIntegration(double callTime) final; + void postIntegration(double callTime) final; + void initializeDynamics() final; + void computeEnergyMomentum(double time) final; + + // DynamicEngine functions + void updateKinematics() final; + + void writeOutputMessages(uint64_t callTime); + + std::string nameOfBodyPosition {"bodyPosition"}; + std::string nameOfBodyVelocity {"bodyVelocity"}; + std::string nameOfBodySigma {"bodySigma"}; + std::string nameOfBodyOmega {"bodyOmega"}; + + double rotEnergy = 0.0; + Eigen::Vector3d rotAngMomPntC_N = Eigen::Vector3d::Zero(); + +private: + StateData* sigmaState = nullptr; + StateData* omegaState_B = nullptr; + StateData* positionState_N = nullptr; + StateData* velocityState_N = nullptr; + + double mass = 1.0; + InertiaTensor IScPntC; + + std::shared_ptr body; + std::shared_ptr baseFrame; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.i b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.i new file mode 100755 index 0000000000..91488c485f --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.i @@ -0,0 +1,43 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%module SixDOFRigidBody +%{ + #include "simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.h" + +%} + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include "std_vector.i" + +%include "sys_model.h" +%include "simulation/dynamics/_GeneralModuleFiles/stateData.h" +%include "simulation/dynamics/_GeneralModuleFiles/dynParamManager.h" +%include "simulation/dynamics/_GeneralModuleFiles/dynamicObject.h" +%include "simulation/dynamics/KinematicsArchitecture/DynamicsEngine.h" +%include "simulation/dynamics/KinematicsArchitecture/SixDOFRigidBody.h" + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Tensor.cpp b/src/simulation/dynamics/KinematicsArchitecture/Tensor.cpp new file mode 100755 index 0000000000..31a2c959da --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Tensor.cpp @@ -0,0 +1,146 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "Tensor.h" +#include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h" + +#include + +Tensor::Tensor(Eigen::Matrix3d newMatrix, const std::shared_ptr& newWrittenFrame) : matrix(std::move(newMatrix)), writtenFrame(newWrittenFrame) { +} + +void Tensor::set(Eigen::Matrix3d newMatrix, const std::shared_ptr& newWrittenFrame) { + this->matrix = std::move(newMatrix); + this->writtenFrame = newWrittenFrame; +} + +Eigen::Matrix3d Tensor::getMatrix(const std::shared_ptr& frame) const { + Eigen::MRPd relativeAttitude = KinematicsEngine::findRelativeAttitude(frame, this->writtenFrame); + Eigen::Matrix3d dcm = relativeAttitude.toRotationMatrix().transpose(); + + return dcm * this->matrix * dcm.transpose(); +} + +std::shared_ptr Tensor::getWrittenFrame() const { + return this->writtenFrame; +} + +Tensor Tensor::operator+(const Tensor& tensor) const { + Eigen::Matrix3d addMatrix = this->matrix + tensor.getMatrix(this->writtenFrame); + auto addTensor = Tensor(addMatrix, this->writtenFrame); + + return addTensor; +} + +void Tensor::operator+=(const Tensor& tensor) { + this->matrix += tensor.getMatrix(this->writtenFrame); +} + +Tensor Tensor::operator-() const { + Eigen::Matrix3d negativeMat = - this->matrix; + auto negativeTensor = Tensor(negativeMat, this->writtenFrame); + + return negativeTensor; +} + +Tensor Tensor::operator-(const Tensor& tensor) const { + Eigen::Matrix3d subtractMatrix = this->matrix - tensor.getMatrix(this->writtenFrame); + auto subtractTensor = Tensor(subtractMatrix, this->writtenFrame); + + return subtractTensor; +} + +void Tensor::operator-=(const Tensor& tensor) { + this->matrix -= tensor.getMatrix(this->writtenFrame); +} + +Vector Tensor::operator*(const Vector& vector) const { + Eigen::Vector3d multiplyMatrix = this->matrix * vector.getMatrix(this->writtenFrame); + auto multiplyVector = Vector(multiplyMatrix, this->writtenFrame); + + return multiplyVector; +} + +Tensor Tensor::operator*(const Tensor& tensor) const { + Eigen::Matrix3d multiplyMatrix = this->matrix * tensor.getMatrix(this->writtenFrame); + auto multiplyTensor = Tensor(multiplyMatrix, this->writtenFrame); + + return multiplyTensor; +} + +void Tensor::operator*=(const Tensor& tensor) { + this->matrix *= tensor.getMatrix(this->writtenFrame); +} + +Tensor Tensor::operator*(double factor) const { + Eigen::Matrix3d multiplyMatrix = this->matrix * factor; + auto multiplyTensor = Tensor(multiplyMatrix, this->writtenFrame); + + return multiplyTensor; +} + +Tensor operator*(double factor, const Tensor& tensor) { + return tensor * factor; +} + +void Tensor::operator*=(double factor) { + this->matrix *= factor; +} + +Tensor Tensor::operator/(double factor) const { + Eigen::Matrix3d divideMatrix = this->matrix / factor; + auto divideTensor = Tensor(divideMatrix, this->writtenFrame); + + return divideTensor; +} + +void Tensor::operator/=(double factor) { + this->matrix /= factor; +} + +Tensor Tensor::inverse() const { + Eigen::Matrix3d inverseMatrix = this->matrix.inverse(); + auto inverseTensor = Tensor(inverseMatrix, this->writtenFrame); + + return inverseTensor; +} + +Tensor Tensor::transpose() const { + Eigen::Matrix3d transposeMatrix = this->matrix.transpose(); + auto transposeTensor = Tensor(transposeMatrix, this->writtenFrame); + + return transposeTensor; +} + +InertiaTensor::InertiaTensor(std::shared_ptr point):point(std::move(point)) { +} + +void InertiaTensor::setFirstOrder(const Eigen::Matrix3d& newMatrix, const std::shared_ptr& newWrittenFrame, const std::shared_ptr& newDerivFrame) { + this->firstOrder.matrix = newMatrix; + this->firstOrder.writtenFrame = newWrittenFrame; + this->firstOrder.derivFrame = newDerivFrame; +} + +void InertiaTensor::setPoint(const std::shared_ptr& newPoint) { + this->point = newPoint; +} + +std::shared_ptr InertiaTensor::getPoint() const { + return this->point; +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Tensor.h b/src/simulation/dynamics/KinematicsArchitecture/Tensor.h new file mode 100755 index 0000000000..23e5dc245d --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Tensor.h @@ -0,0 +1,80 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef TENSOR_H +#define TENSOR_H + +#include "simulation/dynamics/KinematicsArchitecture/Frame.h" +#include "simulation/dynamics/KinematicsArchitecture/Point.h" +#include + +class Tensor { +public: + Tensor() = default; + ~Tensor() = default; + Tensor(Eigen::Matrix3d newMatrix, const std::shared_ptr& newWrittenFrame); // do we change this to const std::shared_ptr&? + + void set(Eigen::Matrix3d newMatrix, const std::shared_ptr& newWrittenFrame); + Eigen::Matrix3d getMatrix(const std::shared_ptr& frame) const; + std::shared_ptr getWrittenFrame() const; + + Tensor operator+(const Tensor& tensor) const; + void operator+=(const Tensor& tensor); + Tensor operator-() const; + Tensor operator-(const Tensor& tensor) const; + void operator-=(const Tensor& tensor); + Vector operator*(const Vector& vector) const; + Tensor operator*(const Tensor& tensor) const; + void operator*=(const Tensor& tensor); + Tensor operator*(double factor) const; + void operator*=(double factor); + Tensor operator/(double factor) const; + void operator/=(double factor); + Tensor inverse() const; + Tensor transpose() const; + +private: + Eigen::Matrix3d matrix = Eigen::Matrix3d::Zero(); + std::shared_ptr writtenFrame; +}; + +Tensor operator*(double factor, const Tensor& tensor); // this is throwing a warning + +struct TensorDerivativeProperties { + Eigen::Matrix3d matrix = Eigen::Matrix3d::Zero(); + + std::weak_ptr writtenFrame; + std::weak_ptr derivFrame; +}; + +class InertiaTensor: public Tensor { +public: + InertiaTensor() = default; + explicit InertiaTensor(std::shared_ptr point); + ~InertiaTensor() = default; + + void setFirstOrder(const Eigen::Matrix3d& newMatrix, const std::shared_ptr& newWrittenFrame, const std::shared_ptr& newDerivFrame); + void setPoint(const std::shared_ptr& newPoint); + std::shared_ptr getPoint() const; + + std::shared_ptr point; + TensorDerivativeProperties firstOrder; +}; + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Tensor.i b/src/simulation/dynamics/KinematicsArchitecture/Tensor.i new file mode 100644 index 0000000000..67d9215b53 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Tensor.i @@ -0,0 +1,39 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" +%include + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Tensor.h" +%} + +%shared_ptr(Tensor) +%shared_ptr(InertiaTensor) + +%include "simulation/dynamics/KinematicsArchitecture/Tensor.h" + +%pythoncode %{ + import sys + protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Vector.cpp b/src/simulation/dynamics/KinematicsArchitecture/Vector.cpp new file mode 100755 index 0000000000..a174901329 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Vector.cpp @@ -0,0 +1,279 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "Vector.h" +#include "simulation/dynamics/KinematicsArchitecture/KinematicsEngine.h" + +#include + +Vector::Vector(Eigen::Vector3d newMatrix, const std::shared_ptr& newWrittenFrame) : matrix(std::move(newMatrix)), writtenFrame(newWrittenFrame) { +} + +void Vector::set(Eigen::Vector3d newMatrix, const std::shared_ptr& newWrittenFrame) { + this->matrix = std::move(newMatrix); + this->writtenFrame = newWrittenFrame; +} + +Eigen::Vector3d Vector::getMatrix(const std::shared_ptr& frame) const { + Eigen::MRPd relativeAttitude = KinematicsEngine::findRelativeAttitude(frame, this->writtenFrame.lock()); + Eigen::Matrix3d dcm = relativeAttitude.toRotationMatrix().transpose(); + + return dcm * this->matrix; +} + +std::shared_ptr Vector::getWrittenFrame() const { + return this->writtenFrame.lock(); +} + +Vector Vector::operator+(const Vector& vec) const{ + Eigen::Vector3d addMatrix = this->matrix + vec.getMatrix(this->writtenFrame.lock()); + auto addVector = Vector(addMatrix, this->writtenFrame.lock()); + + return addVector; +} + +void Vector::operator+=(const Vector& vec){ + this->matrix += vec.getMatrix(this->writtenFrame.lock()); +} + +Vector Vector::operator-() const { + Eigen::Vector3d negativeMatrix = - this->matrix; + auto negativeVector = Vector(negativeMatrix, this->writtenFrame.lock()); + + return negativeVector; +} + +Vector Vector::operator-(const Vector& vec) const { + Eigen::Vector3d subtractMatrix = this->matrix - vec.getMatrix(this->writtenFrame.lock()); + auto subtractVector = Vector(subtractMatrix, this->writtenFrame.lock()); + + return subtractVector; +} + +void Vector::operator-=(const Vector& vec){ + this->matrix -= vec.getMatrix(this->writtenFrame.lock()); +} + +Vector Vector::operator*(double factor) const { + Eigen::Vector3d multiplyMatrix = factor * this->matrix; + auto multiplyVector = Vector(multiplyMatrix, this->writtenFrame.lock()); + + return multiplyVector; +} + +Vector operator*(double factor, const Vector& vector) { + return vector * factor; +} + +void Vector::operator*=(double factor){ + this->matrix *= factor; +} + +Vector Vector::operator/(double factor) const { + Eigen::Vector3d divideMatrix = this->matrix / factor; + auto divideVector = Vector(divideMatrix, this->writtenFrame.lock()); + + return divideVector; +} + +void Vector::operator/=(double factor){ + this->matrix /= factor; +} + +double Vector::dot(const Vector& vec) const { + return this->matrix.dot(vec.getMatrix(this->writtenFrame.lock())); +} + +Vector Vector::cross(const Vector& vec) const { + Eigen::Vector3d crossMatrix = this->matrix.cross(vec.getMatrix(this->writtenFrame.lock())); + auto crossVector = Vector(crossMatrix, this->writtenFrame.lock()); + + return crossVector; +} + +UnitVector::UnitVector(const Eigen::Vector3d& zerothMatrix, + const std::weak_ptr& zerothWrittenFrame) { + this->set(zerothMatrix.normalized(), zerothWrittenFrame.lock()); +} + +void Translation::setPosition(const Eigen::Vector3d& matrix, const std::shared_ptr& writtenFrame) { + this->position.set(matrix, writtenFrame); +} + +void Translation::setPosition(const Vector& newPosition) { + this->position = newPosition; +} + +void Translation::setVelocity(const Eigen::Vector3d& matrix, + const std::shared_ptr& newWrittenFrame, + const std::shared_ptr& newFirstDerivFrame) { + auto newVelocity = Vector(matrix, newWrittenFrame); + this->setVelocity(newVelocity, newFirstDerivFrame); +} + +// We're currently only changing this->firstDerivFrame if it's nullptr because it would mess with the acceleration too, but this feels funky +void Translation::setVelocity(const Vector& newVelocity, const std::shared_ptr& newFirstDerivFrame) { + if (!this->firstDerivFrame) { + this->velocity = newVelocity; + this->firstDerivFrame = newFirstDerivFrame; + } else { + const auto& frameA = this->firstDerivFrame; + const auto& frameB = newFirstDerivFrame; + + const auto& r = this->position; + const auto& rPrimeB = newVelocity; + const auto& omega_AB = KinematicsEngine::findRelativeAngularVelocity(frameA, frameB); + this->velocity = rPrimeB - omega_AB.cross(r); + } +} + +void Translation::setAcceleration(const Eigen::Vector3d& matrix, + const std::shared_ptr& newWrittenFrame, + const std::shared_ptr& newFirstDerivFrame, + const std::shared_ptr& newSecondDerivFrame) { + auto newAcceleration = Vector(matrix, newWrittenFrame); + this->setAcceleration(newAcceleration, newFirstDerivFrame, newSecondDerivFrame); +} + +void Translation::setAcceleration(const Vector& newAcceleration, + const std::shared_ptr& newFirstDerivFrame, + const std::shared_ptr& newSecondDerivFrame) { + if (!this->secondDerivFrame) { + this->secondDerivFrame = newSecondDerivFrame; + if (!this->firstDerivFrame) { + this->firstDerivFrame = newFirstDerivFrame; + this->acceleration = newAcceleration; + + return; + } + } + const auto& frameA = this->firstDerivFrame; + const auto& frameB = this->secondDerivFrame; + const auto& frameC = newFirstDerivFrame; + const auto& frameD = newSecondDerivFrame; + + const auto& r = this->position; + const auto& rPrimeC = this->getVelocity(frameC); + const auto& rPPrimeDC = newAcceleration; + const auto& omega_BD = KinematicsEngine::findRelativeAngularVelocity(frameB, frameD); + const auto& omegaPrimeB_AC = KinematicsEngine::findRelativeAngularAcceleration(frameA, frameC, frameB); + + this->acceleration = rPPrimeDC - omegaPrimeB_AC.cross(r) - omega_BD.cross(rPrimeC); + +} + +Vector Translation::getPosition() const { + return position; +} + +Vector Translation::getVelocity(const std::shared_ptr& newFirstDerivFrame) const { + const auto& frameA = this->firstDerivFrame; + const auto& frameB = newFirstDerivFrame; + + const auto& r = this->position; + const auto& rPrimeA = this->velocity; + const auto& omega_AB = KinematicsEngine::findRelativeAngularVelocity(frameA, frameB); + + return rPrimeA + omega_AB.cross(r); +} + +Vector Translation::getAcceleration(const std::shared_ptr& newFirstDerivFrame, const std::shared_ptr& newSecondDerivFrame) const { + const auto& frameA = this->firstDerivFrame; + const auto& frameB = this->secondDerivFrame; + const auto& frameC = newFirstDerivFrame; + const auto& frameD = newSecondDerivFrame; + + const auto& r = this->position; + const auto& rPrimeC = this->getVelocity(frameC); + const auto& rPPrimeBA = this->acceleration; + const auto& omega_BD = KinematicsEngine::findRelativeAngularVelocity(frameB, frameD); + const auto& omegaPrimeB_AC = KinematicsEngine::findRelativeAngularAcceleration(frameA, frameC, frameB); + + return rPPrimeBA + omegaPrimeB_AC.cross(r) + omega_BD.cross(rPrimeC); +} + +std::shared_ptr Translation::getHeadPoint() const{ + return this->headPoint; +} + +std::shared_ptr Translation::getTailPoint() const{ + return this->tailPoint; +} + +void Rotation::setAttitude(const Eigen::MRPd& matrix) { + this->sigma = matrix; +} + +void Rotation::setAngularVelocity(const Eigen::Vector3d& matrix, const std::shared_ptr& newWrittenFrame) { + auto newAngularVelocity = Vector(matrix, newWrittenFrame); + this->setAngularVelocity(newAngularVelocity); +} + +void Rotation::setAngularVelocity(const Vector& newAngularVelocity) { + this->angularVelocity = newAngularVelocity; +} + +void Rotation::setAngularAcceleration(const Eigen::Vector3d& matrix, const std::shared_ptr& newWrittenFrame, + const std::shared_ptr& newDerivFrame) { + auto newAngularAcceleration = Vector(matrix, newWrittenFrame); + this->setAngularAcceleration(newAngularAcceleration, newDerivFrame); +} + +void Rotation::setAngularAcceleration(const Vector& newAngularAcceleration, const std::shared_ptr& newDerivFrame) { + if (!derivFrame) { + this->angularAcceleration = newAngularAcceleration; + this->derivFrame = newDerivFrame; + } else { + const auto& frameA = this->derivFrame; + const auto& frameB = newDerivFrame; + + const auto& omega = this->angularVelocity; + const auto& omega_AB = KinematicsEngine::findRelativeAngularVelocity(frameA, frameB); + const auto& omegaPrimeB = newAngularAcceleration; + + this->angularAcceleration = omegaPrimeB - omega_AB.cross(omega); + } +} + +Eigen::MRPd Rotation::getAttitude() const { + return this->sigma; +} + +Vector Rotation::getAngularVelocity() const { + return this->angularVelocity; +} + +Vector Rotation::getAngularAcceleration(const std::shared_ptr& newDerivFrame) const { + const auto& frameA = this->derivFrame; + const auto& frameB = newDerivFrame; + + const auto& omega = this->angularVelocity; + const auto& omega_AB = KinematicsEngine::findRelativeAngularVelocity(frameA, frameB); + const auto& omegaPrimeA = this->angularAcceleration; + + return omegaPrimeA + omega_AB.cross(omega); +} + +std::shared_ptr Rotation::getUpperFrame() const { + return this->upperFrame; +} + +std::shared_ptr Rotation::getLowerFrame() const { + return this->lowerFrame; +} diff --git a/src/simulation/dynamics/KinematicsArchitecture/Vector.h b/src/simulation/dynamics/KinematicsArchitecture/Vector.h new file mode 100755 index 0000000000..31b3a7cf4a --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Vector.h @@ -0,0 +1,137 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef VECTOR_H +#define VECTOR_H +#include +#include +#include "architecture/utilities/avsEigenMRP.h" +#include "architecture/utilities/avsEigenSupport.h" + +class Frame; +class Point; + +class Vector { +public: + Vector() = default; + ~Vector() = default; + Vector(Eigen::Vector3d newMatrix, const std::shared_ptr& newWrittenFrame); // do we change this to const std::shared_ptr&? + + void set(Eigen::Vector3d newMatrix, const std::shared_ptr& newWrittenFrame); + Eigen::Vector3d getMatrix(const std::shared_ptr& frame) const; + std::shared_ptr getWrittenFrame() const; + + Vector operator+(const Vector& vec) const; + void operator+=(const Vector& vec); + Vector operator-() const; + Vector operator-(const Vector& vec) const; + void operator-=(const Vector& vec); + Vector operator*(double factor) const; + void operator*=(double factor); + Vector operator/(double factor) const; + void operator/=(double factor); + double dot(const Vector& vec) const; + Vector cross(const Vector& vec) const; + +private: + Eigen::Vector3d matrix = Eigen::Vector3d::Zero(); + std::weak_ptr writtenFrame; +}; + +Vector operator*(double factor, const Vector& vector); // this is throwing a warning + + +class Translation { +public: + Translation(std::shared_ptr headPoint, std::shared_ptr tailPoint) : headPoint(std::move(headPoint)), tailPoint(std::move(tailPoint)) {}; + ~Translation() = default; + + void setPosition(const Eigen::Vector3d& matrix, const std::shared_ptr& writtenFrame); // sets position + void setPosition(const Vector& newPosition); // sets position + void setVelocity(const Eigen::Vector3d& matrix, const std::shared_ptr& newWrittenFrame, const std::shared_ptr& newFirstDerivFrame); // sets velocity + void setVelocity(const Vector& newVelocity, const std::shared_ptr& newFirstDerivFrame); // sets velocity + void setAcceleration(const Eigen::Vector3d& matrix, const std::shared_ptr& newWrittenFrame, const std::shared_ptr& newFirstDerivFrame, const std::shared_ptr& newSecondDerivFrame); // sets acceleration + void setAcceleration(const Vector& newAcceleration, const std::shared_ptr& newFirstDerivFrame, const std::shared_ptr& newSecondDerivFrame); // sets acceleration + Vector getPosition() const; // gets position + Vector getVelocity(const std::shared_ptr& newFirstDerivFrame) const; // gets velocity + Vector getAcceleration(const std::shared_ptr& newFirstDerivFrame, const std::shared_ptr& newSecondDerivFrame) const; // sets acceleration, but stores it such that it is consistent with the firstDerivFrame, has to call TransportTheorem + + std::shared_ptr getHeadPoint() const; + std::shared_ptr getTailPoint() const; + +private: + Vector position; + Vector velocity; + Vector acceleration; + + std::shared_ptr firstDerivFrame = nullptr; + std::shared_ptr secondDerivFrame = nullptr; + + std::shared_ptr headPoint = nullptr; + std::shared_ptr tailPoint = nullptr; +}; + + +class Rotation { +public: + Rotation(std::shared_ptr upperFrame, std::shared_ptr lowerFrame) : upperFrame(std::move(upperFrame)), lowerFrame(std::move(lowerFrame)) {}; + ~Rotation() = default; + + void setAttitude(const Eigen::MRPd& matrix); // sets attitude + void setAngularVelocity(const Eigen::Vector3d& matrix, const std::shared_ptr& newWrittenFrame); // sets angular velocity + void setAngularVelocity(const Vector& newAngularVelocity); // sets angular velocity + void setAngularAcceleration(const Eigen::Vector3d& matrix, const std::shared_ptr& writtenFrame, const std::shared_ptr& derivFrame); // sets angular acceleration + void setAngularAcceleration(const Vector& newAngularAcceleration, const std::shared_ptr& newDerivFrame); // sets angular acceleration + Eigen::MRPd getAttitude() const; // gets attitude + Vector getAngularVelocity() const; // gets angular velocity + Vector getAngularAcceleration(const std::shared_ptr& newDerivFrame) const; // sets angular acceleration + + std::shared_ptr getUpperFrame() const; + std::shared_ptr getLowerFrame() const; + +private: + Eigen::MRPd sigma; + Vector angularVelocity; + Vector angularAcceleration; + + std::shared_ptr derivFrame = nullptr; + + std::shared_ptr upperFrame = nullptr; + std::shared_ptr lowerFrame = nullptr; +}; + + +class ForceVector : public Vector { +public: + ForceVector() = default; + ~ForceVector() = default; + + std::shared_ptr applicationPoint; +}; + + +class UnitVector : public Vector { +public: + UnitVector() = default; + UnitVector(const Eigen::Vector3d& zerothMatrix, const std::weak_ptr& zerothWrittenFrame); + ~UnitVector() = default; +}; + + +#endif diff --git a/src/simulation/dynamics/KinematicsArchitecture/Vector.i b/src/simulation/dynamics/KinematicsArchitecture/Vector.i new file mode 100644 index 0000000000..b2b7e38203 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/Vector.i @@ -0,0 +1,43 @@ +/* + ISC License + + Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_eigen.i" +%include "swig_conly_data.i" + +%include +%shared_ptr(Vector) +%shared_ptr(Translation) +%shared_ptr(Rotation) +%shared_ptr(ForceVector) +%shared_ptr(UnitVector) + +%{ + #include "simulation/dynamics/KinematicsArchitecture/Vector.h" +%} + +%include "simulation/dynamics/KinematicsArchitecture/Vector.h" + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/scenario_Kinematics.py b/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/scenario_Kinematics.py new file mode 100644 index 0000000000..953bef341e --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/scenario_Kinematics.py @@ -0,0 +1,367 @@ +import os +from Basilisk import __path__ + +bskPath = __path__[0] +fileName = os.path.basename(os.path.splitext(__file__)[0]) + +import numpy as np + +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.simulation.KinematicsArchitecture import KinematicsEngine +from Basilisk.simulation.KinematicsArchitecture import Frame +from Basilisk.simulation.KinematicsArchitecture import Vector +from Basilisk.simulation.KinematicsArchitecture import Tensor +from Basilisk.simulation.KinematicsArchitecture import Part +from Basilisk.simulation.KinematicsArchitecture import Joint +from Basilisk.simulation.KinematicsArchitecture import Hinge + +def runLeah(): + myKinematicsEngine = KinematicsEngine() + + # Create frames + frameN = myKinematicsEngine.createFrame() + frameB = myKinematicsEngine.createFrame(frameN) + frameM = myKinematicsEngine.createFrame(frameB) + + # Create parts + part1Inertia = [[10, 0, 0], [0, 10, 0], [0, 0, 10]] + part2Inertia = [[10, 0, 0], [0, 8, 0], [0, 0, 5]] + part1 = myKinematicsEngine.createPart(frameM) + part2 = myKinematicsEngine.createPart(frameB) + + # Create part data + frameF = part1.frame + frameP = part2.frame + part1.IPntSc_S.set(part1Inertia, frameF) + part2.IPntSc_S.set(part2Inertia, frameP) + + # Create assembly + myAssembly = myKinematicsEngine.createAssembly() + myAssembly.addPart(part1) + myAssembly.addPart(part2) + + # Create position vector data + r_PcP_P = [0, 2, 1] # [m] + r_FcF_F = [1, 2, 0] # [m] + r_BN_N = [1.0, 2.0, 3.0] # [m] + r_PB_B = [2.0, 3.0, 4.0] # [m] + r_MB_B = [2.0, 1.0, 3.0] # [m] + r_FM_M = [4.0, 3.0, 1.0] # [m] + + # Create frame attitude data + theta_BN = 30 * np.pi / 180 # [rad] + theta_PB = 10 * np.pi / 180 # [rad] + theta_MB = -5 * np.pi / 180 # [rad] + theta_FM = -10 * np.pi / 180 # [rad] + + dcm_BN = np.array([[1, 0, 0], + [0, np.cos(theta_BN), np.sin(theta_BN)], + [0, -np.sin(theta_BN), np.cos(theta_BN)]]) + dcm_PB = np.array([[np.cos(theta_PB), np.sin(theta_PB), 0], + [-np.sin(theta_PB), np.cos(theta_PB), 0], + [0, 0, 1]]) + dcm_MB = np.array([[np.cos(theta_MB), np.sin(theta_MB), 0], + [-np.sin(theta_MB), np.cos(theta_MB), 0], + [0, 0, 1]]) + dcm_FM = np.array([[np.cos(theta_FM), 0, -np.sin(theta_FM)], + [0, 1, 0], + [np.sin(theta_FM), 0, np.cos(theta_FM)]]) + + sigma_BN = rbk.C2MRP(dcm_BN) + sigma_PB = rbk.C2MRP(dcm_PB) + sigma_MB = rbk.C2MRP(dcm_MB) + sigma_FM = rbk.C2MRP(dcm_FM) + + # Create angular velocity data + omega_PB_P = [0.5, 1, 0.5] # [rad/s] + omega_FM_F = [-1.0, -2.0, 3.0] # [rad/s] + omega_MB_M = [-2.0, -3.0, -4.0] # [rad/s] + omega_BN_B = [0.0, 1, 0.0] # [rad/s] + + # Create frame data + frameB.r_SP.setPosition(r_BN_N, frameN) + frameM.r_SP.setPosition(r_MB_B, frameB) + frameF.r_SP.setPosition(r_FM_M, frameM) + frameP.r_SP.setPosition(r_PB_B, frameB) + frameB.sigma_SP.setAttitude(sigma_BN) + frameP.sigma_SP.setAttitude(sigma_PB) + frameM.sigma_SP.setAttitude(sigma_MB) + frameF.sigma_SP.setAttitude(sigma_FM) + frameB.sigma_SP.setAngularVelocity(omega_BN_B, frameB) + frameP.sigma_SP.setAngularVelocity(omega_PB_P, frameP) + frameM.sigma_SP.setAngularVelocity(omega_MB_M, frameM) + frameF.sigma_SP.setAngularVelocity(omega_FM_F, frameF) + + frameB.tag = "frameB" + frameM.tag = "frameM" + frameF.tag = "frameF" + frameP.tag = "frameP" + frameN.tag = "frameN" + frameB.originPoint.tag = "pointB" + frameP.originPoint.tag = "pointP" + frameM.originPoint.tag = "pointM" + frameF.originPoint.tag = "pointF" + frameN.originPoint.tag = "pointN" + + # Create part data + m1 = 10 # [kg] + m2 = 20 # [kg] + part1.mass = m1 + part2.mass = m2 + part1.r_ScS.setPosition(r_FcF_F, frameF) + part2.r_ScS.setPosition(r_PcP_P, frameP) + part1.r_ScS.getHeadPoint().tag = "pointFc" + part2.r_ScS.getHeadPoint().tag = "pointPc" + + # Create velocity data + rBPrime_PcP_M = [0.1, 0, 0.2] + part2.r_ScS.setVelocity(rBPrime_PcP_M, frameM, frameB) + + # Create inertia derivative data + IBPrime_PntPc_M = [[1, 0, 0], [0, 0.1, 0], [0, 0, 0.2]] + part2.IPntSc_S.setFirstOrder(IBPrime_PntPc_M, frameM, frameB) + + # Call kinematic engine functions + sigma_FPKin = myKinematicsEngine.findRelativeAttitude(frameF, frameP) + # r_FBKin = frameF.r_SP.getPosition().add(frameM.r_SP.getPosition()) + # r_FB_BKin = r_FBKin.getMatrix(frameB) + # omega_FBKin = frameF.sigma_SP.getAngularVelocity().add(frameM.sigma_SP.getAngularVelocity()) + # omega_FB_MKin = omega_FBKin.getMatrix(frameM) + r_PcF_Kin = myKinematicsEngine.findRelativePosition(part2.r_ScS.getHeadPoint(), frameF.getOriginPoint()) + r_PcF_MKin = r_PcF_Kin.getMatrix(frameM) + omega_PFKin = myKinematicsEngine.findRelativeAngularVelocity(frameP, frameF) + omega_PF_MKin = omega_PFKin.getMatrix(frameM) + assemblyMassKin = myKinematicsEngine.getAssemblyMass(myAssembly) + r_AssemCMPntBKin = myKinematicsEngine.getAssemblyCOM(myAssembly, frameB.getOriginPoint()) + r_AssemCMPntB_MKin = r_AssemCMPntBKin.getMatrix(frameM) + IPart1PntBKin = myKinematicsEngine.parallelAxisTheorem(part1, frameB.getOriginPoint()) + IPart1PntB_MKin = IPart1PntBKin.getMatrix(frameM) + IAssemPntBKin = myKinematicsEngine.getAssemblyInertia(myAssembly, frameB.getOriginPoint()) + IAssemPntB_MKin = IAssemPntBKin.getMatrix(frameM) + rFPrime_PcP_Kin = part2.r_ScS.getVelocity(frameF) + rFPrime_PcP_NKin = rFPrime_PcP_Kin.getMatrix(frameN) + # IFPrime_PntPc_Kin = myKinematicsEngine.getFirstOrder(part2.IPntSc_S, frameF) + # IFPrime_PntPc_NKin = IFPrime_PntPc_Kin.getMatrix(frameN) + + # Calculate the truth data + sigma_FP = [0.0, 0.0, 0.0] + r_FB_B = [0.0, 0.0, 0.0] + omega_FB_M = [0.0, 0.0, 0.0] + r_PcF_M = [0.0, 0.0, 0.0] + IPart1PntB_M = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] + + # 1. Calculate sigma_FP + dcm_FP = np.matmul(np.matmul(dcm_FM, dcm_MB), np.transpose(dcm_PB)) + sigma_FP = rbk.C2MRP(dcm_FP) + + # 2. Calculate r_FB_B + r_FM_B = np.matmul(np.transpose(dcm_MB), r_FM_M) + r_FB_B = np.add(r_FM_B, r_MB_B) + + # 3. Calculate omega_FB_M + omega_FM_M = np.matmul(np.transpose(dcm_FM), omega_FM_F) + omega_FB_M = np.add(omega_FM_M, omega_MB_M) + + # 4. Calculate r_PcF_M + r_PcP_M = np.matmul(np.matmul(dcm_MB, np.transpose(dcm_PB)), r_PcP_P) + r_PB_M = np.matmul(dcm_MB, r_PB_B) + r_PcB_M = np.add(r_PcP_M, r_PB_M) + r_MB_M = np.matmul(dcm_MB, r_MB_B) + r_PcM_M = np.subtract(r_PcB_M, r_MB_M) + r_PcF_M = np.subtract(r_PcM_M, r_FM_M) + + # 5. Calculate omega_PF_M + dcm_PM = np.matmul(dcm_PB, dcm_MB.transpose()) + omega_PF_M = np.subtract(np.subtract(np.matmul(dcm_PM.transpose(), omega_PB_P), omega_MB_M), np.matmul(dcm_FM.transpose(), omega_FM_F)) + + # 6. Calculate IPart1PntB_M + rFcB_M = np.add(np.add(np.matmul(np.transpose(dcm_FM), r_FcF_F), r_FM_M), np.matmul(dcm_MB, r_MB_B)) + rTildeFcB_M = [[0, -rFcB_M[2], rFcB_M[1]], + [rFcB_M[2], 0, -rFcB_M[0]], + [-rFcB_M[1], rFcB_M[0], 0]] + IPart1PntB_M = np.matmul(np.matmul(np.transpose(dcm_FM), part1Inertia), dcm_FM) + m1 * np.matmul(rTildeFcB_M, np.transpose(rTildeFcB_M)) + + # 7. Calculate assembly mass + assemblyMass = m1 + m2 + + # 8. Calculate assembly CoM + r_FcB_M = np.add(np.add(np.matmul(np.transpose(dcm_FM), r_FcF_F), r_FM_M), r_MB_M) + r_AssemCMPntB_M = np.add(m2 * r_PcB_M, m1 * r_FcB_M) / assemblyMass + + # 9. Calculate assembly inertia IAssemPntB_M + rTildePcB_M = [[0, -r_PcB_M[2], r_PcB_M[1]], + [r_PcB_M[2], 0, -r_PcB_M[0]], + [-r_PcB_M[1], r_PcB_M[0], 0]] + IPart2PntB_M = np.matmul(np.matmul(np.transpose(dcm_PM), part2Inertia), dcm_PM) + m2 * np.matmul(rTildePcB_M, np.transpose(rTildePcB_M)) + IAssemPntB_M = np.add(IPart1PntB_M, IPart2PntB_M) + + # Testing tensor/vector math functions + # r_MB_BVecDotr_FM_MKin = myKinematicsEngine.dot(frameM.r_SP, frameF.r_SP, frameB) + # r_MB_BCrossDotr_FM_MKin = myKinematicsEngine.cross(frameM.r_SP, frameF.r_SP, frameB) + # IAssemPntB_MTensor = myKinematicsEngine.createInertiaTensor(frameB.originPoint) + # IAssemPntB_MTensor.setZerothOrder(IAssemPntB_M, frameM) + # IAssemPntB_MInvKin = myKinematicsEngine.tensorInverse(IAssemPntB_MTensor) + # IAssemPntB_MTimesr_MB_BKin = myKinematicsEngine.tensorTimesVector(IAssemPntB_MTensor, frameM.r_SP, frameB) + # IAssemPntB_MTimesIAssemPntB_MKin = myKinematicsEngine.tensorTimesTensor(IAssemPntB_MTensor, IAssemPntB_MTensor, frameB) + + # Calculating tensor/vector math truth data + # 1. Dot product + r_MB_BVecDotr_FM_M = np.dot(r_MB_B, r_FM_B) + + # 2. Cross product + r_MB_BCrossr_FM_M = np.cross(r_MB_B, r_FM_B) + + # 3. Tensor inverse + IAssemPntB_MInv = np.linalg.inv(IAssemPntB_M) + + # 4. Tensor times vector + IAssemPntB_MTimesr_MB_B = np.matmul(np.matmul(dcm_MB.transpose(), np.matmul(IAssemPntB_M, dcm_MB)), r_MB_B) + + # Tensor times tensor + IAssemPntB_MTimesIAssemPntB_M = np.matmul(np.matmul(dcm_MB.transpose(), np.matmul(IAssemPntB_M, dcm_MB)), np.matmul(dcm_MB.transpose(), np.matmul(IAssemPntB_M, dcm_MB))) + + # Calculate vector transport theorem result + dcm_NM = np.matmul(np.transpose(dcm_BN), np.transpose(dcm_MB)) + dcm_NF = np.matmul(dcm_NM, np.transpose(dcm_FM)) + omega_BM_M = [-omega_MB_M[0], -omega_MB_M[1], -omega_MB_M[2]] + omega_BF_N = np.subtract(np.matmul(dcm_NM, omega_BM_M), np.matmul(dcm_NF, omega_FM_F)) + dcm_NP = np.matmul(dcm_NF, dcm_FP) + r_PcP_N = np.matmul(dcm_NP, r_PcP_P) + crossTerm = np.cross(omega_BF_N, r_PcP_N) + derivTerm = np.matmul(dcm_NM, rBPrime_PcP_M) + rFPrime_PcP_N = np.add(derivTerm, crossTerm) + + # Calculate inertia transport theorem result + omegaTilde_BF_N = [[0, -omega_BF_N[2], omega_BF_N[1]], + [omega_BF_N[2], 0, -omega_BF_N[0]], + [-omega_BF_N[1], omega_BF_N[0], 0]] + I_PntPc_N = np.matmul(np.matmul(dcm_NP, part2Inertia), np.transpose(dcm_NP)) + derivTerm = np.matmul(np.matmul(dcm_NM, IBPrime_PntPc_M), np.transpose(dcm_NM)) + crossTerm = np.subtract(np.matmul(omegaTilde_BF_N, I_PntPc_N), np.matmul(I_PntPc_N, omegaTilde_BF_N)) + IFPrime_PntPc_N = np.add(derivTerm, crossTerm) + + print("\n** ** ** ** ** TESTING 'ZEROTH' ORDER RELATIVE KINEMATICS ** ** ** ** **") + + print("\n\n1. RELATIVE ATTITUDE:") + print("\nTRUTH: sigma_FP: ") + print(sigma_FP) + print("\nKINEMATICS ENGINE: sigma_FP: ") + print(sigma_FPKin) + + # print("\n\n2. ADD POSITION VECTORS:") + # print("\nTRUTH: r_FB_B: ") + # print(r_FB_B) + # print("\nKINEMATICS ENGINE: r_FB_B: ") + # print(r_FB_BKin) + + # print("\n\n3. ADD ANGULAR VELOCITY VECTORS:") + # print("\nTRUTH: omega_FB_M: ") + # print(omega_FB_M) + # print("\nKINEMATICS ENGINE: omega_FB_M: ") + # print(omega_FB_MKin) + + print("\n\n4. RELATIVE POSITION:") + print("\nTRUTH: r_PcF_M: ") + print(r_PcF_M) + print("\nKINEMATICS ENGINE: r_PcF_M: ") + print(r_PcF_MKin) + + print("\n\n5. RELATIVE ANGULAR VELOCITY:") + print("\nTRUTH: omega_PF_M: ") + print(omega_PF_M) + print("\nKINEMATICS ENGINE: omega_PF_M: ") + print(omega_PF_MKin) + + print("\n\n6. PARALLEL AXIS THEOREM:") + print("\nTRUTH: Inertia: ") + print(IPart1PntB_M) + print("\nKINEMATICS ENGINE: Inertia: ") + print(IPart1PntB_MKin) + + print("\n\n7. ASSEMBLY MASS:") + print("\nTRUTH: Assembly Mass: ") + print(assemblyMass) + print("\nKINEMATICS ENGINE: Assembly Mass: ") + print(assemblyMassKin) + + print("\n\n8. ASSEMBLY COM:") + print("\nTRUTH: r_AssemCMPntB_M: ") + print(r_AssemCMPntB_M) + print("\nKINEMATICS ENGINE: r_AssemCMPntB_M: ") + print(r_AssemCMPntB_MKin) + + print("\n\n9. ASSEMBLY INERTIA:") + print("\nTRUTH: IAssemPntB_M: ") + print(IAssemPntB_M) + print("\nKINEMATICS ENGINE: IAssemPntB_M: ") + print(IAssemPntB_MKin) + + # print("\n** ** ** ** ** TESTING FIRST ORDER GETTER FUNCTIONS ** ** ** ** **") + + print("\n\n1. VECTOR TRANSPORT THEOREM:") + print("\nTRUTH: rFPrime_PcP_N: ") + print(rFPrime_PcP_N) + print("\nKINEMATICS ENGINE: rFPrime_PcP_N: ") + print(rFPrime_PcP_NKin) + + # print("\n\n2. INERTIA TRANSPORT THEOREM:") + # print("\nTRUTH: IFPrime_PntPc_N: ") + # print(IFPrime_PntPc_N) + # print("\nKINEMATICS ENGINE: IFPrime_PntPc_N: ") + # print(IFPrime_PntPc_NKin) + + # print("\n** ** ** ** ** TESTING TENSOR/VECTOR MATH FUNCTIONS ** ** ** ** **") + + # print("\n\n1. VECTOR DOT PRODUCT:") + # print("\nTRUTH: r_MB_BVecDotr_FM_M: ") + # print(r_MB_BVecDotr_FM_MKin) + # print("\nKINEMATICS ENGINE: r_MB_BVecDotr_FM_M: ") + # print(r_MB_BVecDotr_FM_M) + # + # print("\n\n2. VECTOR CROSS PRODUCT:") + # print("\nTRUTH: r_MB_BVecCrossr_FM_M: ") + # print(r_MB_BCrossr_FM_M) + # print("\nKINEMATICS ENGINE: r_MB_BCrossDotr_FM_M: ") + # print(r_MB_BCrossDotr_FM_MKin.matrix) + # + # print("\n\n3. INERTIA INVERSE:") + # print("\nTRUTH: IAssemPntB_MInv: ") + # print(IAssemPntB_MInv) + # print("\nKINEMATICS ENGINE: IAssemPntB_MInv: ") + # print(IAssemPntB_MInvKin.matrix) + # + # print("\n\n4. TENSOR TIMES VECTOR:") + # print("\nTRUTH: IAssemPntB_MTimesr_MB_BKin: ") + # print(IAssemPntB_MTimesr_MB_B) + # print("\nKINEMATICS ENGINE: IAssemPntB_MTimesr_MB_BKin: ") + # print(IAssemPntB_MTimesr_MB_BKin.matrix) + # + # print("\n\n5. TENSOR TIMES TENSOR:") + # print("\nTRUTH: IAssemPntB_MTimesIAssemPntB_M: ") + # print(IAssemPntB_MTimesIAssemPntB_M) + # print("\nKINEMATICS ENGINE: IAssemPntB_MTimesIAssemPntB_M: ") + # print(IAssemPntB_MTimesIAssemPntB_MKin.matrix) + +def runJoao(): + myKinematicsEngine = KinematicsEngine.KinematicsEngine() + + myInertialFrame = myKinematicsEngine.createFrame() + myInertialFrame.tag = "inertial" + myBodyFrame = myKinematicsEngine.createFrame(myInertialFrame) + myBodyFrame.tag = "body" + + myPart1 = myKinematicsEngine.createPart(myBodyFrame) + myPart1.frame.tag = "part1" + myPart2 = myKinematicsEngine.createPart(myPart1.frame) + myPart2.frame.tag = "part2" + + myJoint = myKinematicsEngine.createRotaryOneDOFJoint() + # myJoint.hingeVector[0].spinAxis = [1, 0, 0] + myJoint.lowerFrame.tag = "lower" + myJoint.upperFrame.tag = "upper" + + myKinematicsEngine.connect(myPart1, myJoint, myPart2) + +if __name__ == "__main__": + runLeah() + #runJoao() \ No newline at end of file diff --git a/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/test_SixDOFRigidBody.py b/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/test_SixDOFRigidBody.py new file mode 100644 index 0000000000..16cb0dbe06 --- /dev/null +++ b/src/simulation/dynamics/KinematicsArchitecture/_UnitTest/test_SixDOFRigidBody.py @@ -0,0 +1,220 @@ +# ISC License +# +# Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + +# +# Unit Test Script +# Module Name: spinningBodies +# Author: João Vaz Carneiro +# Creation Date: July 14, 2023 +# + +import os +import numpy as np +import matplotlib.pyplot as plt +from Basilisk import __path__ + +bskPath = __path__[0] +fileName = os.path.basename(os.path.splitext(__file__)[0]) + +from Basilisk.simulation.KinematicsArchitecture import KinematicsEngine +from Basilisk.simulation.KinematicsArchitecture import Frame +from Basilisk.simulation.KinematicsArchitecture import Vector +from Basilisk.simulation.KinematicsArchitecture import Tensor +from Basilisk.simulation.KinematicsArchitecture import Part +from Basilisk.simulation.KinematicsArchitecture import Joint +from Basilisk.simulation.KinematicsArchitecture import Hinge + +from Basilisk.simulation import SixDOFRigidBody +from Basilisk.simulation import spacecraft +from Basilisk.utilities import SimulationBaseClass +from Basilisk.utilities import macros +from Basilisk.utilities import unitTestSupport + +def test_SixDOFRigidBody(show_plots): + r""" + **Validation Test Description** + + **Description of Variables Being Tested** + + - ``finalRotAngMom`` + - ``finalRotEnergy`` + + against their initial values. + """ + [testResults, testMessage] = dynamicsEngine_SixDOFRigidBody(show_plots) + assert testResults < 1, testMessage + + +def dynamicsEngine_SixDOFRigidBody(show_plots): + __tracebackhide__ = True + + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty list to store test log messages + + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) + scSim = SimulationBaseClass.SimBaseClass() + simulationTimeStep = macros.sec2nano(0.01) + dynProcess = scSim.CreateNewProcess(unitProcessName) + dynProcess.addTask(scSim.CreateNewTask(unitTaskName, simulationTimeStep)) + + # + # Kinematics setup + # + myKinematicsEngine = KinematicsEngine() + myInertialFrame = myKinematicsEngine.createFrame() + myInertialFrame.tag = "inertial" + + myPart = myKinematicsEngine.createPart(myInertialFrame) + myPart.frame.tag = "part" + myPart.mass = 5.0 + inertia = [[34, 1, 6], [1, 15, 3], [6, 3, 10]] + myPart.IPntSc_S.set(inertia, myPart.frame) + myPart.r_ScS.setPosition([1.0, -0.5, 1.5], myPart.frame) + myPart.r_ScS.setVelocity([0.0, 0.0, 0.0], myPart.frame, myPart.frame) + myPart.frame.r_SP.setPosition([10.0, 50.0, -30.0], myInertialFrame) + myPart.frame.r_SP.setVelocity([1.0, -3.0, 2.0], myInertialFrame, myInertialFrame) + myPart.frame.sigma_SP.setAttitude([0.1, 0.2, -0.3]) + myPart.frame.sigma_SP.setAngularVelocity([0.05, 0.03, -0.02], myPart.frame) + + # + # Dynamics setup + # + myDynamicsEngine = SixDOFRigidBody.SixDOFRigidBody(myKinematicsEngine, myInertialFrame, myPart) + myDynamicsEngine.ModelTag = "myDynamicsEngine" + scSim.AddModelToTask(unitTaskName, myDynamicsEngine) + + # + # Logging + # + samplingTime = simulationTimeStep + stateLog = myPart.bodyStateOutMsg.recorder(samplingTime) + scSim.AddModelToTask(unitTaskName, stateLog) + scSim.AddVariableForLogging(myDynamicsEngine.ModelTag + ".rotEnergy", samplingTime, 0, 0, 'double') + scSim.AddVariableForLogging(myDynamicsEngine.ModelTag + ".rotAngMomPntC_N", samplingTime, 0, 2, 'double') + + # + # Simulation + # + scSim.InitializeSimulation() + simulationTime = macros.sec2nano(10.) + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # + # Plotting + # + timeAxis = stateLog.times() * macros.NANO2SEC + dataSigmaBN = stateLog.sigma_BN + dataOmegaBN = stateLog.omega_BN_B + dataPos = stateLog.r_CN_N + dataVel = stateLog.v_CN_N + + plt.close("all") + plt.figure(1) + for idx in range(3): + plt.plot(timeAxis, dataSigmaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r'$\sigma_' + str(idx) + '$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Attitude $\sigma_{B/R}$') + + plt.figure(2) + for idx in range(3): + plt.plot(timeAxis, dataOmegaBN[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r'$\omega_' + str(idx) + '$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Angular Velocity') + + plt.figure(3) + for idx in range(3): + plt.plot(timeAxis, dataPos[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r'$r_' + str(idx) + '$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Position') + + plt.figure(4) + for idx in range(3): + plt.plot(timeAxis, dataVel[:, idx], + color=unitTestSupport.getLineColor(idx, 3), + label=r'$rDot_' + str(idx) + '$') + plt.legend(loc='lower right') + plt.xlabel('Time [sec]') + plt.ylabel(r'Velocity') + + # + # Verification + # + energy = scSim.GetLogVariableData(myDynamicsEngine.ModelTag + ".rotEnergy") + angularMomentum_N = scSim.GetLogVariableData(myDynamicsEngine.ModelTag + ".rotAngMomPntC_N") + + # Setup the conservation quantities + initialRotAngMom_N = [[angularMomentum_N[0, 1], angularMomentum_N[0, 2], angularMomentum_N[0, 3]]] + initialRotEnergy = [[energy[0, 1]]] + + plt.figure(5) + for idx in range(3): + plt.plot(timeAxis, (angularMomentum_N[:, idx+1] - angularMomentum_N[0, idx+1]) / angularMomentum_N[0, idx+1], + color=unitTestSupport.getLineColor(idx, 3), + label=r'$H_' + str(idx) + '$') + plt.legend(loc='best') + plt.xlabel('Time [sec]') + plt.ylabel(r'Angular Momentum') + + plt.figure(6) + plt.plot(timeAxis, (energy[:, 1] - energy[0, 1]) / energy[0, 1]) + plt.xlabel('Time [sec]') + plt.ylabel(r'Energy') + + if show_plots: + plt.show() + plt.close("all") + + # Testing setup + accuracy = 1e-12 + angularMomentum_N = np.delete(angularMomentum_N, 0, axis=1) # remove time column + energy = np.delete(energy, 0, axis=1) # remove time column + + for i in range(0, len(initialRotAngMom_N)): + # check a vector values + if not unitTestSupport.isArrayEqualRelative(angularMomentum_N[i], initialRotAngMom_N[i], 3, accuracy): + testFailCount += 1 + testMessages.append( + "FAILED: sixDOFRigidBody integrated test failed rotational angular momentum unit test") + + for i in range(0, len(initialRotEnergy)): + # check a vector values + if not unitTestSupport.isArrayEqualRelative(energy[i], initialRotEnergy[i], 1, accuracy): + testFailCount += 1 + testMessages.append("FAILED: sixDOFRigidBody integrated test failed rotational energy unit test") + + if testFailCount == 0: + print("PASSED: " + " sixDOFRigidBody gravity integrated test") + + assert testFailCount < 1, testMessages + # return fail count and join into a single string all messages in the list + # testMessage + return [testFailCount, ''.join(testMessages)] + + +if __name__ == "__main__": + dynamicsEngine_SixDOFRigidBody(True)