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