JointVelCost::JointVelCost(const VarArray& vars, const VectorXd& coeffs) :
Cost("JointVel"), vars_(vars), coeffs_(coeffs) {
for (int i=0; i < vars.rows()-1; ++i) {
for (int j=0; j < vars.cols(); ++j) {
AffExpr vel;
exprInc(vel, exprMult(vars(i,j), -1));
exprInc(vel, exprMult(vars(i+1,j), 1));
exprInc(expr_, exprMult(exprSquare(vel),coeffs_[j]));
}
}
}
JointAccCost::JointAccCost(const VarArray& vars, const VectorXd& coeffs) :
Cost("JointAcc"), vars_(vars), coeffs_(coeffs) {
for (int i=0; i < vars.rows()-2; ++i) {
for (int j=0; j < vars.cols(); ++j) {
AffExpr acc;
exprInc(acc, exprMult(vars(i,j), -1));
exprInc(acc, exprMult(vars(i+1,j), 2));
exprInc(acc, exprMult(vars(i+2,j), -1));
exprInc(expr_, exprMult(exprSquare(acc), coeffs_[j]));
}
}
}
RotationContinuityQuadraticCost::RotationContinuityQuadraticCost(const VarVector& vars, double coeff, NeedleProblemHelperPtr helper) : Cost("RotationContinuity"), vars(vars), coeff(coeff), helper(helper)
{
for (int i = 1; i < vars.size(); ++i)
{
exprInc(expr, exprMult(exprSquare(exprSub(AffExpr(vars[i]), AffExpr(vars[i-1]))), coeff));
}
}
ConvexObjectivePtr NeedleCollisionClearanceCost::convex(const vector<double>& x) {
ConvexObjectivePtr out(new ConvexObjective());
vector<AffExpr> exprs;
EnvironmentBasePtr env = helper->pis[0]->local_configs[0]->GetEnv();
BOOST_FOREACH(const CollisionEvaluatorPtr& cc, this->ccs) {
vector<AffExpr> tmp_exprs;
cc->CalcDistExpressions(x, tmp_exprs);
for (int i = 0; i < tmp_exprs.size(); ++i) {
exprs.push_back(exprMult(tmp_exprs[i], -1));
}
}
if (exprs.size() > 0) {
out->addMax(exprs, this->coeff);
} else {
out->addAffExpr(AffExpr(-helper->collision_clearance_threshold * this->coeff));
}
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;
}
RotationQuadraticCost::RotationQuadraticCost(const VarVector& vars, double coeff, NeedleProblemHelperPtr helper) : Cost("Rotation"), vars(vars), coeff(coeff), helper(helper) {
for (int i = 0; i < vars.size(); ++i) {
exprInc(expr, exprMult(exprSquare(vars[i]), coeff));
}
}
