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; }