ConvexObjectivePtr CostFromErrFunc::convex(const vector<double>& xin, Model* model) {
VectorXd x = getVec(xin, vars_);
MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_);
ConvexObjectivePtr out(new ConvexObjective(model));
VectorXd y = f_->call(x);
for (int i=0; i < jac.rows(); ++i) {
AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_);
if (coeffs_.size()>0) {
exprScale(aff, coeffs_[i]);
if (coeffs_[i] == 0) continue;
}
switch (pen_type_) {
case SQUARED:
out->addQuadExpr(exprSquare(aff));
break;
case ABS:
out->addAbs(aff, 1);
break;
case HINGE:
out->addHinge(aff, 1);
break;
default:
assert(0 && "unreachable");
}
}
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 ConstraintFromFunc::convex(const vector<double>& xin, Model* model) {
VectorXd x = getVec(xin, vars_);
MatrixXd jac = (dfdx_) ? dfdx_->call(x) : calcForwardNumJac(*f_, x, epsilon_);
ConvexConstraintsPtr out(new ConvexConstraints(model));
VectorXd y = f_->call(x);
for (int i=0; i < jac.rows(); ++i) {
AffExpr aff = affFromValGrad(y[i], x, jac.row(i), vars_);
if (coeffs_.size()>0) {
if (coeffs_[i] == 0) continue;
exprScale(aff, coeffs_[i]);
}
if (type() == INEQ) out->addIneqCnt(aff);
else out->addEqCnt(aff);
}
return out;
}
