ConvexObjectivePtr CostFromFunc::convex(const vector<double>& xin) {
VectorXd x = getVec(xin, vars_);
ConvexObjectivePtr out(new ConvexObjective());
QuadExpr quad;
if (!full_hessian_) {
double val;
VectorXd grad,hess;
calcGradAndDiagHess(*f_, x, epsilon_, val, grad, hess);
hess = hess.cwiseMax(VectorXd::Zero(hess.size()));
//QuadExpr& quad = out->quad_;
quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(hess.cwiseProduct(x));
quad.affexpr.vars = vars_;
quad.affexpr.coeffs = toDblVec(grad - hess.cwiseProduct(x));
quad.vars1 = vars_;
quad.vars2 = vars_;
quad.coeffs = toDblVec(hess*.5);
}
else {
double val;
VectorXd grad;
MatrixXd hess;
calcGradHess(f_, x, epsilon_, val, grad, hess);
MatrixXd pos_hess = MatrixXd::Zero(x.size(), x.size());
Eigen::SelfAdjointEigenSolver<MatrixXd> es(hess);
VectorXd eigvals = es.eigenvalues();
MatrixXd eigvecs = es.eigenvectors();
for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
if (eigvals(i) > 0) pos_hess += eigvals(i) * eigvecs.col(i) * eigvecs.col(i).transpose();
}
//QuadExpr& quad = out->quad_;
quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(pos_hess * x);
quad.affexpr.vars = vars_;
quad.affexpr.coeffs = toDblVec(grad - pos_hess * x);
int nquadterms = (x.size() * (x.size()-1))/2;
quad.coeffs.reserve(nquadterms);
quad.vars1.reserve(nquadterms);
quad.vars2.reserve(nquadterms);
for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
quad.vars1.push_back(vars_[i]);
quad.vars2.push_back(vars_[i]);
quad.coeffs.push_back(pos_hess(i,i)/2);
for (size_t j=i+1; j != end; ++j) { //tricky --- eigen size() is signed
quad.vars1.push_back(vars_[i]);
quad.vars2.push_back(vars_[j]);
quad.coeffs.push_back(pos_hess(i,j));
}
}
}
out->addQuadExpr(quad);
return out;
}
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;
}
// CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
VectorXd operator()(const VectorXd& dof_vals) const {
manip_->SetDOFValues(toDblVec(dof_vals));
OR::Transform newpose = link_->GetTransform();
OR::Transform pose_err = pose_inv_ * newpose;
VectorXd err = concat(rotVec(pose_err.rot), toVector3d(pose_err.trans));
return err;
}
size_t NeedleCollisionHash::hash(const DblVec& x) {
DblVec extended_x = x;
for (int i = 0; i < helper->pis.size(); ++i) {
for (int j = 0; j < helper->pis[i]->local_configs.size(); ++j) {
DblVec state = toDblVec(logDown(helper->pis[i]->local_configs[j]->pose));
extended_x.insert(extended_x.end(), state.begin(), state.end());
}
}
return boost::hash_range(extended_x.begin(), extended_x.end());
}
virtual void sigma_points(const BeliefT& belief, vector<DblVec>* output_sigma_points) const {
assert (output_sigma_points != nullptr);
StateT state;
VarianceT sqrt_sigma;
extract_state_and_sqrt_sigma(belief, &state, &sqrt_sigma);
output_sigma_points->clear();
//output_sigma_points->push_back( toDblVec(sigma_point(state, sqrt_sigma, 0)) );
for (int i = 0; i <= 2*state_dim; ++i) {
output_sigma_points->push_back( toDblVec(sigma_point(state, sqrt_sigma, i)) );
}
}
VectorXd RotationError::operator()(const VectorXd& a) const {
DblVec phis;
DblVec Deltas;
phis = toDblVec(a);
VectorXd ret(1);
ret(0) = -this->total_rotation_limit;
for (int i = 0; i < phis.size(); ++i) {
ret(0) += fabs(phis[i]);
}
return ret;
}
VectorXd CurvatureError::operator()(const VectorXd& a) const {
DblVec curvatures;
DblVec Deltas;
curvatures = toDblVec(a.head(pi->curvature_vars.size()));
Deltas = DblVec(pi->curvature_vars.size(), a(a.size() - 1));
VectorXd ret(1);
ret(0) = -this->total_curvature_limit;
for (int i = 0; i < curvatures.size(); ++i) {
ret(0) += fabs(curvatures[i]) * Deltas[i];
}
return ret;
}
double CurvatureCost::operator()(const VectorXd& a) const {
DblVec curvatures;
DblVec Deltas;
curvatures = toDblVec(a.head(pi->curvature_vars.size()));
Deltas = DblVec(pi->curvature_vars.size(), a(a.size() - 1));
double ret = 0;
for (int i = 0; i < curvatures.size(); ++i)
{
double tmp = curvatures[i] * Deltas[i];
ret += tmp * tmp;
}
return ret * coeff;
}
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;
}
vector<double> ConstraintFromFunc::value(const vector<double>& xin) {
VectorXd x = getVec(xin, vars_);
VectorXd err = f_->call(x);
if (coeffs_.size()>0) err.array() *= coeffs_.array();
return toDblVec(err);
}
VectorXd operator()(const VectorXd& dof_vals) {
manip_->SetDOFValues(toDblVec(dof_vals));
OR::Transform newpose = link_->GetTransform();
return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_);
}
// CartPoseCostCalculator(const CartPoseCostCalculator& other) : pose_(other.pose_), manip_(other.manip_), rs_(other.rs_) {}
VectorXd operator()(const VectorXd& dof_vals) {
manip_->SetDOFValues(toDblVec(dof_vals));
OR::Transform newpose = link_->GetTransform();
return pt_world_ - toVector3d(newpose.trans);
}
void initialize_robot(RobotBasePtr robot, const Vector2d& start) {
robot->SetDOFValues(toDblVec(start));
}
vector<double> ConstraintFromNumDiff::value(const vector<double>& xin) {
VectorXd x = getVec(xin, vars_);
return toDblVec(f_->call(x));
}
