/**
* Provide the probe values to be reported that correspond to the probe labels.
*/
SimTK::Vector Probe::getProbeOutputs(const State& s) const
{
if (isDisabled()) {
stringstream errorMessage;
errorMessage << getConcreteClassName()
<< ": Cannot get the output from Probe '"
<< getName() << "' because it has been disabled." << endl;
throw (Exception(errorMessage.str()));
}
// For now, this is scalarized, i.e. compile the result of the separate
// Measure for each scalar element of the probe input into a SimTK::Vector
// of outputs.
SimTK::Vector output(getNumProbeInputs());
for (int i=0; i<getNumProbeInputs(); ++i) {
if (getOperation() == "integrate")
output[i] = getGain() * (afterOperationValues[i].getValue(s) + getInitialConditions()(i));
else
output[i] = getGain() * afterOperationValues[i].getValue(s);
}
return output;
//return afterOperationValueVector.getValue(s); // save for when we can directly operate on Vector SimTK::Measures
}
Model& ModelComponent::updModel()
{
if(!_model)
throw Exception("ModelComponent::updModel(): component '" + getName() +
"' of type " + getConcreteClassName() + " does not belong to a model. "
"Have you called Model::initSystem()?");
return *_model;
}
/**
* Create the underlying system component(s).
*/
void Probe::addToSystem(MultibodySystem& system) const
{
Super::addToSystem(system);
if (isDisabled())
return;
// Make writable briefly so we can finalize the Probe to include
// references to the allocated System resources.
Probe* mutableThis = const_cast<Probe*>(this);
// ---------------------------------------------------------------------
// Create a <double> Measure of the value to be probed (operand).
// For now, this is scalarized, i.e. a separate Measure is created
// for each probe input element in the Vector.
// ---------------------------------------------------------------------
// save for when we can directly operate on Vector SimTK::Measures
//ProbeMeasure<SimTK::Vector> beforeOperationValueVector(system, *this);
int npi = getNumProbeInputs();
SimTK::Array_<ProbeMeasure<double> > beforeOperationValues;
mutableThis->afterOperationValues.resize(npi);
for (int i=0; i<npi; ++i) {
ProbeMeasure<double> tmpPM(system, *this, i);
beforeOperationValues.push_back(tmpPM);
}
// Assign the correct (operation) Measure subclass to the operand
// ==============================================================
// ---------------------------------------------------------------------
// Return the original probe value (no operation)
// ---------------------------------------------------------------------
if (getOperation() == "value") {
for (int i=0; i<npi; ++i) {
mutableThis->afterOperationValues[i] = beforeOperationValues[i];
}
}
// ---------------------------------------------------------------------
// Integrate the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "integrate") {
// check to see that size of initial condition vector
// is the same size as the data being integrated.
if (getInitialConditions().size() != getProbeOutputLabels().getSize()) {
stringstream errorMessage;
errorMessage << getConcreteClassName() << "(" + getName() <<
"): Mismatch between the size of the data labels corresponding to the "
"size of the data vector being integrated ("
<< getProbeOutputLabels().getSize()
<< ") and size of initial conditions vector ("
<< getInitialConditions().size() << ").\nAssuming an initial condition vector of "
<< getProbeOutputLabels().getSize() << " zeros." << endl;
cout << errorMessage.str() << endl;
SimTK::Vector newInitCond(getProbeOutputLabels().getSize());
newInitCond = 0;
Probe* mutableThis = const_cast<Probe*>(this);
mutableThis->setInitialConditions(newInitCond);
//throw (Exception(errorMessage.str()));
}
for (int i=0; i<getNumProbeInputs(); ++i) {
//Measure::Constant initCond(system, getInitialConditions()(i)); // init cond is handled as a special case in getProbeOutputs()
Measure::Constant initCond(system, 0.0);
mutableThis->afterOperationValues[i] = Measure::Integrate(
system, beforeOperationValues[i], initCond);
}
}
// ---------------------------------------------------------------------
// Differentiate the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "differentiate") {
for (int i=0; i<getNumProbeInputs(); ++i) {
mutableThis->afterOperationValues[i] = Measure::Differentiate(
system, beforeOperationValues[i]);
}
}
// ---------------------------------------------------------------------
// Get the minimum of the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "minimum") {
for (int i=0; i<getNumProbeInputs(); ++i) {
mutableThis->afterOperationValues[i] = Measure::Minimum(
system, beforeOperationValues[i]);
}
}
// ---------------------------------------------------------------------
// Get the absolute minimum of the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "minabs") {
for (int i=0; i<getNumProbeInputs(); ++i) {
mutableThis->afterOperationValues[i] = Measure::MinAbs(
system, beforeOperationValues[i]);
}
}
// ---------------------------------------------------------------------
// Get the maximum of the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "maximum") {
for (int i=0; i<getNumProbeInputs(); ++i) {
mutableThis->afterOperationValues[i] = Measure::Maximum(
system, beforeOperationValues[i]);
}
}
// ---------------------------------------------------------------------
// Get the absolute maximum of the probe value
// ---------------------------------------------------------------------
else if (getOperation() == "maxabs") {
for (int i=0; i<getNumProbeInputs(); ++i) {
mutableThis->afterOperationValues[i] = Measure::MaxAbs(
system, beforeOperationValues[i]);
}
}
// ---------------------------------------------------------------------
// Throw exception (invalid operation)
// ---------------------------------------------------------------------
else {
stringstream errorMessage;
errorMessage << getConcreteClassName() << ": Invalid probe operation: "
<< getOperation()
<< ". Currently supports 'value', 'integrate', 'differentiate', "
"'minimum', 'minabs', 'maximum', 'maxabs'." << endl;
throw (Exception(errorMessage.str()));
}
}
