vector<double> BPMPDModel::getVarValues(const VarVector& vars) const {
vector<double> out(vars.size());
for (int i=0; i < vars.size(); ++i) {
int varind = vars[i].var_rep->index;
out[i] = m_soln[varind];
}
return out;
}
VarVector _GetVars(py::list ijs, const VarArray& vars) {
VarVector out;
int n = py::len(ijs);
for (int k=0; k < n; ++k) {
int i = py::extract<int>(ijs[k][0]);
int j = py::extract<int>(ijs[k][1]);
out.push_back(vars(i,j));
}
return out;
}
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));
}
}
Token getValue(Token t)
{
if(t.tkntype == TKN_NUMBER)
return t;
else
{
t.tkntype = TKN_NUMBER;
t.number = varv.getVar(t.str);
return t;
}
}
bool Variable::unify(const GlobalStorage *g, const Value& t, const VarVector& vars, ValueVector& vals) const
{
// TODO: constant access would be nice
for (size_t i = 0; i < vars.size(); ++i)
{
// if variable is bound
if (vars[i] == uid_)
{
// variable is not set, then set, else compare
if (vals[i].type_ == Value::UNDEF)
{
vals[i] = t;
return true;
}
else
return vals[i].equal(t);
}
}
FAIL(true);
}
inline OutVecType getVec1(const vector<double>& x, const VarVector& vars) {
OutVecType out(vars.size());
for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
return out;
}
inline void setVec(DblVec& x, const VarVector& vars, const VecType& vals) {
assert(vars.size() == vals.size());
for (int i = 0; i < vars.size(); ++i) {
x[vars[i].var_rep->index] = vals[i];
}
}
void PushObject::updateModel(const Eigen::MatrixXd& traj, GRBQuadExpr& objective) {
VectorXd& times = m_problem->m_times;
MatrixXd perts_tk = getSinBasis(times/times.maxCoeff(), fmin(6, times.size()/2));
MatrixXd pinvperts_tk = perts_tk * (perts_tk.transpose() * perts_tk).inverse();
m_exactObjective = simulateTraj2(traj, true); // current value
LOG_INFO_FMT("current val: %.3f", m_exactObjective);
MatrixXd dy_jk(traj.cols(), perts_tk.cols());
double eps = 3e-4; // scale for joint angle change
Matrix3d A;
A << sq(eps/2), eps/2, 1,
0, 0, 1,
sq(eps/2), -eps/2, 1;
Matrix3d Ainv = A.inverse();
MatrixXd grad_tj(traj.rows(), traj.cols());
m_obj = GRBQuadExpr(0);
for (int j = 0; j < traj.cols(); ++j) {
VarVector v;
VectorXd vactual(traj.rows()-1);
for (int t=1; t < traj.rows(); ++t) {
v.push_back(m_problem->m_trajVars.at(t,j));
vactual(t-1) = traj(t,j);
}
for (int k = 0; k < perts_tk.cols(); ++k) {
MatrixXd newTraj = traj;
newTraj.col(j) = traj.col(j) + (eps/2)*perts_tk.col(k);
double plusVal = simulateTraj2(newTraj, false);
newTraj.col(j) = traj.col(j) - (eps/2)*perts_tk.col(k);
double minusVal = simulateTraj2(newTraj, false);
LOG_DEBUG_FMT("joint %i, basis %i, pert vals: %.4e %.4e ", j, k, plusVal-m_exactObjective,minusVal-m_exactObjective);
dy_jk(j,k) = (plusVal - minusVal)/eps;
Vector3d y;
y << plusVal-m_exactObjective, 0, minusVal - m_exactObjective;
Vector3d abc = Ainv*y;
GRBLinExpr q;
int T = traj.rows()-1;
VectorXd pertVec=perts_tk.block(1,k,T, 1);
q.addTerms(pertVec.data(),v.data(), T);
double qactual = pertVec.dot(vactual);
m_obj += fmax(abc(0),0)*(q-qactual)*(q-qactual) + abc(1)*(q-qactual);
}
// grad_tj.col(j) = pinvperts_tk * dy_jk.row(j).transpose();
}
m_obj += m_exactObjective;
// cout << "dy_jk:" << endl;
// cout << dy_jk << endl;
// cout << "grad_tj:" << endl;
// cout << grad_tj << endl;
// m_obj.addTerms(grad_tj.data()+7, m_problem->m_trajVars.m_data.data()+7, (traj.rows()-1)*traj.cols());
// m_obj += m_exactObjective - (grad_tj.middleRows(1, traj.rows()-1).array() * traj.middleRows(1, traj.rows()-1).array()).sum();
objective += m_obj;
}
DblVec getDblVec(const vector<double>& x, const VarVector& vars) {
DblVec out(vars.size());
for (unsigned i=0; i < vars.size(); ++i) out[i] = x[vars[i].var_rep->index];
return out;
}
void BPMPDModel::removeVars(const VarVector& vars) {
vector<int>inds = vars2inds(vars);
for (int i=0; i < vars.size(); ++i) vars[i].var_rep->removed = true;
}
/**
* @brief Sorts the given vector by the name of its elements (case-insensitively).
*/
void sortByName(VarVector &vec) {
std::sort(vec.begin(), vec.end(), compareVars);
}
void IRCSession::calculate(string privmsg)
{
sk = TokenStack();
while(!q.empty())
{
Token t = q.get();
if(t.tkntype == TKN_NUMBER || t.tkntype == TKN_NAME)
sk.put(t);
else if(t.tkntype == TKN_OPERATOR)
{
if(t.str == "+")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, o1.number+o2.number, "");
sk.put(e);
}
else if(t.str == "-")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, o1.number-o2.number, "");
sk.put(e);
}
else if(t.str == "*")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, o1.number*o2.number, "");
sk.put(e);
}
else if(t.str == "/")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, o1.number/o2.number, "");
sk.put(e);
}
else if(t.str == "%")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, cast_int(o1.number)%cast_int(o2.number), "");
sk.put(e);
}
else if(t.str == "^" || t.str == "**")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, pow(o1.number, o2.number), "");
sk.put(e);
}
/*else if(t.str == "sin")
{
Token o1, o2;
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o2 = getValue(sk.get());
if(!sk.empty() && (sk.seek().tkntype == TKN_NUMBER || sk.seek().tkntype == TKN_NAME))
o1 = getValue(sk.get());
Token e = Token(TKN_NUMBER, sin(12), "");
sk.put(e);
}*/
}
}
double d;
if(!sk.empty())
d = getValue(sk.get()).number;
else
d = 0.0f;
if(d-cast_int(d) < 0.00000049)
SendChatMessage(PRIVMSG, privmsg.c_str(), "%d", cast_int(d));
else
SendChatMessage(PRIVMSG, privmsg.c_str(), "%f", d);
varv.clear();
}
void IRCSession::setConsts()
{
varv.setVar("pi", 3.1415926535897932384626433832795f, true);
}
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));
}
}
