AffExpr affFromValGrad(double y, const VectorXd& x, const VectorXd& dydx, const VarVector& vars) {
AffExpr aff;
aff.constant = y - dydx.dot(x);
aff.coeffs = toDblVec(dydx);
aff.vars = vars;
aff = cleanupAff(aff);
return aff;
}
QuadExpr cleanupQuad(const QuadExpr& q) {
QuadExpr out;
out.affexpr = cleanupAff(q.affexpr);
for (size_t i=0; i < q.size(); ++i) {
if (fabs(q.coeffs[i]) > 1e-8) {
out.coeffs.push_back(q.coeffs[i]);
out.vars1.push_back(q.vars1[i]);
out.vars2.push_back(q.vars2[i]);
}
}
return out;
}
virtual void CollisionsToDistanceExpressions(const vector<BeliefCollision>& collisions, Configuration& rad,
const Link2Int& link2ind, const VarVector& theta_vars0, const VarVector& theta_vars1, const DblVec& theta_vals0, const DblVec& theta_vals1, vector<AffExpr>& exprs, bool isTimestep1, NamePairs& bodyNames) {
vector<AffExpr> exprs0, exprs1;
BeliefDiscreteCollisionEvaluator<BeliefFuncT>::CollisionsToDistanceExpressions(collisions, rad, link2ind, theta_vars0, theta_vals0, exprs0, false, bodyNames);
BeliefDiscreteCollisionEvaluator<BeliefFuncT>::CollisionsToDistanceExpressions(collisions, rad, link2ind, theta_vars1, theta_vals1, exprs1, true, bodyNames);
exprs.resize(exprs0.size());
for (int i=0; i < exprs0.size(); ++i) {
exprScale(exprs0[i], (1-collisions[i].time));
exprScale(exprs1[i], collisions[i].time);
exprs[i] = AffExpr(0);
exprInc(exprs[i], exprs0[i]);
exprInc(exprs[i], exprs1[i]);
cleanupAff(exprs[i]);
}
}
ConvexConstraintsPtr ConstraintFromNumDiff::convex(const vector<double>& xin, Model* model) {
VectorXd x = getVec(xin, vars_);
MatrixXd jac = calcForwardNumJac(*f_, x, epsilon_);
ConvexConstraintsPtr out(new ConvexConstraints(model));
VectorXd y = f_->call(x);
for (int i=0; i < jac.rows(); ++i) {
if (enabled_.empty() || enabled_[i]) {
AffExpr aff;
aff.constant = y[i] - jac.row(i).dot(x);
aff.coeffs = toDblVec(jac.row(i));
aff.vars = vars_;
aff = cleanupAff(aff);
if (type() == INEQ) out->addIneqCnt(aff);
else out->addEqCnt(aff);
}
}
return out;
}
ConvexObjectivePtr CostFromNumDiffErr::convex(const vector<double>& xin, Model* model) {
VectorXd x = getVec(xin, vars_);
MatrixXd jac = calcForwardNumJac(*f_, x, epsilon_);
ConvexObjectivePtr out(new ConvexObjective(model));
VectorXd y = f_->call(x);
for (int i=0; i < jac.rows(); ++i) {
if (coeffs_[i] > 0) {
AffExpr aff;
aff.constant = y[i] - jac.row(i).dot(x);
aff.coeffs = toDblVec(jac.row(i));
aff.vars = vars_;
aff = cleanupAff(aff);
if (pen_type_ == SQUARED) {
out->addQuadExpr(exprMult(exprSquare(aff), coeffs_[i]));
}
else {
out->addAbs(aff, coeffs_[i]);
}
}
}
return out;
}
