void Model::
computeInverseMassInertia()
{
if (ainv_upper_triangular_.empty()) {
ainv_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
}
deFloat const one(1);
for (size_t irow(0); irow < ndof_; ++irow) {
taoJoint * joint(kgm_tree_->info[irow].joint);
// Compute one column of Ainv by solving forward dynamics of the
// corresponding joint having a unit torque, while all the
// others remain unactuated. This works on the kgm_tree because
// it has zero speeds, thus the Coriolis-centrifgual effects are
// zero, and by using zero gravity we get pure system dynamics:
// acceleration = mass_inv * force (in matrix form).
joint->setTau(&one);
taoDynamics::fwdDynamics(kgm_tree_->root, &zero_gravity);
joint->zeroTau();
// Retrieve the column of Ainv by reading the joint
// accelerations generated by the column-selecting unit torque
// (into a flattened upper triangular matrix).
for (size_t icol(0); icol <= irow; ++icol) {
kgm_tree_->info[icol].joint->getDDQ(&ainv_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
}
}
// Reset all the accelerations.
for (size_t ii(0); ii < ndof_; ++ii) {
kgm_tree_->info[ii].joint->zeroDDQ();
}
}
autoConfusion Confusion_groupStimuli (Confusion me, const char32 *labels, const char32 *newLabel, long newpos) {
try {
long ncondense = Melder_countTokens (labels);
autoNUMvector<long> irow (1, my numberOfRows);
for (long i = 1; i <= my numberOfRows; i++) {
irow[i] = i;
}
for (char32 *token = Melder_firstToken (labels); token != nullptr; token = Melder_nextToken ()) {
for (long i = 1; i <= my numberOfRows; i++) {
if (Melder_equ (token, my rowLabels[i])) {
irow[i] = 0;
break;
}
}
}
long nfound = 0;
for (long i = 1; i <= my numberOfRows; i++) {
if (irow[i] == 0) {
nfound ++;
}
}
if (nfound == 0) {
Melder_throw (U"Invalid stimulus labels.");
}
if (nfound != ncondense) {
Melder_warning (U"One or more of the given stimulus labels are suspect.");
}
long newnstim = my numberOfRows - nfound + 1;
if (newpos < 1) {
newpos = 1;
}
if (newpos > newnstim) {
newpos = newnstim;
}
autoConfusion thee = Confusion_create (newnstim, my numberOfColumns);
NUMstrings_copyElements (my columnLabels, thy columnLabels, 1, my numberOfColumns);
TableOfReal_setRowLabel (thee.get(), newpos, newLabel);
long inewrow = 1;
for (long i = 1; i <= my numberOfRows; i++) {
long rowpos = newpos;
if (irow[i] > 0) {
if (inewrow == newpos) {
inewrow++;
}
rowpos = inewrow;
inewrow++;
TableOfReal_setRowLabel (thee.get(), rowpos, my rowLabels[i]);
}
for (long j = 1; j <= my numberOfColumns; j++) {
thy data[rowpos][j] += my data[i][j];
}
}
return thee;
} catch (MelderError) {
Melder_throw (me, U": stimuli not grouped.");
}
}
void Model::
computeMassInertia()
{
if (a_upper_triangular_.empty()) {
a_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
}
deFloat const one(1);
for (size_t irow(0); irow < ndof_; ++irow) {
taoJoint * joint(kgm_joints_[irow]);
// Compute one column of A by solving inverse dynamics of the
// corresponding joint having a unit acceleration, while all the
// others remain fixed. This works on the KGM tree because it
// has zero speeds, thus the Coriolis-centrifgual effects are
// zero, and by using zero gravity we get pure system dynamics:
// force = mass * acceleration (in matrix form).
joint->setDDQ(&one);
taoDynamics::invDynamics(kgm_root_, &zero_gravity);
joint->zeroDDQ();
// Retrieve the column of A by reading the joint torques
// required for the column-selecting unit acceleration (into a
// flattened upper triangular matrix).
for (size_t icol(0); icol <= irow; ++icol) {
kgm_joints_[icol]->getTau(&a_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
}
}
// Reset all the torques.
for (size_t ii(0); ii < ndof_; ++ii) {
kgm_joints_[ii]->zeroTau();
}
}
bool Model::
computeJacobian(taoDNode const * node,
double gx, double gy, double gz,
Matrix & jacobian) const
{
if ( ! node) {
return false;
}
ancestry_table_t::const_iterator iae(ancestry_table_.find(const_cast<taoDNode*>(node)));
if (iae == ancestry_table_.end()) {
return false;
}
ancestry_list_t const & alist(iae->second);
#ifdef DEBUG
fprintf(stderr, "computeJacobian()\ng: [% 4.2f % 4.2f % 4.2f]\n", gx, gy, gz);
#endif // DEBUG
// \todo Implement support for more than one joint per node, and
// more than one DOF per joint.
jacobian = Matrix::Zero(6, ndof_);
ancestry_list_t::const_iterator ia(alist.begin());
ancestry_list_t::const_iterator iend(alist.end());
for (/**/; ia != iend; ++ia) {
deVector6 Jg_col;
ia->joint->getJgColumns(&Jg_col);
int const icol(ia->id);
#ifdef DEBUG
fprintf(stderr, "iJg[%d]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
Jg_col.elementAt(0), Jg_col.elementAt(1), Jg_col.elementAt(2),
Jg_col.elementAt(3), Jg_col.elementAt(4), Jg_col.elementAt(5));
#endif // DEBUG
for (size_t irow(0); irow < 6; ++irow) {
jacobian.coeffRef(irow, icol) = Jg_col.elementAt(irow);
}
// Add the effect of the joint rotation on the translational
// velocity at the global point (column-wise cross product with
// [gx;gy;gz]). Note that Jg_col.elementAt(3) is the
// contribution to omega_x etc, because the upper 3 elements of
// Jg_col are v_x etc. (And don't ask me why we have to
// subtract the cross product, it probably got inverted
// somewhere)
jacobian.coeffRef(0, icol) -= -gz * Jg_col.elementAt(4) + gy * Jg_col.elementAt(5);
jacobian.coeffRef(1, icol) -= gz * Jg_col.elementAt(3) - gx * Jg_col.elementAt(5);
jacobian.coeffRef(2, icol) -= -gy * Jg_col.elementAt(3) + gx * Jg_col.elementAt(4);
#ifdef DEBUG
fprintf(stderr, "0Jg[%d]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
jacobian.coeff(0, icol), jacobian.coeff(1, icol), jacobian.coeff(2, icol),
jacobian.coeff(3, icol), jacobian.coeff(4, icol), jacobian.coeff(5, icol));
#endif // DEBUG
}
return true;
}
ublas::vector<ublas::matrix<double> > wishart_InvA_rnd(const int df, ublas::matrix<double>& S, const int mc) {
// Generates wishart matrix allowing for singular wishart
size_t p = S.size1();
ublas::vector<double> D(p);
ublas::matrix<double> P(p, p);
ublas::matrix<double> F(p, p);
F = ublas::zero_matrix<double>(p, p);
// make copy of S
// ublas::matrix<double> SS(S);
lapack::gesvd('A', 'A', S, D, P, F);
// svd0(S, P, D, F);
// P = trans(P);
//! correct for singular matrix
std::vector<size_t> ii;
for (size_t i=0; i<D.size(); ++i)
if (D(i) > norm_inf(D)*1e-9)
ii.push_back(i);
size_t r = ii.size();
ublas::indirect_array<> idx(r);
for (size_t i=0; i<r; ++i)
idx(i) = ii[i];
ublas::indirect_array<> irow(p);
for (size_t i=0; i<irow.size(); ++ i)
irow(i) = i;
ublas::matrix<double> Q(p, r);
// Q = prod(project(P, irow, idx), diagm(ublas::apply_to_all<functor::inv_sqrt<double> >(project(D, idx))));
// rprod does not seem any faster than diagonalizing D before multiplication
// Q = rprod(project(P, irow, idx), ublas::apply_to_all<functor::inv_sqrt<double> >(D));
axpy_prod(project(trans(P), irow, idx), diagm(ublas::apply_to_all<functor::inv_sqrt<double> >(project(D, idx))), Q, true);
// generate mc samples
ublas::vector<ublas::matrix<double> > K(mc);
for (int i=0; i<mc; ++i)
K(i) = wishart_1(df, Q, p, r);
return K;
}
bool Model::
computeJacobian(taoDNode const * node,
double gx, double gy, double gz,
Matrix & jacobian) const
{
if ( ! node) {
return false;
}
#ifdef DEBUG
fprintf(stderr, "computeJacobian()\ng: [% 4.2f % 4.2f % 4.2f]\n", gx, gy, gz);
#endif // DEBUG
// \todo Implement support for more than one joint per node, and
// more than one DOF per joint.
jacobian.resize(6, ndof_);
for (size_t icol(0); icol < ndof_; ++icol) {
deVector6 Jg_col; // in NDOF case, this will become an array of deVector6...
// in NOJ case, we will have to loop over all joints of a node...
kgm_joints_[icol]->getJgColumns(&Jg_col);
#ifdef DEBUG
fprintf(stderr, "iJg[%zu]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
Jg_col.elementAt(0), Jg_col.elementAt(1), Jg_col.elementAt(2),
Jg_col.elementAt(3), Jg_col.elementAt(4), Jg_col.elementAt(5));
#endif // DEBUG
for (size_t irow(0); irow < 6; ++irow) {
jacobian.coeffRef(irow, icol) = Jg_col.elementAt(irow);
}
// Add the effect of the joint rotation on the translational
// velocity at the global point (column-wise cross product with
// [gx;gy;gz]). Note that Jg_col.elementAt(3) is the
// contribution to omega_x etc, because the upper 3 elements of
// Jg_col are v_x etc. (And don't ask me why we have to
// subtract the cross product, it probably got inverted
// somewhere)
jacobian.coeffRef(0, icol) -= -gz * Jg_col.elementAt(4) + gy * Jg_col.elementAt(5);
jacobian.coeffRef(1, icol) -= gz * Jg_col.elementAt(3) - gx * Jg_col.elementAt(5);
jacobian.coeffRef(2, icol) -= -gy * Jg_col.elementAt(3) + gx * Jg_col.elementAt(4);
#ifdef DEBUG
fprintf(stderr, "0Jg[%zu]: [ % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f % 4.2f]\n",
icol,
jacobian.coeff(0, icol), jacobian.coeff(1, icol), jacobian.coeff(2, icol),
jacobian.coeff(3, icol), jacobian.coeff(4, icol), jacobian.coeff(5, icol));
#endif // DEBUG
}
return true;
}
bool Model::
getInverseMassInertia(Matrix & inverse_mass_inertia) const
{
if (ainv_upper_triangular_.empty()) {
return false;
}
inverse_mass_inertia.resize(ndof_, ndof_);
for (size_t irow(0); irow < ndof_; ++irow) {
for (size_t icol(0); icol <= irow; ++icol) {
inverse_mass_inertia.coeffRef(irow, icol) = ainv_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)];
if (irow != icol) {
inverse_mass_inertia.coeffRef(icol, irow) = inverse_mass_inertia.coeff(irow, icol);
}
}
}
return true;
}
T GaussJordan(VVT &a, VVT &b) {
const int n = a.size();
const int m = b[0].size();
VI irow(n), icol(n), ipiv(n);
T det = 1;
for (int i = 0; i < n; i++) {
int pj = -1, pk = -1;
for (int j = 0; j < n; j++) if (!ipiv[j])
for (int k = 0; k < n; k++) if (!ipiv[k])
if (pj == -1 || fabs(a[j][k]) > fabs(a[pj][pk])) { pj = j; pk = k; }
if (fabs(a[pj][pk]) < EPS) { return 0; }
ipiv[pk]++;
swap(a[pj], a[pk]);
swap(b[pj], b[pk]);
if (pj != pk) det *= -1;
irow[i] = pj;
icol[i] = pk;
T c = 1.0 / a[pk][pk];
det *= a[pk][pk];
a[pk][pk] = 1.0;
for (int p = 0; p < n; p++) a[pk][p] *= c;
for (int p = 0; p < m; p++) b[pk][p] *= c;
for (int p = 0; p < n; p++) if (p != pk) {
c = a[p][pk];
a[p][pk] = 0;
for (int q = 0; q < n; q++) a[p][q] -= a[pk][q] * c;
for (int q = 0; q < m; q++) b[p][q] -= b[pk][q] * c;
}
}
for (int p = n-1; p >= 0; p--) if (irow[p] != icol[p]) {
for (int k = 0; k < n; k++) swap(a[k][irow[p]], a[k][icol[p]]);
}
return det;
}
long
ARPACKEigenSolver::solveSparse(int nev, bool shift_invert, double sigma, char * which, int ncv, double tol, int maxit,
double * resid, bool AutoShift)
{
#ifdef THEA_HAVE_SUPERLU
MatrixWrapper<double>::SparseColumnMatrix const & scm = matrix.getSparseColumnMatrix();
if (scm.isEmpty())
{
THEA_WARNING << getName() << ": Attempting to compute eigenvalues of an empty matrix -- no eigenpairs computed";
return 0;
}
// Create the matrix
alwaysAssertM(MatrixUtil::isSquare(scm), std::string(getName()) + ": Operator matrix is not square");
alwaysAssertM(scm.isValid(), std::string(getName()) + ": Operator matrix has invalid internal state");
TheaArray<int> irow(scm.getRowIndices().begin(), scm.getRowIndices().end());
TheaArray<int> pcol(scm.getColumnIndices().begin(), scm.getColumnIndices().end());
int nnz = (int)scm.numSetElements();
// Repeat the isValid() checks to check if something got screwed up during the conversion
alwaysAssertM(irow.size() == scm.getValues().size(),
std::string(getName()) + ": irow and nzval arrays should have same size");
alwaysAssertM(pcol.size() == (array_size_t)scm.numRows() + 1,
getName() + format(": pcol array should have %d + 1 = %d entries, instead has %d entries", scm.numRows(),
scm.numRows() + 1, (int)pcol.size()));
alwaysAssertM(nnz == pcol[pcol.size() - 1],
std::string(getName()) + ": (n + 1)th entry of pcol array should be number of non-zeros");
ARluNonSymMatrix<double, double> arm(scm.numRows(), nnz, const_cast<double *>(&scm.getValues()[0]), &irow[0], &pcol[0]);
// Setup the problem
shared_ptr< ARluNonSymStdEig<double> > eig =
shift_invert ? shared_ptr< ARluNonSymStdEig<double> >(new ARluNonSymStdEig<double>(nev, arm, sigma, which, ncv, tol,
maxit, resid, AutoShift))
: shared_ptr< ARluNonSymStdEig<double> >(new ARluNonSymStdEig<double>(nev, arm, which, ncv, tol, maxit,
resid, AutoShift));
eig->Trace();
// Find eigenpairs
array_size_t nconv = (array_size_t)eig->FindEigenvectors();
eigenvalues.resize(nconv);
eigenvectors.resize(nconv);
array_size_t n = (array_size_t)scm.numRows();
for (array_size_t i = 0; i < nconv; ++i)
{
eigenvalues[i] = Eigenvalue(eig->EigenvalueReal((int)i), eig->EigenvalueImag((int)i));
eigenvectors[i].resize(n);
for (array_size_t j = 0; j < n; ++j)
eigenvectors[i][j] = std::complex<double>(eig->EigenvectorReal((int)i, (int)j), eig->EigenvectorImag((int)i, (int)j));
}
return (long)nconv;
#else
throw Error(std::string(getName()) + ": Sparse linear solver (SuperLU) not available");
#endif
}
Row Project::get(Dir dir)
{
static Record emptyrec;
if (first)
{
first = false;
src_hdr = source->header();
proj_hdr = src_hdr.project(flds);
if (strategy == LOOKUP)
{
if (idx)
idx->free();
idx = new VVtree(td = new TempDest);
indexed = false;
}
}
if (strategy == COPY)
{
return source->get(dir);
}
else if (strategy == SEQUENTIAL)
{
if (dir == NEXT)
{
// output the first of each group
// i.e. skip over rows the same as previous output
Row row;
do
if (Eof == (row = source->get(NEXT)))
return Eof;
while (! rewound && equal(proj_hdr, row, currow));
rewound = false;
prevrow = currow;
currow = row;
// output the first row of a new group
return Row(lisp(emptyrec, row_to_key(src_hdr, row, flds)));
}
else // dir == PREV
{
// output the last of each group
// i.e. output when *next* record is different
// (to get the same records as NEXT)
if (rewound)
prevrow = source->get(PREV);
rewound = false;
Row row;
do
{
if (Eof == (row = prevrow))
return Eof;
prevrow = source->get(PREV);
}
while (equal(proj_hdr, row, prevrow));
// output the last row of a group
currow = row;
return Row(lisp(emptyrec, row_to_key(src_hdr, row, flds)));
}
}
else
{
verify(strategy == LOOKUP);
if (rewound)
{
rewound = false;
if (dir == PREV && ! indexed)
{
// pre-build the index
Row row;
while (Eof != (row = source->get(NEXT)))
{
Record key = row_to_key(src_hdr, row, flds);
Vdata data(row.data);
for (Lisp<Record> rs = row.data; ! nil(rs); ++rs)
td->addref(rs->ptr());
// insert will only succeed on first of dups
idx->insert(VVslot(key, &data));
}
source->rewind();
indexed = true;
}
}
Row row;
while (Eof != (row = source->get(dir)))
{
Record key = row_to_key(src_hdr, row, flds);
VVtree::iterator iter = idx->find(key);
if (iter == idx->end())
{
for (Lisp<Record> rs = row.data; ! nil(rs); ++rs)
td->addref(rs->ptr());
Vdata data(row.data);
verify(idx->insert(VVslot(key, &data)));
return Row(lisp(emptyrec, key));
}
else
{
Vdata* d = iter->data;
Records rs;
for (int i = d->n - 1; i >= 0; --i)
rs.push(Record::from_int(d->r[i], theDB()->mmf));
Row irow(rs);
if (row == irow)
return Row(lisp(emptyrec, key));
}
}
if (dir == NEXT)
indexed = true;
return Eof;
}
}
void Bayes::sample_muOmega() {
double tau = 10.0;
int d0 = p + 2;
// matrix_t S0(p, p);
ublas::matrix<double> S0(p, p);
S0 = d0 * ublas::identity_matrix<double>(p, p)/4.0;
std::vector<int> idx;
ublas::indirect_array<> irow(p);
// projection - want every row
for (size_t i=0; i<irow.size(); ++i)
irow(i) = i;
// size_t rank;
ublas::matrix<double> S(p, p);
// matrix_t SS(p, p);
ublas::matrix<double> SS(p, p);
ublas::matrix<double> Omega_inv(p, p);
// identity matrix for inverting cholesky factorization
ublas::matrix<double> I(p, p);
I.assign(ublas::identity_matrix<double> (p, p));
ublas::symmetric_adaptor<ublas::matrix<double>, ublas::upper> SH(I);
// triangular matrix for cholesky_decompose
ublas::triangular_matrix<double, ublas::lower, ublas::row_major> L(p, p);
int df;
for (int j=0; j<k; ++j) {
idx = find_all(z, j);
int n_idx = idx.size();
ublas::matrix<double> xx(p, n_idx);
ublas::matrix<double> e(p, n_idx);
// ublas::matrix<double> m(p, 1);
ublas::vector<double> m(p);
ublas::indirect_array<> icol(n_idx);
for (size_t i=0; i<idx.size(); ++i)
icol(i) = idx[i];
if (n_idx > 0) {
double a = tau/(1.0 + n_idx*tau);
//! REFACTOR - should be able to do matrix_sum directly on projection rather than make a copy?
xx.assign(project(x, irow, icol));
m.assign(ublas::matrix_sum(xx, 1)/n_idx);
// e.assign(xx - outer_prod(column(m, 0), ublas::scalar_vector<double>(n_idx, 1)));
e.assign(xx - outer_prod(m, ublas::scalar_vector<double>(n_idx, 1)));
S.assign(prod(e, trans(e)) + outer_prod(m, m) * n_idx * a/tau);
// SS = trans(S) + S0;
SS = S + S0;
df = d0 + n_idx;
// Omega(j).assign(wishart_rnd(df, SS));
Omega(j).assign(wishart_InvA_rnd(df, SS));
SH.assign(Omega(j));
cholesky_decompose(SH, L);
Omega_inv.assign(solve(SH, I, ublas::upper_tag()));
mu(j).assign(a*n_idx*m + sqrt(a)*prod(Omega_inv, Rmath::rnorm(0, 1, p)));
} else {
// Omega(j).assign(wishart_rnd(d0, S0));
Omega(j).assign(wishart_InvA_rnd(d0, S0));
SH.assign(Omega(j));
cholesky_decompose(SH, L);
Omega_inv.assign(solve(SH, I, ublas::upper_tag()));
mu(j).assign(sqrt(tau) * prod(Omega_inv, Rmath::rnorm(0, 1, p)));
}
}
}
