void MakeJacobian(CLink *Skelet, TJacobian J) {
/* **************************************************************************************************************************** */
int i;
MDFloat ai[4],
bi[4],
v[4],
Pn[4],
Pj[4];
MDFloat tempT[4][4];
MDFloat tempZTi[4][4];
MDFloat *ptrZTi = (MDFloat*)tempZTi;
MDFloat *ptrtempT = (MDFloat*)tempT;
/* **************************************************************************************************************************** */
// Calcul de la matrice de transformation globale de l'effecteur terminal (ZTi)
LoadIdentity(ptrZTi);
for (i=0; i<_NO_OF_LINKS_; i++) {
// ..
MultMatrix4x4((MDFloat(*)[4])Skelet[i].Get_im1Ti(),tempZTi,tempT);
CopyMatrix4x4(ptrtempT,ptrZTi);
Skelet[i].Set_ZTi(tempZTi);
}
/* **************************************************************************************************************************** */
MDFloat (*ZTi)[4];
MDFloat (*ZTN)[4];
// Extraction de la position de l'effecteur terminal
// ..
ZTN = (MDFloat(*)[4])Skelet[_NO_OF_LINKS_-1].Get_ZTi();
Pn[0] = ZTN[0][3];
Pn[1] = ZTN[1][3];
Pn[2] = ZTN[2][3];
Pn[3] = 1.0;
// Calcul de la Jacobienne (3 lignes)
for (i=0; i<_NO_OF_LINKS_; i++) {
// Caclul du vecteur reliant l'effecteur terminal aux différents repères (locaux) des segments du squelette : v = Pn - Pj
// ..
ZTi = (MDFloat(*)[4])Skelet[i].Get_ZTi();
Pj[0] = ZTi[0][3];
Pj[0] = ZTi[1][3];
Pj[0] = ZTi[2][3];
Pj[0] = 1.0;
VectSub(Pn,Pj,v);
// Extraction du vecteur de rotation suivant l'axe oX (1ere colonne)
// ..
ai[0] = ZTi[0][0];
ai[1] = ZTi[1][0];
ai[2] = ZTi[2][0];
ai[3] = 1.0;
// Calcul du vacteur orthogonal au vecteur v et au vecteur de rotation suivant l'axe oX : bi = ai X v
// ..
VectProd(ai,v,bi);
// Calcul de la 1ere colonne de la Jacobienne (DDL : oX) : J[0][x]t = [bi[0],bi[1],bi[2],ai[0],ai[1],ai[2]]
// ..
J[i*3][0] = bi[0];
J[i*3][1] = bi[1];
J[i*3][2] = bi[2];
J[i*3][3] = ai[0];
J[i*3][4] = ai[1];
J[i*3][5] = ai[2];
// Extraction du vecteur de rotation suivant l'axe oY (2eme colonne)
// ..
ai[0] = ZTi[0][1];
ai[1] = ZTi[1][1];
ai[2] = ZTi[2][1];
ai[3] = 1.0;
// Calcul du vacteur orthogonal au vecteur v et au vecteur de rotation suivant l'axe oY : bi = ai X v
// ..
VectProd(ai,v,bi);
// Calcul de la 2eme colonne de la Jacobienne (DDL : oY) : J[1][x]t = [bi[0],bi[1],bi[2],ai[0],ai[1],ai[2]]
// ..
J[i*3+1][0] = bi[0];
J[i*3+1][1] = bi[1];
J[i*3+1][2] = bi[2];
J[i*3+1][3] = ai[0];
J[i*3+1][4] = ai[1];
J[i*3+1][5] = ai[2];
// Extraction du vecteur de rotation suivant l'axe oZ (3eme colonne)
// ..
ai[0] = ZTi[0][2];
ai[1] = ZTi[1][2];
ai[2] = ZTi[2][2];
ai[3] = 1.0;
// Calcul du vacteur orthogonal au vecteur v et au vecteur de rotation suivant l'axe oZ : bi = ai X v
// ..
VectProd(ai,v,bi);
// Calcul de la 3eme colonne de la Jacobienne (DDL : oZ) : J[2][x]t = [bi[0],bi[1],bi[2],ai[0],ai[1],ai[2]]
// ..
J[i*3+2][0] = bi[0];
J[i*3+2][1] = bi[1];
J[i*3+2][2] = bi[2];
J[i*3+2][3] = ai[0];
J[i*3+2][4] = ai[1];
J[i*3+2][5] = ai[2];
} FILE *f ;
f = fopen ( "test.jac", "w" ) ;PrintJacobian(f,J);
/* **************************************************************************************************************************** */
}
// Vissage MatTrans2screw(Mat33 rotmatrix, Coord3D t0, Coord3D t1)
Screw MatTrans2screw(const Matrix& mat)
{
Coord3D trans;
Mat33 rotmatrix;
trans.x = mat(0,3);
trans.y = mat(1,3);
trans.z = mat(2,3);
// Coord3D trans = t0-t1;
for(int i=0; i<3; i++)
for(int j=0; j<3; j++)
rotmatrix[i][j]=mat(i,j);
// std::cout << trans.toString();
Screw screw;
Coord3D eigenvect;
Coord3D x,y,z;
x.x = rotmatrix[0][0] ; x.y = rotmatrix[1][0]; x.z=rotmatrix[2][0];
y.x = rotmatrix[0][1] ; y.y = rotmatrix[1][1]; y.z=rotmatrix[2][1];
z.x = rotmatrix[0][2] ; z.y = rotmatrix[1][2]; z.z=rotmatrix[2][2];
Coord3D pt ; //un point de l'axe de rotation
double a = rotmatrix[0][0] ; // Xx
double b = rotmatrix[1][1] ; // Yy
double c = rotmatrix[2][2] ; // Zz
if (fabs(1+a-b-c) > EPSILON)
{
eigenvect.x = x.x + 1 - b - c ;
eigenvect.y = x.y + y.x ;
eigenvect.z = x.z + z.x ;
screw.unitVector = eigenvect / Norm(eigenvect);
screw.normtranslation = ScalProd(screw.unitVector,trans);
Coord3D s = trans - screw.normtranslation*screw.unitVector ;
screw.point.x = 0 ;
screw.point.y = s.z*z.y + s.y*(1-z.z) ;
screw.point.z = s.y*y.z+s.z*(1-y.y);
screw.point = screw.point/(1+x.x-y.y-z.z) ;
}
else if (fabs(1-a+b-c)> EPSILON)
{
eigenvect.x = y.x + x.y;
eigenvect.y = y.y + 1 - x.x - z.z ;
eigenvect.z = y.z + z.y ;
screw.unitVector = eigenvect / Norm(eigenvect);
screw.normtranslation = ScalProd(screw.unitVector,trans);
Coord3D s = trans - screw.normtranslation*screw.unitVector ;
screw.point.x = s.z*z.x + s.x*(1-z.z);
screw.point.y = 0 ;
screw.point.z = s.x*x.z + s.z*(1-x.x);
screw.point = screw.point/(1-x.x+y.y-z.z) ;
}
else if (fabs(1-a-b+c)>EPSILON)
{
eigenvect.x = z.x + x.z ;
eigenvect.y = z.y + y.z ;
eigenvect.z = z.z + 1 - x.x - y.y ;
screw.unitVector = eigenvect / Norm(eigenvect);
screw.normtranslation = ScalProd(screw.unitVector,trans);
Coord3D s = trans - screw.normtranslation*screw.unitVector ;
screw.point.x = s.y*y.x + s.x*(1-y.y) ;
screw.point.y = s.x*x.y + s.y*(1-x.x) ;
screw.point.z = 0 ;
screw.point = screw.point/(1-x.x-y.y+z.z) ;
}
else // angle=0
{
screw.point = Coord3D(0,0,0);
if(Norm(trans)!=0)
{
screw.unitVector=trans /Norm(trans);
}
else { screw.unitVector = Coord3D(0,0,1); /*axe arbitraire*/ }
screw.normtranslation=Norm(trans);
screw.angle=0;
return screw;
}
//creation d'un vecteur non aligne avec screw.unitVector:
Coord3D v (1,0,0);
if (fabs(Angle(screw.unitVector,v)) < 0.1) v= Coord3D(0,0,1); //v et axe colin�aires: on change v
Coord3D u = v - ScalProd(v,screw.unitVector)*screw.unitVector ;
u = u/Norm(u);
Coord3D uprime;
Mat33xcoord3D(rotmatrix,u,uprime);
double cost = ScalProd(u,uprime);
Coord3D usec;
VectProd(screw.unitVector,u,usec);
double sint = ScalProd(usec,uprime);
if (cost < -1 ) cost=-1;
if (cost >1 ) cost= 1 ;
screw.angle = acos(cost);
if (sint < 0) screw.angle = -screw.angle ;
screw.angle *= -1;
return screw ;
}
