Code: Select all
Constraint::CoordinateCouplerImpl::CoordinateCouplerImpl
(SimbodyMatterSubsystem& matter,
const Function* function,
const Array_<MobilizedBodyIndex>& coordMobod,
const Array_<MobilizerQIndex>& coordQIndex)
: Implementation(matter, 1, 0, 0), function(function),
coordBodies(coordMobod.size()), coordIndices(coordQIndex),
temp(coordBodies.size()), referenceCount(new int[1])
{
assert(coordBodies.size() == coordIndices.size());
assert(coordIndices.size() == function->getArgumentSize()); <------- Assertion error
assert(function->getMaxDerivativeOrder() >= 2);
referenceCount[0] = 1;
for (int i = 0; i < (int)coordBodies.size(); ++i) {
const MobilizedBody& mobod = matter.getMobilizedBody(coordMobod[i]);
coordBodies[i] = addConstrainedMobilizer(mobod);
}
}
For context, my code looks something like this:
Code: Select all
OpenSim::Array<std::string> independentCoordinateNames;
OpenSim::Array<double> coefficients;
for (const auto& coupling : couplings)
{
independentCoordinateNames.append(...));
coefficients.append(...);
}
coefficients.append(0.0); // constant
auto* constraint = new OpenSim::CoordinateCouplerConstraint();
constraint->setIndependentCoordinateNames(independentCoordinateNames);
constraint->setDependentCoordinateName(coordinate.getName());
constraint->setFunction(OpenSim::LinearFunction(coefficients));
mModel->addConstraint(constraint);