void Body::calcCMJacobian(Link *base, dmatrix &J)
{
// prepare subm, submwc
JointPathPtr jp;
if (base){
jp = getJointPath(rootLink(), base);
Link *skip = jp->joint(0);
skip->subm = rootLink()->m;
skip->submwc = rootLink()->m*rootLink()->wc;
Link *l = rootLink()->child;
if (l){
if (l != skip) {
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
while(l){
if (l != skip){
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
}
}
// assuming there is no branch between base and root
for (int i=1; i<jp->numJoints(); i++){
l = jp->joint(i);
l->subm = l->parent->m + l->parent->subm;
l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
}
J.resize(3, numJoints());
}else{
rootLink()->calcSubMassCM();
J.resize(3, numJoints()+6);
}
// compute Jacobian
std::vector<int> sgn(numJoints(), 1);
if (jp) {
for (int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
}
for (int i=0; i<numJoints(); i++){
Link *j = joint(i);
switch(j->jointType){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega(sgn[j->jointId]*j->R*j->a);
Vector3 arm((j->submwc-j->subm*j->p)/totalMass_);
Vector3 dp(omega.cross(arm));
J.col(j->jointId) = dp;
break;
}
default:
std::cerr << "calcCMJacobian() : unsupported jointType("
<< j->jointType << std::endl;
}
}
if (!base){
int c = numJoints();
J(0, c ) = 1.0; J(0, c+1) = 0.0; J(0, c+2) = 0.0;
J(1, c ) = 0.0; J(1, c+1) = 1.0; J(1, c+2) = 0.0;
J(2, c ) = 0.0; J(2, c+1) = 0.0; J(2, c+2) = 1.0;
Vector3 dp(rootLink()->submwc/totalMass_ - rootLink()->p);
J(0, c+3) = 0.0; J(0, c+4) = dp(2); J(0, c+5) = -dp(1);
J(1, c+3) = -dp(2); J(1, c+4) = 0.0; J(1, c+5) = dp(0);
J(2, c+3) = dp(1); J(2, c+4) = -dp(0); J(2, c+5) = 0.0;
}
}
/**
calculate the mass matrix using the unit vector method
\todo replace the unit vector method here with
a more efficient method that only requires O(n) computation time
The motion equation (dv != dvo)
| | | dv | | | | fext |
| out_M | * | dw | + | b1 | = | tauext |
| | |ddq | | | | u |
*/
void Body::calcMassMatrix(dmatrix& out_M)
{
// buffers for the unit vector method
dmatrix b1;
dvector ddqorg;
dvector uorg;
Vector3 dvoorg;
Vector3 dworg;
Vector3 root_w_x_v;
Vector3 g(0, 0, 9.8);
uint nJ = numJoints();
int totaldof = nJ;
if( !isStaticModel_ ) totaldof += 6;
out_M.resize(totaldof,totaldof);
b1.resize(totaldof, 1);
// preserve and clear the joint accelerations
ddqorg.resize(nJ);
uorg.resize(nJ);
for(uint i = 0; i < nJ; ++i){
Link* ptr = joint(i);
ddqorg[i] = ptr->ddq;
uorg [i] = ptr->u;
ptr->ddq = 0.0;
}
// preserve and clear the root link acceleration
dvoorg = rootLink_->dvo;
dworg = rootLink_->dw;
root_w_x_v = rootLink_->w.cross(rootLink_->vo + rootLink_->w.cross(rootLink_->p));
rootLink_->dvo = g - root_w_x_v; // dv = g, dw = 0
rootLink_->dw.setZero();
setColumnOfMassMatrix(b1, 0);
if( !isStaticModel_ ){
for(int i=0; i < 3; ++i){
rootLink_->dvo[i] += 1.0;
setColumnOfMassMatrix(out_M, i);
rootLink_->dvo[i] -= 1.0;
}
for(int i=0; i < 3; ++i){
rootLink_->dw[i] = 1.0;
Vector3 dw_x_p = rootLink_->dw.cross(rootLink_->p); // spatial acceleration caused by ang. acc.
rootLink_->dvo -= dw_x_p;
setColumnOfMassMatrix(out_M, i + 3);
rootLink_->dvo += dw_x_p;
rootLink_->dw[i] = 0.0;
}
}
for(uint i = 0; i < nJ; ++i){
Link* ptr = joint(i);
ptr->ddq = 1.0;
int j = i + 6;
setColumnOfMassMatrix(out_M, j);
out_M(j, j) += ptr->Jm2; // motor inertia
ptr->ddq = 0.0;
}
// subtract the constant term
for(size_t i = 0; i < (size_t)out_M.cols(); ++i){
out_M.col(i) -= b1;
}
// recover state
for(uint i = 0; i < nJ; ++i){
Link* ptr = joint(i);
ptr->ddq = ddqorg[i];
ptr->u = uorg [i];
}
rootLink_->dvo = dvoorg;
rootLink_->dw = dworg;
}
bool sffs::apply(const dmatrix& src,const ivector& srcIds,
dmatrix& dest) const {
bool ok=true;
dest.clear();
parameters param=getParameters();
// initialize cross validator
costFunction *cF;
cF = param.usedCostFunction;
cF->setSrc(src,srcIds);
int featureToInsert(0),featureToDelete(0),i;
double oldRate,newRate;
bool doInclude=true;
bool terminate=false;
int nbFeatures=src.columns();
std::list<int> in,out;
std::list<int>::iterator it;
std::map<double,int> values;
double value;
for (i=0; i<nbFeatures; i++) {
out.push_back(i);
}
ivector posInSrc(nbFeatures,-1);//saves the position in src of the inserted
// feature to mark it as not used if this feature is deleted later
dvector regRate(nbFeatures); // the recognition rates after the insertion
// of a new feature
if (param.nbFeatures<2) {
setStatusString("You will have to choose at least two features. Set nbFeatures=2");
return false;
}
// add the first best two features; do 2 steps sfs
for (i=0; i<2; i++ ) {
if (dest.columns()<src.columns() && !terminate) {
// add space for one extra feature
for (it=out.begin(); it!=out.end(); it++) {
in.push_back(*it);
cF->apply(in,value);
values[value]=*it;
in.pop_back();
}
// search for maximum in regRate; all possibilities not tested are -1
in.push_back((--values.end())->second);
out.remove((--values.end())->second);
}
}
cF->apply(in,oldRate);
while (!terminate) {
// STEP 1: include the best possible feature
if (static_cast<int>(in.size())<src.columns() &&
!terminate && doInclude) {
values.clear();
for (it=out.begin(); it!=out.end(); it++) {
in.push_back(*it);
cF->apply(in,value);
values[value]=*it;
in.pop_back();
}
featureToInsert=(--values.end())->second;
in.push_back(featureToInsert);
out.remove(featureToInsert);
}
// STEP 2: conditional exclusion
if (in.size()>0 && !terminate) {
values.clear();
for (it=in.begin(); it!=in.end(); it++) {
int tmp=*it;
it=in.erase(it);
cF->apply(in,value);
values[value]=tmp;
in.insert(it,tmp);
it--;
}
featureToDelete=(--values.end())->second;
// if the least significant feature is equal to the most significant
// feature that was included in step 1, leave feature and
// include the next one
if (featureToDelete==featureToInsert) {
doInclude=true;
} else { // delete this feature and compute new recognition rate
// if the feature to delete is not the last feature in dest,
// change the feature against the last feature in dest and delete
// the last column in dest, otherwise if the feature to delete
// is equal to the last feature in dest nothing will be done,
// because this is already the lacking feature in temp
cF->apply(in,newRate);
// if recognition rate without least significant feature is better
// than with this feature delete it
if (newRate>oldRate) {
in.remove(featureToDelete);
out.push_back(featureToDelete);
// search for another least significant feature before
// including the next one
doInclude=false;
oldRate=newRate;
} else {
doInclude=true;
}
// if only two features left, include the next one
if (dest.columns()<=2) {
doInclude=true;
}
}
} // end of exclusion
// test if the predetermined number of features is reached
terminate=(param.nbFeatures==static_cast<int>(in.size()));
} // while (!terminate)
// Now fill dest
const int sz = static_cast<int>(in.size());
dest.resize(src.rows(), sz, 0., false, false);
ivector idvec(false, sz);
std::list<int>::const_iterator lit = in.begin();
for (i=0; i < sz; ++i) {
idvec.at(i)=*lit;
++lit;
}
for (i=0; i < src.rows(); ++i) {
const dvector& svec = src.getRow(i);
dvector& dvec = dest.getRow(i);
for (int j=0; j < sz; ++j) {
dvec.at(j) = svec.at(idvec.at(j));
}
}
return ok;
};
/**
calculate Pseudo-Inverse using SVD(Singular Value Decomposition)
by lapack library DGESVD (_a can be non-square matrix)
*/
int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
{
int i, j, k;
char jobu = 'A';
char jobvt = 'A';
int m = (int)_a.rows();
int n = (int)_a.cols();
int max_mn = max(m,n);
int min_mn = min(m,n);
dmatrix a(m,n);
a = _a;
int lda = m;
double *s = new double[max_mn];
int ldu = m;
double *u = new double[ldu*m];
int ldvt = n;
double *vt = new double[ldvt*n];
int lwork = max(3*min_mn+max_mn, 5*min_mn); // for CLAPACK ver.2 & ver.3
double *work = new double[lwork];
int info;
for(i = 0; i < max_mn; i++) s[i] = 0.0;
dgesvd_(&jobu, &jobvt, &m, &n, &(a(0,0)), &lda, s, u, &ldu, vt, &ldvt, work,
&lwork, &info);
double smin, smax=0.0;
for (j = 0; j < min_mn; j++) if (s[j] > smax) smax = s[j];
smin = smax*_sv_ratio; // default _sv_ratio is 1.0e-3
for (j = 0; j < min_mn; j++) if (s[j] < smin) s[j] = 0.0;
//------------ calculate pseudo inverse pinv(A) = V*S^(-1)*U^(T)
// S^(-1)*U^(T)
for (j = 0; j < m; j++){
if (s[j]){
for (i = 0; i < m; i++) u[j*m+i] /= s[j];
}
else {
for (i = 0; i < m; i++) u[j*m+i] = 0.0;
}
}
// V * (S^(-1)*U^(T))
_a_pseu.resize(n,m);
for(j = 0; j < n; j++){
for(i = 0; i < m; i++){
_a_pseu(j,i) = 0.0;
for(k = 0; k < min_mn; k++){
if(s[k]) _a_pseu(j,i) += vt[j*n+k] * u[k*m+i];
}
}
}
delete [] work;
delete [] vt;
delete [] s;
delete [] u;
return info;
}
void Body::calcAngularMomentumJacobian(Link *base, dmatrix &H)
{
// prepare subm, submwc
JointPathPtr jp;
dmatrix M;
calcCMJacobian(base, M);
M.conservativeResize(3, numJoints());
M *= totalMass();
if (base){
jp = getJointPath(rootLink(), base);
Link *skip = jp->joint(0);
skip->subm = rootLink()->m;
skip->submwc = rootLink()->m*rootLink()->wc;
Link *l = rootLink()->child;
if (l){
if (l != skip) {
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
while(l){
if (l != skip){
l->calcSubMassCM();
skip->subm += l->subm;
skip->submwc += l->submwc;
}
l = l->sibling;
}
}
// assuming there is no branch between base and root
for (unsigned int i=1; i<jp->numJoints(); i++){
l = jp->joint(i);
l->subm = l->parent->m + l->parent->subm;
l->submwc = l->parent->m*l->parent->wc + l->parent->submwc;
}
H.resize(3, numJoints());
}else{
rootLink()->calcSubMassCM();
H.resize(3, numJoints()+6);
}
// compute Jacobian
std::vector<int> sgn(numJoints(), 1);
if (jp) {
for (unsigned int i=0; i<jp->numJoints(); i++) sgn[jp->joint(i)->jointId] = -1;
}
for (unsigned int i=0; i<numJoints(); i++){
Link *j = joint(i);
switch(j->jointType){
case Link::ROTATIONAL_JOINT:
{
Vector3 omega(sgn[j->jointId]*j->R*j->a);
Vector3 Mcol = M.col(j->jointId);
Matrix33 jsubIw;
j->calcSubMassInertia(jsubIw);
Vector3 dp = jsubIw*omega;
if (j->subm!=0) dp += (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
break;
}
case Link::SLIDE_JOINT:
{
if(j->subm!=0){
Vector3 Mcol =M.col(j->jointId);
Vector3 dp = (j->submwc/j->subm).cross(Mcol);
H.col(j->jointId) = dp;
}
break;
}
default:
std::cerr << "calcCMJacobian() : unsupported jointType("
<< j->jointType << ")" << std::endl;
}
}
if (!base){
int c = numJoints();
H.block(0, c, 3, 3).setZero();
Matrix33 Iw;
rootLink_->calcSubMassInertia(Iw);
H.block(0, c+3, 3, 3) = Iw;
Vector3 cm = calcCM();
Matrix33 cm_cross;
cm_cross <<
0.0, -cm(2), cm(1),
cm(2), 0.0, -cm(0),
-cm(1), cm(0), 0.0;
H.block(0,0,3,c) -= cm_cross * M;
}
}