void Stwart(vector<long>&IA,vector<long>&MA,
vector<long>&JROW,vector<long>&JCOL)
{
static long M = JROW.size();
vector<long> IR(M),IC(M),R(M),C(M),IP(M),JP(M),IW(M);
static long KERNS, MEND, LG, IER, L = MA[M];
// printf("M = %ld\n",M);
// forVector(MA,i) printf("MA[%ld] = %ld\n",i,MA[i]);
// forVector(IA,i) printf("IA[%ld] = %ld\n",i,IA[i]);
// printf("L=%ld\n",L);
// forVector(R,i) printf("R[%ld] = %ld\n",i,R[i]);
// forVector(C,i) printf("C[%ld] = %ld\n",i,C[i]);
// forVector(IR,i) printf("IR[%ld] = %ld\n",i,IR[i]);
// forVector(IC,i) printf("IC[%ld] = %ld\n",i,IC[i]);
// forVector(JROW,i) printf("JROW[%ld] = %ld\n",i,JROW[i]);
// forVector(JCOL,i) printf("JCOL[%ld] = %ld\n",i,JCOL[i]);
// forVector(IP,i) printf("IP[%ld] = %ld\n",i,IP[i]);
// forVector(JP,i) printf("JP[%ld] = %ld\n",i,JP[i]);
// forVector(IW,i) printf("IW[%ld] = %ld\n",i,IW[i]);
printf("KERNS = %ld, MEND = %ld, LG = %ld, IER = %ld\n",
KERNS, MEND, LG, IER);
stwart_(&IA[0],&L,&MA[0],&M,&R[0],&C[0],&IR[0],&IC[0],
&JROW[0],&JCOL[0],&IP[0],&JP[0],&KERNS,&MEND,&IW[0],&LG,&IER);
}
size_t
SpatialEdge::newEdge(size_t emindex, size_t index, int k)
{
Edge *en, *em;
size_t swap;
em = &edges_[emindex];
switch (k) {
case 0:
em->start_ = IV(1);
em->end_ = IV(2);
break;
case 1:
em->start_ = IV(0);
em->end_ = IV(2);
break;
case 2:
em->start_ = IV(0);
em->end_ = IV(1);
break;
}
// sort the vertices by increasing index
if(em->start_ > em->end_) {
swap = em->start_;
em->start_ = em->end_;
em->end_ = swap;
}
// check all previous edges for a match, return pointer if
// already present, log the midpoint with the new face as well
if( (en=edgeMatch(em)) != NULL){
IW(k) = en->mid_;
return emindex;
}
// this is a new edge, immediately process the midpoint,
// and save it with the nodes and the edge as well
insertLookup(em);
IW(k) = getMidPoint(em);
em->mid_ = IW(k);
return ++emindex;
}
static void
dec_1000a_disable_intr(int irq)
{
int imrval = IRQ2IMR(irq);
int i = imrval >= 16;
IW(i, IR(i) & ~(1 << (imrval & 0xf)));
}
void pSubChain::calc_acc_body()
{
// if(!children[1]) return;
#ifdef USE_MPI
if(sim->rank != rank) return;
if(children[0] && sim->rank != children[0]->rank)
{
children[0]->recv_acc();
}
if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
{
children[1]->recv_acc();
}
#endif
// update_log << "--- " << last_joint->name << ": calc_acc_body" << endl;
int i, j;
// compute f_temp
static fVec da;
static fMat PK;
PK.resize(6, n_dof);
da.resize(n_const);
for(i=0; i<6; i++)
{
for(j=0; j<n_dof; j++)
PK(i, j) = P(i, joint_index[j]);
}
if(last_joint->n_dof > 0)
{
switch(last_joint->j_type)
{
case JROTATE:
case JSLIDE:
tau(0) = last_joint->tau;
break;
case JSPHERE:
tau(0) = last_joint->tau_n(0);
tau(1) = last_joint->tau_n(1);
tau(2) = last_joint->tau_n(2);
break;
case JFREE:
tau(0) = last_joint->tau_f(0);
tau(1) = last_joint->tau_f(1);
tau(2) = last_joint->tau_f(2);
tau(3) = last_joint->tau_n(0);
tau(4) = last_joint->tau_n(1);
tau(5) = last_joint->tau_n(2);
break;
default:
break;
}
da6.mul(PK, tau);
}
else
da6.zero();
// cerr << "da6(0) = " << tran(da6) << endl;
// + child_side - parent_side ?
da6 += children[0]->acc_temp[last_index[0]];
// cerr << "da6(1) = " << tran(da6) << endl;
if(children[1])
da6 -= children[1]->acc_temp[last_index[1]];
// cerr << "da6(2) = " << tran(da6) << endl;
// motion controlled joints
if(!last_joint->t_given)
{
switch(last_joint->j_type)
{
case JROTATE:
case JSLIDE:
da6(axis) -= last_joint->qdd;
// update_log << last_joint->name << ": qdd = " << last_joint->qdd << endl;
break;
case JSPHERE:
da6(3) -= last_joint->rel_ang_acc(0);
da6(4) -= last_joint->rel_ang_acc(1);
da6(5) -= last_joint->rel_ang_acc(2);
break;
case JFREE:
da6(0) -= last_joint->rel_lin_acc(0);
da6(1) -= last_joint->rel_lin_acc(1);
da6(2) -= last_joint->rel_lin_acc(2);
da6(3) -= last_joint->rel_ang_acc(0);
da6(4) -= last_joint->rel_ang_acc(1);
da6(5) -= last_joint->rel_ang_acc(2);
break;
}
}
static fVec f(6);
// cerr << "Gamma = " << Gamma << endl;
// cerr << "Gamma_inv = " << Gamma_inv << endl;
#if 0
// actually we could save some computation by
// selecting const rows first
for(i=0; i<n_const; i++)
da(i) = -da6(const_index[i]);
f_temp.mul(Gamma_inv, da);
// f_temp.lineq_posv(Gamma, da);
// compute acc at all outer joints
for(i=0; i<n_dof; i++)
f(joint_index[i]) = tau(i);
for(i=0; i<n_const; i++)
f(const_index[i]) = f_temp(i);
// cerr << "da = " << tran(da) << endl;
// cerr << "f_temp = " << tran(f_temp) << endl;
// cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
#else
#if 0
f.mul(W, da6);
for(i=0; i<n_dof; i++)
{
f(joint_index[i]) += tau(i);
}
#else
static fVec db(6), Wdb(6), IWRtau(6);
static fMat IWR;
IWR.resize(6, n_dof);
db.set(children[0]->acc_temp[last_index[0]]);
if(children[1])
db -= children[1]->acc_temp[last_index[1]];
Wdb.mul(W, db);
for(i=0; i<6; i++)
{
for(j=0; j<n_dof; j++)
{
IWR(i, j) = IW(joint_index[j], i);
}
}
IWRtau.mul(IWR, tau);
// cerr << "W = " << tran(W) << endl;
// update_log << "db = " << tran(db) << endl;
// cerr << "Wdb = " << tran(Wdb) << endl;
// cerr << "IWRtau = " << tran(IWRtau) << endl;
f.add(Wdb, IWRtau);
// update_log << "f = " << tran(f) << endl;
#ifdef PSIM_TEST
////// -> test
for(i=0; i<n_const; i++)
{
da(i) = -da6(const_index[i]);
f_temp(i) = f(const_index[i]);
}
// cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
total_gamma_error += (Gamma*f_temp-da) * (Gamma*f_temp-da);
////// <-
#endif
#endif
#endif
for(i=0; i<n_outer_joints; i++)
{
int org = outer_joints_origin[i];
int index = outer_joints_index[i];
int ilast = last_index[org];
acc_temp[i].mul(children[org]->Lambda[index][ilast], f);
if(org == 1)
{
acc_temp[i] *= -1.0;
}
acc_temp[i] += children[org]->acc_temp[index];
// update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
}
#ifdef USE_MPI
if(parent && sim->rank != parent->rank)
{
send_acc(parent->rank);
}
#endif
}
void pSubChain::calc_inertia_body()
{
// for space
// if(!children[1]) return;
#ifdef USE_MPI
if(sim->rank != rank) return;
if(children[0] && sim->rank != children[0]->rank)
{
// cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[0]->rank << endl;
children[0]->recv_inertia();
}
if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
{
// cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[1]->rank << endl;
children[1]->recv_inertia();
}
#endif
int i, j;
// cerr << "---- " << last_joint->name << ": calc_inertia_body" << endl;
// P
if(children[1])
{
P.add(children[0]->Lambda[last_index[0]][last_index[0]], children[1]->Lambda[last_index[1]][last_index[1]]);
}
else
{
P.set(children[0]->Lambda[last_index[0]][last_index[0]]);
}
// cerr << "Lambda[0] = " << children[0]->Lambda[last_index[0]][last_index[0]] << endl;
// cerr << "Lambda[1] = " << children[1]->Lambda[last_index[1]][last_index[1]] << endl;
#ifdef PSIM_TEST
{
fMat U1(6,6), V1T(6,6), U2(6,6), V2T(6,6);
fVec sigma1(6), sigma2(6);
children[0]->Lambda[last_index[0]][last_index[0]].svd(U1, sigma1, V1T);
children[1]->Lambda[last_index[1]][last_index[1]].svd(U2, sigma2, V2T);
double s1 = sigma1.length(), s2 = sigma2.length();
if(s1 > 1e-8 && s2 > 1e-8)
{
double ratio = (s1 < s2) ? s2/s1 : s1/s2;
if(max_sigma_ratio < 0.0 || ratio > max_sigma_ratio)
{
max_sigma_ratio = ratio;
max_sigma_ratio_joint = last_joint;
}
sigma_ratios(last_joint->i_dof) = ratio;
// cerr << last_joint->name << ": " << s1 << ", " << s2 << " -> " << ratio << endl;
}
}
#endif
if(children[0] == children[1])
{
P -= children[0]->Lambda[last_index[0]][last_index[1]];
P -= children[0]->Lambda[last_index[1]][last_index[0]];
}
// P should be symmetric
// P += tran(P);
// P *= 0.5;
P.symmetric();
#ifndef USE_DCA
// Gamma
if(n_const > 0)
{
for(i=0; i<n_const; i++)
{
for(j=0; j<n_const; j++)
Gamma(i, j) = P(const_index[i], const_index[j]);
}
if(children[0] != children[1])
{
// Gamma is symmetric, positive-definite
if(Gamma_inv.inv_posv(Gamma))
Gamma_inv.inv_svd(Gamma);
// Gamma_inv.inv_porfs(Gamma);
#ifdef PSIM_TEST
fMat U(n_const, n_const), VT(n_const, n_const);
fVec sigma(n_const);
Gamma.svd(U, sigma, VT);
double cn = sigma(0) / sigma(n_const-1);
if(max_condition_number < 0.0 || cn > max_condition_number)
{
max_condition_number = cn;
max_condition_number_joint = last_joint;
}
condition_numbers(last_joint->i_dof) = cn;
// cerr << "condition_number = " << cn << endl;
// fMat Gamma_inv2(n_const, n_const);
// Gamma_inv2.inv_svd(Gamma, 2000);
// cerr << last_joint->name << ": " << cn << endl;
// fMat I(n_const, n_const);
// I.identity();
// cerr << last_joint->name << ": " << I - Gamma*Gamma_inv << endl;
// cerr << last_joint->name << ": " << Gamma_inv - Gamma_inv2 << endl;
#endif
}
else
{
// Gamma may be singular
Gamma_inv.inv_svd(Gamma);
// cerr << Gamma_inv * Gamma << endl;
}
}
// W, IW
W.zero();
for(i=0; i<n_const; i++)
{
for(int j=0; j<n_const; j++)
{
W(const_index[i], const_index[j]) = -Gamma_inv(i, j);
}
}
#else // #ifndef USE_DCA
static fMat SV, VSV;
SV.resize(n_dof, 6);
VSV.resize(n_dof, 6);
Vhat.inv_posv(P);
if(n_dof > 0)
{
for(i=0; i<n_dof; i++)
{
for(int j=0; j<n_dof; j++)
{
SVS(i,j) = Vhat(joint_index[i], joint_index[j]);
}
for(int j=0; j<6; j++)
{
SV(i,j) = Vhat(joint_index[i], j);
}
}
// cerr << "SVS = " << SVS << endl;
VSV.lineq(SVS, SV);
// cerr << "VSV = " << VSV << endl;
W.mul(tran(SV), VSV);
// W *= -1.0;
}
else
{
W.zero();
}
W -= Vhat;
#endif
// cerr << "P = " << P << endl;
// cerr << "W = " << W << endl;
IW.mul(P, W);
for(i=0; i<6; i++)
{
IW(i, i) += 1.0;
}
// cerr << "IW = " << IW << endl;
// Lambda
if(n_const == 0)
{
for(i=0; i<n_outer_joints; i++)
{
int org_i = outer_joints_origin[i];
int index_i = outer_joints_index[i];
for(j=i; j<n_outer_joints; j++)
{
int org_j = outer_joints_origin[j];
int index_j = outer_joints_index[j];
if(children[org_i] == children[org_j])
{
Lambda[i][j].set(children[org_i]->Lambda[index_i][index_j]);
}
if(i != j)
{
Lambda[j][i].tran(Lambda[i][j]);
}
}
}
}
else
{
for(i=0; i<n_outer_joints; i++)
{
int org_i = outer_joints_origin[i];
int index_i = outer_joints_index[i];
fMat& Lambda_i = children[org_i]->Lambda[index_i][last_index[org_i]];
#ifndef USE_DCA
int m, n;
static fMat LKi, KLj, GKLj;
LKi.resize(6, n_const);
KLj.resize(n_const, 6);
GKLj.resize(n_const, 6);
for(m=0; m<6; m++)
{
for(n=0; n<n_const; n++)
LKi(m, n) = Lambda_i(m, const_index[n]);
}
for(j=i; j<n_outer_joints; j++)
{
int org_j = outer_joints_origin[j];
int index_j = outer_joints_index[j];
fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
for(m=0; m<n_const; m++)
{
for(n=0; n<6; n++)
KLj(m, n) = Lambda_j(const_index[m], n);
}
GKLj.mul(Gamma_inv, KLj);
// GKLj.lineq_posv(Gamma, KLj);
Lambda[i][j].mul(LKi, GKLj);
if(children[org_i] == children[org_j])
{
Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
Lambda[i][j] *= -1.0;
}
if(i != j)
{
Lambda[j][i].tran(Lambda[i][j]);
}
else
{
Lambda[i][j].symmetric();
}
}
#else // #ifndef USE_DCA
for(j=i; j<n_outer_joints; j++)
{
int org_j = outer_joints_origin[j];
int index_j = outer_joints_index[j];
fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
static fMat WL(6,6);
WL.mul(W, Lambda_j);
WL *= -1.0;
Lambda[i][j].mul(Lambda_i, WL);
if(children[org_i] == children[org_j])
{
Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
Lambda[i][j] *= -1.0;
}
if(i != j)
{
Lambda[j][i].tran(Lambda[i][j]);
}
else
{
Lambda[i][j].symmetric();
}
}
#endif
}
}
#ifdef USE_MPI
if(parent && sim->rank != parent->rank)
{
// cerr << sim->rank << ": joint (" << last_joint->name << "): send_inertia to " << parent->rank << endl;
send_inertia(parent->rank);
}
#endif
}
/*
* Initialize mystery ICU
*/
static void
pci_1000a_imi(void)
{
IW(0, IR(0) & 1);
IW(1, IR(0) & 3);
}
void
MAST::NPSOLOptimizationInterface::optimize() {
#if MAST_ENABLE_NPSOL == 1
// make sure that functions have been provided
libmesh_assert(_funobj);
libmesh_assert(_funcon);
int
N = _feval->n_vars(),
NCLIN = 0,
NCNLN = _feval->n_eq()+_feval->n_ineq(),
NCTOTL = N+NCLIN+NCNLN,
LDA = std::max(NCLIN, 1),
LDJ = std::max(NCNLN, 1),
LDR = N,
INFORM = 0, // on exit: Reports result of call to NPSOL
// < 0 either funobj or funcon has set this to -ve
// 0 => converged to point x
// 1 => x satisfies optimality conditions, but sequence of iterates has not converged
// 2 => Linear constraints and bounds cannot be satisfied. No feasible solution
// 3 => Nonlinear constraints and bounds cannot be satisfied. No feasible solution
// 4 => Major iter limit was reached
// 6 => x does not satisfy first-order optimality to required accuracy
// 7 => function derivatives seem to be incorrect
// 9 => input parameter invalid
ITER = 0, // iter count
LENIW = 3*N + NCLIN + 2*NCNLN,
LENW = 2*N*N + N*NCLIN + 2*N*NCNLN + 20*N + 11*NCLIN + 21*NCNLN;
Real
F = 0.; // on exit: final objective
std::vector<int>
IW (LENIW, 0),
ISTATE (NCTOTL, 0); // status of constraints l <= r(x) <= u,
// -2 => lower bound is violated by more than delta
// -1 => upper bound is violated by more than delta
// 0 => both bounds are satisfied by more than delta
// 1 => lower bound is active (to within delta)
// 2 => upper bound is active (to within delta)
// 3 => boundars are equal and equality constraint is satisfied
std::vector<Real>
A (LDA, 0.), // this is used for liear constraints, not currently handled
BL (NCTOTL, 0.),
BU (NCTOTL, 0.),
C (NCNLN, 0.), // on exit: nonlinear constraints
CJAC (LDJ* N, 0.), //
// on exit: CJAC(i,j) is the partial derivative of ith nonlinear constraint
CLAMBDA (NCTOTL, 0.), // on entry: need not be initialized for cold start
// on exit: QP multiplier from the QP subproblem, >=0 if istate(j)=1, <0 if istate(j)=2
G (N, 0.), // on exit: objective gradient
R (LDR*N, 0.), // on entry: need not be initialized if called with Cold Statrt
// on exit: information about Hessian, if Hessian=Yes, R is upper Cholesky factor of approx H
X (N, 0.), // on entry: initial point
// on exit: final estimate of solution
W (LENW, 0.), // workspace
xmin (N, 0.),
xmax (N, 0.);
// now setup the lower and upper limits for the variables and constraints
_feval->init_dvar(X, xmin, xmax);
for (unsigned int i=0; i<N; i++) {
BL[i] = xmin[i];
BU[i] = xmax[i];
}
// all constraints are assumed to be g_i(x) <= 0, so that the upper
// bound is 0 and lower bound is -infinity
for (unsigned int i=0; i<NCNLN; i++) {
BL[i+N] = -1.e20;
BU[i+N] = 0.;
}
std::string nm;
// nm = "List";
// npoptn_(nm.c_str(), (int)nm.length());
// nm = "Verify level 3";
// npoptn_(nm.c_str(), (int)nm.length());
npsol_(&N,
&NCLIN,
&NCNLN,
&LDA,
&LDJ,
&LDR,
&A[0],
&BL[0],
&BU[0],
_funcon,
_funobj,
&INFORM,
&ITER,
&ISTATE[0],
&C[0],
&CJAC[0],
&CLAMBDA[0],
&F,
&G[0],
&R[0],
&X[0],
&IW[0],
&LENIW,
&W[0],
&LENW);
#endif // MAST_ENABLE_NPSOL 1
}
