void SingleTimestepCollisionEvaluator::CalcDists(const DblVec& x, DblVec& dists, DblVec& weights) {
DblVec dofvals = getDblVec(x, m_vars);
m_rad->SetDOFValues(dofvals);
vector<Collision> collisions;
GetCollisionsCached(dofvals, collisions);
dists.reserve(dists.size() + collisions.size());
weights.reserve(weights.size() + collisions.size());
BOOST_FOREACH(const Collision& col, collisions) {
Link2Int::iterator itA = m_link2ind.find(col.linkA),
itB = m_link2ind.find(col.linkB);
if (itA != m_link2ind.end() || itB != m_link2ind.end()) {
dists.push_back(col.distance);
weights.push_back(col.weight);
}
}
void displayGradient(DblVec& gradientP){
cout << "DEBUG DISPLAY GRADIENT\n";
for(size_t i = 0; i < gradientP.size(); i++){
if(gradientP[i] != 0) cout << i << " : " << gradientP[i] << endl;
}
cout << "DEBUG DISPLAY GRADIENT OVER\n";
}
double cal_score(vector <size_t> instance){
//f(x)=(UW)(TV)' + Px
double score = 0.0;
DblVec UW;
DblVec TV;
int dimLatent = W.size() / getUserFeaCount();
for (size_t j = 0 ; j < dimLatent; j++){
UW.push_back(0);
TV.push_back(0);
}
for (size_t j = 0; j < instance.size(); j++){
score += P[instance[j]] * 1.0;
if(instance[j] < getAdFeaCount()){
for(size_t k = 0; k < dimLatent; k++){
int v_index = instance[j] * dimLatent + k;
TV[k]+=V[v_index];
}
}
else if(instance[j] < getAdFeaCount() + getUserFeaCount()){
for(size_t k = 0; k < dimLatent; k++){
int w_index = (instance[j] - getAdFeaCount())*dimLatent + k;
UW[k] += W[w_index];
}
}
}
for (size_t j = 0; j < dimLatent; j++){
score += UW[j]*TV[j];
}
return score;
}
ConvexConstraintsPtr ZMPConstraint::convex(const DblVec& x, Model* model) {
DblVec curvals = getDblVec(x, m_vars);
m_rad->SetDOFValues(curvals);
DblMatrix jacmoment = DblMatrix::Zero(3, curvals.size());
OR::Vector moment(0,0,0);
float totalmass = 0;
BOOST_FOREACH(const KinBody::LinkPtr& link, m_rad->GetRobot()->GetLinks()) {
if (!link->GetGeometries().empty()) {
OR::Vector cm = link->GetGlobalCOM();
moment += cm * link->GetMass();
jacmoment += m_rad->PositionJacobian(link->GetIndex(), cm) * link->GetMass();
totalmass += link->GetMass();
}
}
moment /= totalmass;
jacmoment /= totalmass;
AffExpr x_expr = AffExpr(moment.x) + varDot(jacmoment.row(0), m_vars) - jacmoment.row(0).dot(toVectorXd(curvals));
AffExpr y_expr = AffExpr(moment.y) + varDot(jacmoment.row(1), m_vars) - jacmoment.row(1).dot(toVectorXd(curvals));
ConvexConstraintsPtr out(new ConvexConstraints(model));
for (int i=0; i < m_ab.rows(); ++i) {
out->addIneqCnt(m_ab(i,0) * x_expr + m_ab(i,1) * y_expr + m_c(i));
}
return out;
}
double OptimizerState::dotProduct(const DblVec& a, const DblVec& b){
double result = 0;
for(size_t i=0; i < a.size(); i++){
result += a[i] * b[i];
}
return result;
}
double LogisticRegressionObjective::EvalLocal(const DblVec& input, DblVec& gradient){
double loss = 0.0;
for (size_t i = 0; i < input.size(); i++){
loss += 0.5 * input[i] * input[i] * l2weight;
gradient[i] = l2weight * input[i];
}
for (size_t i = 0; i < problem.NumInstances(); i++){
double score = problem.ScoreOf(i, input);
double insLoss, insProb;
if (score < -30){
insLoss = -score;
insProb = 0;
}else if (score > 30){
insLoss = 0;
insProb = 1;
}else {
double temp = 1.0 + exp(-score);
insLoss = log(temp);
insProb = 1.0/ temp;
}
loss += insLoss;
problem.AddMultTo(i, 1.0 - insProb, gradient);
}
return loss ;
}
double OptimizerState::dotProduct(const DblVec& a, const DblVec& b){
double result = 0;
// cout << a[0] << " " << b[0] << endl;
for(size_t i=0; i < a.size(); i++){
result += a[i] * b[i];
}
return result;
}
void testProblem(ScalarOfVectorPtr f, VectorOfVectorPtr g, ConstraintType cnt_type,
const DblVec& init, const DblVec& sol) {
OptProbPtr prob;
size_t n = init.size();
assert (sol.size() == n);
setupProblem(prob, n);
prob->addCost(CostPtr(new CostFromFunc(f, prob->getVars(), "f", true)));
prob->addConstraint(ConstraintPtr(new ConstraintFromFunc(g, prob->getVars(), VectorXd(), cnt_type,"g")));
BasicTrustRegionSQP solver(prob);
solver.max_iter_ = 1000;
solver.min_trust_box_size_ = 1e-5;
solver.min_approx_improve_ = 1e-10;
solver.merit_error_coeff_ = 1;
solver.initialize(init);
OptStatus status = solver.optimize();
EXPECT_EQ(status, OPT_CONVERGED);
expectAllNear(solver.x(), sol, .01);
}
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;
}
double calSparsity(const DblVec& vec, size_t dimLatent){
int num = vec.size() / dimLatent;
int sparseNum = 0;
double sum = 0;
for(int i = 0; i < num; i++){
sum = 0;
for(int j = 0; j < dimLatent; j++)
sum += vec[i*dimLatent+j] * vec[i*dimLatent+j];
if(sum < 1e-10) sparseNum++;
}
return (double)sparseNum / num;
}
void printVector(const DblVec &vec, const char* filename) {
ofstream outfile(filename);
if (!outfile.good()) {
cerr << "error opening matrix file " << filename << endl;
exit(1);
}
for (size_t i=0; i<vec.size(); i++) {
outfile << vec[i] << endl;
}
outfile.close();
}
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;
}
double LogisticRegressionObjective::EvalLocalMultiThread(const DblVec& input, DblVec& gradient){
/*
create 24 thread;
each thread calculate a loss and gradient
*/
int threadNum = 24;
double lossList[threadNum];
DblVec *gradList = new DblVec[threadNum];
for(int i = 0; i < threadNum; i++){
gradList[i] = DblVec(gradient.size());
}
threadList = new pthread_t[threadNum];
for(int i = 0; i < threadNum; i++){
Parameter*p = new Parameter(*this, input, gradList[i], lossList[i], i, threadNum);
pthread_create(&threadList[i], NULL, ThreadEvalLocal, p);
}
for(int i = 0; i < threadNum; i++){
pthread_join(threadList[i], NULL);
}
double loss = 0.0;
for(int j = 0; j < gradient.size(); j++){
gradient[j] = 0.0;
}
for(int i = 0; i < threadNum; i++){
loss += lossList[i];
for(int j = 0; j < gradient.size(); j++){
gradient[j] += gradList[i][j];
}
}
delete []gradList;
delete []threadList;
return loss;
}
double Mse(const DblVec &rec, const char* filename) {
static DblVec temp(rec.size());
ifstream infile(filename);
if (!infile.good()) {
cerr << "error opening matrix file " << filename << endl;
exit(1);
}
// outfile << "%%MatrixMarket matrix array real general" << endl;
// outfile << "1 " << vec.size() << endl;
for (size_t i=0; i<rec.size(); i++) {
float val;
infile >> val;
temp[i] = val;
}
infile.close();
double final_mse = 0;
for (size_t i=0; i<rec.size();i++){
final_mse += (rec[i]-temp[i])*(rec[i]-temp[i]);
}
final_mse /=rec.size();
return final_mse;
}
double LogisticRegressionObjective::Eval(const DblVec& input, DblVec& gradient){
// clock_t start,finish;
// double totaltime;
DblVec localInput = input;
DblVec localGradient = gradient;
MPI_Bcast(&localInput[0], localInput.size(), MPI_DOUBLE, 0, MPI_COMM_WORLD);
MPI_Bcast(&localGradient[0], localGradient.size(), MPI_DOUBLE, 0, MPI_COMM_WORLD);
double loss = EvalLocalMultiThread(localInput, localGradient);
// start=clock();
// double loss = EvalLocal(localInput, localGradient);
double gloss = 0.0;
MPI_Reduce(&localGradient[0], &gradient[0], input.size(), MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
MPI_Reduce(&loss, &gloss, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
// finish=clock();
// totaltime=(double)(finish-start)/CLOCKS_PER_SEC;
// cout<<"\nReduce Time: "<<totaltime<<"seconds"<<endl;
return gloss;
}
double NeedleCollisionClearanceCost::value(const vector<double>& x, Model* model) {
DblVec dists;
EnvironmentBasePtr env = helper->pis[0]->local_configs[0]->GetEnv();
BOOST_FOREACH(const CollisionEvaluatorPtr& cc, this->ccs) {
DblVec tmp_dists;
cc->CalcDists(x, tmp_dists);
for (int i = 0; i < tmp_dists.size(); ++i) {
dists.push_back(-tmp_dists[i]);
}
}
if (dists.size() > 0) {
return vecMax(dists) * this->coeff;
} else {
return - helper->collision_clearance_threshold * this->coeff;
}
}
void CirclesSeparation::initialize(DblVec &x, DblVec &y, DblVec r, DblVec m) {
_N = (int)x.size();
double minx = 10000, maxx = -10000, miny = 10000, maxy = -10000;
int i;
for (i = 0; i < _N; ++i) {
minx = min(minx, x[i]);
maxx = max(maxx, x[i]);
miny = min(miny, y[i]);
maxy = max(maxy, y[i]);
}
_cenx = (minx+maxx) * 0.5;
_ceny = (miny+maxy) * 0.5;
for (i = 0; i < _N; ++i) {
x[i] -= _cenx, y[i] -= _ceny;
}
_x = x, _y = y, _r = r, _m = m, _ox = x, _oy = y;
}
double SparseLeastSquaresObjective::Eval(const DblVec& input, DblVec& gradient) {
static DblVec temp(problem.n);
if (input.size() != problem.numFeats) {
cerr << "Error: input is not the correct size." << endl;
exit(1);
}
for (size_t i=0; i<problem.n; i++) {
temp[i] = -problem.target[i];
for (size_t j=problem.instance_starts[i]; j < problem.instance_starts[i+1]; j++) {
size_t idx = problem.indices[j];
double val = (double) problem.values[j];
temp[i] += input[idx] * val;
}
}
double value = 0.0;
for (size_t j=0; j<problem.numFeats; j++) {
value += input[j] * input[j] * l2weight;
gradient[j] = l2weight * input[j];
}
for (size_t i=0; i<problem.n; i++) {
if (temp[i] == 0) continue;
value += temp[i] * temp[i];
for (size_t j=problem.instance_starts[i]; j<problem.instance_starts[i+1]; j++) {
size_t idx = problem.indices[j];
double val = (double) problem.values[j];
gradient[idx] += val * temp[i];
}
}
return 0.5 * value + 1.0;
}
void InitInfo::fromJson(const Json::Value& v) {
string type_str;
childFromJson(v, type_str, "type");
int n_steps = gPCI->basic_info.n_steps;
int n_dof = gPCI->rad->GetDOF();
if (type_str == "stationary") {
data = toVectorXd(gPCI->rad->GetDOFValues()).transpose().replicate(n_steps, 1);
}
else if (type_str == "given_traj") {
FAIL_IF_FALSE(v.isMember("data"));
const Value& vdata = v["data"];
if (vdata.size() != n_steps) {
PRINT_AND_THROW("given initialization traj has wrong length");
}
data.resize(n_steps, n_dof);
for (int i=0; i < n_steps; ++i) {
DblVec row;
fromJsonArray(vdata[i], row, n_dof);
data.row(i) = toVectorXd(row);
}
}
else if (type_str == "straight_line") {
FAIL_IF_FALSE(v.isMember("endpoint"));
DblVec endpoint;
childFromJson(v, endpoint, "endpoint");
if (endpoint.size() != n_dof) {
PRINT_AND_THROW(boost::format("wrong number of dof values in initialization. expected %i got %j")%n_dof%endpoint.size());
}
data = TrajArray(n_steps, n_dof);
DblVec start = gPCI->rad->GetDOFValues();
for (int idof = 0; idof < n_dof; ++idof) {
data.col(idof) = VectorXd::LinSpaced(n_steps, start[idof], endpoint[idof]);
}
}
}
void OptimizerState::scaleInto(DblVec& a, const DblVec& b, double c){
for (size_t i= 0; i < a.size(); i++){
a[i] = b[i] * c;
}
}
void OptimizerState::scale(DblVec& a, double b){
for (size_t i = 0; i < a.size(); i++){
a[i] *= b;
}
}
void OptimizerState::addMultInto(DblVec& a, const DblVec& b, const DblVec& c, double d){
for (size_t i = 0; i < a.size(); i++){
a[i] = b[i] + c[i] * d;
}
}
void OptimizerState::add(DblVec& a, const DblVec& b){
for (size_t i = 0; i <a.size(); i++){
a[i] += b[i];
}
}
void OptimizerState::addMult(DblVec& a, const DblVec& b, double c){
for (size_t i = 0; i < a.size(); i++){
a[i] += b[i] * c;
}
}
inline double vecSum(const DblVec& v) {
double out = 0;
for (int i=0; i < v.size(); ++i) out += v[i];
return out;
}
inline double vecAbsSum(const DblVec& v) {
double out = 0;
for (int i=0; i < v.size(); ++i) out += fabs(v[i]);
return out;
}
void expectAllNear(const DblVec& x, const DblVec& y, double abstol) {
EXPECT_EQ(x.size(), y.size());
stringstream ss;
LOG_INFO("checking %s ?= %s", CSTR(x), CSTR(y));
for (size_t i=0; i < x.size(); ++i) EXPECT_NEAR(x[i], y[i], abstol);
}
inline double vecHingeSum(const DblVec& v) {
double out = 0;
for (int i=0; i < v.size(); ++i) out += pospart(v[i]);
return out;
}
inline double vecDot(const DblVec& a, const DblVec& b) {
assert(a.size() == b.size());
double out=0;
for (int i=0; i < a.size(); ++i) out += a[i]*b[i];
return out;
}