bool LeoModel::loadConstraintSetsFromFile (const char* filename, bool verbose) {
// initialize Constraint Sets
LuaTable lua_table = LuaTable::fromFile (filename);
vector<LuaKey> constraint_set_keys = lua_table["constraint_sets"].keys();
vector<string> constraint_set_names;
for (size_t i = 0; i < constraint_set_keys.size(); i++) {
if (constraint_set_keys[i].type == LuaKey::String) {
constraint_set_names.push_back (constraint_set_keys[i].string_value);
}
else {
cerr << "Found invalid constraint set name, string expected!" << endl;
abort();
}
}
for (size_t si = 0; si < constraint_set_names.size(); si++) {
string set_name_str = constraint_set_names[si];
if (verbose) {
cout << "ConstraintSet '" << set_name_str << endl;
}
unsigned int constraint_count =
lua_table["constraint_sets"][constraint_set_names[si].c_str()].length();
ConstraintSet cs;
ConstraintSetInfo csi;
csi.name = set_name_str;
csi.constraints.resize (constraint_count);
for (int ci = 0; ci < constraint_count; ci++) {
ConstraintInfo constraint_info =
lua_table["constraint_sets"][set_name_str.c_str()][ci + 1];
string point_name = constraint_info.point_name.c_str();
constraint_info.point_id = ci;
if (verbose) {
cout << " Adding Constraint point: " << points[point_name].name << endl;
cout << " body id = " << points[point_name].body_id << endl;
cout << " normal = [" << constraint_info.normal.transpose() << "]" << endl;
}
cs.AddConstraint (
points[point_name].body_id,
points[point_name].point_local,
constraint_info.normal
);
csi.constraints[ci] = constraint_info;
}
// save constraint set infos
constraintSetInfos[set_name_str] = csi;
// assign constraint set
constraints[set_name_str] = cs;
// TODO check which solver works better
constraints[set_name_str].linear_solver = LinearSolverHouseholderQR;
// constraintSets[set_name].linear_solver = LinearSolverPartialPivLU;
constraints[set_name_str].Bind (model);
}
// check whether we missed some sets
if (constraints.size() != constraint_set_names.size()) {
cerr << "Error: not all constraint sets have been loaded!" << endl;
abort();
}
return true;
}
