int gradientFunc(const Vector& parameters,
bool new_parameters,
Vector& gradient) const override
{ SimTK_ASSERT2_ALWAYS(gradient.size() == getNumFreeQs(),
"AssemblySystem::gradientFunc(): expected gradient vector of"
" size %d but got %d; this is likely a problem with the optimizer"
" which is required to allocate the right amount of space.",
getNumFreeQs(), gradient.size());
++nEvalGradient;
if (new_parameters)
setInternalStateFromFreeQs(parameters);
for (unsigned i=0; i < assembler.reporters.size(); ++i)
assembler.reporters[i]->handleEvent(getInternalState());
// This will record the indices of any goals we encounter that can't
// provide their own gradients; we'll handle them all together at
// the end.
Array_<AssemblyConditionIndex> needNumericalGradient;
gradient = 0;
Vector tmpGradient(gradient.size());
for (unsigned i=0; i < assembler.goals.size(); ++i) {
AssemblyConditionIndex goalIx = assembler.goals[i];
const AssemblyCondition& cond = *assembler.conditions[goalIx];
const int stat = (assembler.forceNumericalGradient
? -1
: cond.calcGoalGradient(getInternalState(),
tmpGradient));
if (stat == -1) {
needNumericalGradient.push_back(goalIx);
continue;
}
if (stat != 0)
return stat;
gradient += assembler.weights[goalIx] * tmpGradient;
}
if (!needNumericalGradient.empty()) {
//cout << "Need numerical gradient for "
// << needNumericalGradient.size() << " goals." << endl;
NumGradientFunc numGoals(assembler, needNumericalGradient);
// Essential to use central difference here so that the
// approximate gradient is actually zero at the optimum
// solution, otherwise IpOpt won't converge.
Differentiator gradNumGoals
(numGoals,Differentiator::CentralDifference);
// weights are already included here
gradient += gradNumGoals.calcGradient(getFreeQsFromInternalState());
nEvalObjective += gradNumGoals.getNumCallsToUserFunction();
}
//cout << "Grad=" << gradient << endl;
return 0;
}
void testInsert() {
const int data1[3] = {7, -2, 3};
const int data2[4] = {101, 121, -111, 321};
int wdata[3] = {99, 9999, 999}; // writable
Array_<int> a(data2,data2+4); // copy the data
SimTK_TEST(&a[0] != data2);
ArrayViewConst_<int> avc(data2, data2+4); // share the data
SimTK_TEST(&avc[1] == data2+1);
Array_<int> aw(wdata, wdata+3, DontCopy()); // shared
SimTK_TEST(&aw[0] == wdata);
// Can't insert into non-owner.
SimTK_TEST_MUST_THROW(aw.insert(aw.begin(), avc.begin(), avc.end()));
// Unless we're inserting zero elements; that's allowed.
aw.insert(&aw[1], avc.begin(), avc.begin());
Array_<int> ac(data1, data1+3);
std::vector<int> vc(data1, data1+3);
ac.insert(&ac[1], &a[1], &a[1]+2);
vc.insert(vc.begin()+1, &a[1], &a[1]+2);
SimTK_TEST(ac.size()==5);
SimTK_TEST(ac == vc); // 7, 121, -111, -2, 3
// insert vc onto beginning and end of ac
ac.insert(ac.begin(), vc.begin(), vc.end());
ac.insert(ac.end(), vc.begin(), vc.end());
SimTK_TEST(ac.size()==15);
SimTK_TEST(ac(0,5)==vc && ac(5,5)==vc && ac(10,5)==vc);
// Shrink ac back down to 5 again.
ac.erase(ac.begin()+2, ac.begin()+12);
SimTK_TEST(ac == vc);
SimTK_TEST(ac.allocated() >= 15);
ac.shrink_to_fit();
SimTK_TEST(ac.allocated() < 15);
SimTK_TEST(ac == vc); // make sure we didn't lose the data
// Now try some null insertions
Array_<int> null;
ac.insert(ac.begin(), null.begin(), null.end());
ac.insert(ac.begin()+2, null.begin(), null.end());
ac.insert(ac.end(), null.begin(), null.end());
ac.insert(ac.begin(), 0, 929);
ac.insert(ac.begin()+2, 0, 929);
ac.insert(ac.end(), 0, 929);
SimTK_TEST(ac == vc);
ArrayView_<int> null2;
null.insert(null.begin(), null2.begin(), null2.end());
SimTK_TEST(null.empty() && null2.empty());
// How about inserting into a null array?
null.insert(null.begin(), 3, 987);
SimTK_TEST(null == std::vector<int>(3,987));
null.deallocate(); // back to null
SimTK_TEST(null.data()==0 && null.size()==0 && null.allocated()==0);
null.insert(null.begin(), ac.begin(), ac.end());
SimTK_TEST(null == vc);
null.deallocate();
// Fill in a bunch of 1000's in the middle, erase the beginning and
// end, and make sure we see just the 1000's.
ac.insert(ac.begin()+2, 99, 1000);
ac.erase(ac.begin(), ac.begin()+2);
ac.erase(ac.end()-3, ac.end());
SimTK_TEST(ac == Array_<int>(99,1000));
}
int constraintJacobian(const Vector& parameters,
bool new_parameters,
Matrix& J) const override
{ ++nEvalJacobian;
if (new_parameters)
setInternalStateFromFreeQs(parameters);
for (unsigned i=0; i < assembler.reporters.size(); ++i)
assembler.reporters[i]->handleEvent(getInternalState());
assert(J.nrow() == getNumEqualityConstraints());
assert(J.ncol() == getNumFreeQs());
const int n = getNumFreeQs();
// This will record the indices of any constraints we encounter that
// can't provide their own gradients; we'll handle them all together
// at the end.
Array_<AssemblyConditionIndex> needNumericalJacobian;
Array_<int> firstEqn;
Array_<int> nEqns;
int needy = 0;
int nxtEqn = 0;
for (unsigned i=0; i < assembler.errors.size(); ++i) {
AssemblyConditionIndex consIx = assembler.errors[i];
const AssemblyCondition& cond = *assembler.conditions[consIx];
const int m = cond.getNumErrors(getInternalState());
const int stat = (assembler.forceNumericalJacobian
? -1
: cond.calcErrorJacobian(getInternalState(),
J(nxtEqn,0,m,n)));
if (stat == -1) {
needNumericalJacobian.push_back(consIx);
firstEqn.push_back(nxtEqn);
nEqns.push_back(m);
needy += m;
} else if (stat != 0)
return stat;
nxtEqn += m;
}
if (!needNumericalJacobian.empty()) {
//cout << "Need numerical Jacobian for "
// << needNumericalJacobian.size() << " constraints." << endl;
NumJacobianFunc numCons(assembler, needNumericalJacobian,
nEqns, needy);
// Forward difference should be fine here, unlike for the
// gradient because we converge on the solution value
// rather than the derivative norm.
Differentiator jacNumCons(numCons);
Matrix numJ = jacNumCons.calcJacobian(getFreeQsFromInternalState());
nEvalConstraints += jacNumCons.getNumCallsToUserFunction();
// Fill in the missing rows.
int nxtInNumJ = 0;
for (unsigned i=0; i < needNumericalJacobian.size(); ++i) {
J(firstEqn[i],0,nEqns[i],n) = numJ(nxtInNumJ,0,nEqns[i],n);
nxtInNumJ += nEqns[i];
}
}
//cout << "J=" << J;
return 0;
}
