def __init__(self, rigidBodyState, forceParam, inertiaParam, integrationMethod="Euler", discardedTimeStepCallback=None, simDefinition=None): super().__init__(rigidBodyState, forceParam, inertiaParam, integrationMethod, discardedTimeStepCallback, simDefinition) self.state = StateList([rigidBodyState]) self.derivativeFuncs = [self.getRigidBodyStateDerivative]
class StatefulRigidBody(RigidBody): ''' Intended to represent rigid bodies that have additional stateful properties outside of their rigd body state (ex. tank levels, actuator positions). State is defined a by a `StateList` instead of by a `MAPLEAF.Motion.RigidBodyState`. ''' def __init__(self, rigidBodyState, forceParam, inertiaParam, integrationMethod="Euler", discardedTimeStepCallback=None, simDefinition=None): super().__init__(rigidBodyState, forceParam, inertiaParam, integrationMethod, discardedTimeStepCallback, simDefinition) self.state = StateList([rigidBodyState]) self.derivativeFuncs = [self.getRigidBodyStateDerivative] def addStateVariable(self, name, currentValue, derivativeFunction): ''' Pass in the current value of a parameter which needs to be integrated, and a derivative function Derivative function should accept the current time and StateList as inputs and return the derivative of the new parameter ''' self.state.addStateVariable(name, currentValue) self.derivativeFuncs.append(derivativeFunction) def getStateDerivative(self, time, state): return StateList([ derivativeFunc(time, state) for derivativeFunc in self.derivativeFuncs ], _nameToIndexMap=state.nameToIndexMap) def timeStep(self, deltaT): # TODO: Current derivative estimates are first-order, replace with a more accurate iterative method self.state.estimatedDerivative = self.lastStateDerivative integrationResult = self.integrate(self.state, self.time, self.getStateDerivative, deltaT) # This is where the simulation time and state are kept track of self.time += integrationResult.dt self.state = integrationResult.newValue # Save for next time step self.lastStateDerivative = integrationResult.derivativeEstimate return integrationResult
def test_getLogHeader(self): state1 = StateList([1, 2], ["var1", "var2"]) header = state1.getLogHeader() self.assertEqual(header, " var1 var2") state2 = StateList([1, 2]) header = state2.getLogHeader() self.assertEqual(header, " StateVariable0 StateVariable1")
def test_variableNames(self): state1 = StateList([1, 2], ["var1", "var2"]) self.assertEqual(state1.nameToIndexMap, {"var1": 0, "var2": 1}) self.assertEqual(state1.var1, 1) self.assertEqual(state1.var2, 2) with self.assertRaises(ValueError): forbiddenNameState = StateList([1, 2], ["position", "var2"]) with self.assertRaises(ValueError): duplicateNameState = StateList([1, 2], ["var", "var"]) with self.assertRaises(ValueError): tooManyVarNamesState = StateList([1, 2], ["var1", "var2", "var3"]) with self.assertRaises(AttributeError): invalidAttribute = state1.var3 # Test assigning to a variable state1.var1 = 3 self.assertEqual(state1.var1, 3) # Test assigning to non-existent attribute with self.assertRaises(AttributeError): state1.var3 = "invalidValue" # Shouldn't work because item 0 isn't a rigidbodystate with self.assertRaises(AttributeError): state1.position = Vector(0, 0, 1) # Make item 0 one and try again state1[0] = RigidBodyState() state1.position = Vector(0, 0, 1) self.assertEqual(state1.position, Vector(0, 0, 1))
def test_arithmeticOperators(self): state1 = StateList([1, 2]) state2 = StateList([2, 4]) additionResult = state1 + state2 self.assertEqual(additionResult, StateList([3, 6])) subtractionResult = state2 - state1 self.assertEqual(subtractionResult, StateList([1, 2])) multiplicationResult = state1 * 3 self.assertEqual(multiplicationResult, StateList([3, 6])) divisionResult = state1 / 2 self.assertEqual(divisionResult, StateList([0.5, 1])) negateResult = -state1 self.assertEqual(negateResult, StateList([-1, -2])) absVal = abs(state2) absVal2 = abs(negateResult) self.assertEqual(absVal, 6) self.assertEqual(absVal2, 3)
def getStateDerivative(self, time, state): return StateList([ derivativeFunc(time, state) for derivativeFunc in self.derivativeFuncs ], _nameToIndexMap=state.nameToIndexMap)
def test_str(self): state1 = StateList([1, 2], ["var1", "var2"]) string = str(state1) self.assertEqual(string, "1 2")
def test_addStateVariables(self): state1 = StateList([1, 2], ["var1", "var2"]) state1.addStateVariable("var3", 3) self.assertEqual(state1.var3, 3)