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)