VarVector _GetVars(py::list ijs, const VarArray& vars) {
VarVector out;
int n = py::len(ijs);
for (int k=0; k < n; ++k) {
int i = py::extract<int>(ijs[k][0]);
int j = py::extract<int>(ijs[k][1]);
out.push_back(vars(i,j));
}
return out;
}
void PushObject::updateModel(const Eigen::MatrixXd& traj, GRBQuadExpr& objective) {
VectorXd& times = m_problem->m_times;
MatrixXd perts_tk = getSinBasis(times/times.maxCoeff(), fmin(6, times.size()/2));
MatrixXd pinvperts_tk = perts_tk * (perts_tk.transpose() * perts_tk).inverse();
m_exactObjective = simulateTraj2(traj, true); // current value
LOG_INFO_FMT("current val: %.3f", m_exactObjective);
MatrixXd dy_jk(traj.cols(), perts_tk.cols());
double eps = 3e-4; // scale for joint angle change
Matrix3d A;
A << sq(eps/2), eps/2, 1,
0, 0, 1,
sq(eps/2), -eps/2, 1;
Matrix3d Ainv = A.inverse();
MatrixXd grad_tj(traj.rows(), traj.cols());
m_obj = GRBQuadExpr(0);
for (int j = 0; j < traj.cols(); ++j) {
VarVector v;
VectorXd vactual(traj.rows()-1);
for (int t=1; t < traj.rows(); ++t) {
v.push_back(m_problem->m_trajVars.at(t,j));
vactual(t-1) = traj(t,j);
}
for (int k = 0; k < perts_tk.cols(); ++k) {
MatrixXd newTraj = traj;
newTraj.col(j) = traj.col(j) + (eps/2)*perts_tk.col(k);
double plusVal = simulateTraj2(newTraj, false);
newTraj.col(j) = traj.col(j) - (eps/2)*perts_tk.col(k);
double minusVal = simulateTraj2(newTraj, false);
LOG_DEBUG_FMT("joint %i, basis %i, pert vals: %.4e %.4e ", j, k, plusVal-m_exactObjective,minusVal-m_exactObjective);
dy_jk(j,k) = (plusVal - minusVal)/eps;
Vector3d y;
y << plusVal-m_exactObjective, 0, minusVal - m_exactObjective;
Vector3d abc = Ainv*y;
GRBLinExpr q;
int T = traj.rows()-1;
VectorXd pertVec=perts_tk.block(1,k,T, 1);
q.addTerms(pertVec.data(),v.data(), T);
double qactual = pertVec.dot(vactual);
m_obj += fmax(abc(0),0)*(q-qactual)*(q-qactual) + abc(1)*(q-qactual);
}
// grad_tj.col(j) = pinvperts_tk * dy_jk.row(j).transpose();
}
m_obj += m_exactObjective;
// cout << "dy_jk:" << endl;
// cout << dy_jk << endl;
// cout << "grad_tj:" << endl;
// cout << grad_tj << endl;
// m_obj.addTerms(grad_tj.data()+7, m_problem->m_trajVars.m_data.data()+7, (traj.rows()-1)*traj.cols());
// m_obj += m_exactObjective - (grad_tj.middleRows(1, traj.rows()-1).array() * traj.middleRows(1, traj.rows()-1).array()).sum();
objective += m_obj;
}
