// for any post XML deseraialization intialization
void Controller::connectToModel(Model& model)
{
Super::connectToModel(model);
if (getProperty_actuator_list().size() > 0){
if (IO::Uppercase(get_actuator_list(0)) == "ALL"){
setActuators(model.getActuators());
// setup actuators to ensure actuators added by controllers are also setup properly
// TODO: Adopt the controls (discrete state variables) of the Actuator
return;
}
else{
Set<Actuator> actuatorsByName;
for (int i = 0; i < getProperty_actuator_list().size(); i++){
if (model.updActuators().contains(get_actuator_list(i)))
actuatorsByName.adoptAndAppend(&model.updActuators().get(get_actuator_list(i)));
else
cerr << "WARN: Controller::connectToModel : Actuator " << get_actuator_list(i) << " was not found and will be ignored." << endl;
}
actuatorsByName.setMemoryOwner(false);
setActuators(actuatorsByName);
}
}
}
void testOrientationsReference()
{
// column labels for orientation sensor data
vector<std::string> labels{ "A", "B", "C", "D", "E", "F" };
// for testing construct a set of marker weights in a different order
vector<int> order = { 3, 5, 1, 4, 0, 2 };
size_t nc = labels.size(); // number of columns of orientation data
size_t nr = 5; // number of rows of orientation data
TimeSeriesTable_<SimTK::Rotation> orientationData;
orientationData.setColumnLabels(labels);
for (size_t r{ 0 }; r < nr; ++r) {
SimTK::RowVector_<SimTK::Rotation> row{ int(nc), SimTK::Rotation() };
orientationData.appendRow(0.1*r, row);
}
Set<OrientationWeight> orientationWeights;
for (size_t m{ 0 }; m < nc; ++m)
orientationWeights.adoptAndAppend(
new OrientationWeight(labels[order[m]], double(order[m])));
std::cout << orientationWeights.dump() << std::endl;
OrientationsReference orientationsRef(orientationData, &orientationWeights);
Model model;
SimTK::State& s = model.initSystem();
s.updTime() = 0.0;
SimTK::Array_<string> names = orientationsRef.getNames();
SimTK::Array_<double> weights;
orientationsRef.getWeights(s, weights);
SimTK_ASSERT_ALWAYS(names.size() == weights.size(),
"Number of markers does not match number of weights.");
for (unsigned int i{ 0 }; i < names.size(); ++i) {
std::cout << names[i] << ": " << weights[i] << std::endl;
SimTK_ASSERT_ALWAYS(weights[i] == double(i),
"Mismatched weight to marker.");
}
// Add marker weights for markers not present in the data
orientationWeights.adoptAndAppend(new OrientationWeight("X", 0.1));
orientationWeights.insert(0, new OrientationWeight("Y", 0.01));
OrientationsReference orientationsRef2(orientationData, &orientationWeights);
auto& oWeightSet = orientationsRef2.get_orientation_weights();
// verify that internal weight set was updated
std::cout << oWeightSet.dump() << std::endl;
names = orientationsRef2.getNames();
orientationsRef2.getWeights(s, weights);
SimTK_ASSERT_ALWAYS(names.size() == weights.size(),
"Number of orientation sensors does not match number of weights.");
for (unsigned int i = 0; i < names.size(); ++i) {
std::cout << names[i] << ": " << weights[i] << std::endl;
SimTK_ASSERT_ALWAYS(weights[i] == double(i),
"Mismatched weight to orientation sensor.");
}
}
