// 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."); } }