void FindConfiguration () {
/* **************************************************************************************************************************** */
MDFloat Err;
MDFloat ErrVectP[4];
MDFloat ErrVectO[4];
int NoOfIter;
/* **************************************************************************************************************************** */
NoOfIter = 0;
/* **************************************************************************************************************************** */
// Calcul de l'erreur
// ..
VectSub(PNew, PnIter , ErrVectP);
VectSub(ONew, OnIter, ErrVectO);
Err = sqrt ( ScalProd(ErrVectP,ErrVectP) + ScalProd(ErrVectO,ErrVectO) );
while ( (Err > _OUTER_EPSILON_) && (NoOfIter<_MAX_OUTER_ITER_) ) {
// ..
InnerIterate();
VectSub(PNew, PnIter , ErrVectP);
VectSub(ONew, OnIter, ErrVectO);
Err = sqrt ( ScalProd(ErrVectP,ErrVectP) + ScalProd(ErrVectO,ErrVectO) );
NoOfIter++;
}
/* **************************************************************************************************************************** */
#ifdef _DEBUG
fprintf(stderr, "OUTER loop: %d\n", NoOfIter);
#endif
/* **************************************************************************************************************************** */
}
void InnerIterate() {
/* **************************************************************************************************************************** */
int i;
int NoOfIter;
MDFloat dX[_JAC_N_]; // Vecteur d'entrée (position & orientation désirées)
MDFloat dTheta[_JAC_M_]; // Vecteur de sortie (DDL : oX1,oY1,oZ1,oX2,oY2,oZ2,etc.)
MDFloat (*ZTN)[4];
MDFloat TempJt[_JAC_N_][_JAC_M_];
MDFloat JdThetaP[4]; // Vecteurs d'erreur
MDFloat JdThetaO[4];
MDFloat ErrVectP[4];
MDFloat ErrVectO[4];
MDFloat Pn[4]; // Position actuelle de l'effecteur
MDFloat dXPn[4]; // Variation désirée en position : dXPn = PNew - Pn
MDFloat On[4]; // Orientation actuelle de l'effecteur
MDFloat dXOn[4]; // Variation désirée en orientation : dXOn = ONew - On
MDFloat Err; // Erreur
CLink BackUpSkelet[_NO_OF_LINKS_];
/* **************************************************************************************************************************** */
NoOfIter = 0;
// Sauvegarde du squelette **
memcpy(BackUpSkelet, Skelet, _NO_OF_LINKS_*sizeof(CLink));
/* **************************************************************************************************************************** */
/////////////////////////////
// Calcul du vecteur d'entrée
/////////////////////////////
// Intilisation du vecteur d'entrée
// ..
for(i = 0; i<_JAC_N_;i++)dX[i] = 0.0;
// Calcul de la variation de position "dXPn" du vecteur d'entrée "dX"
// ..
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;
VectSub(PNew,Pn,dXPn);
// Calcul de la variation en orientation "dXOn" du vecteur d'entrée "dX"
// ..
GetEulerAngles(ZTN, On);
On[3] = 1.0;
VectSub(ONew, On, dXOn);
dX[0] = dXPn[0]*0.5;
dX[1] = dXPn[1]*0.5;
dX[2] = dXPn[2]*0.5;
dX[3] = dXOn[0]*0.5;
dX[4] = dXOn[1]*0.5;
dX[5] = dXOn[2]*0.5;
/* **************************************************************************************************************************** */
///////////////////////////////////////
// Calcul du vecteur de sortie "dTheta"
///////////////////////////////////////
do { // Améliore la convergence de l'algorithme **
memcpy(Skelet, BackUpSkelet, _NO_OF_LINKS_*sizeof(CLink));
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul de la Jacobienne
// ..
MakeJacobian(Skelet,J);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul de la Jacobienne transposée
// ..
TransposeJacobian(J, Jt);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Incorporation d'une raideur à la Jacobienne : poids forts aux articulations éloignée de l'effecteur.
// ..
memcpy(TempJt, Jt, _JAC_N_*_JAC_M_*sizeof(MDFloat));
MultKJacobianT(K , TempJt , Jt);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Calcul du vecteur de sortie : dTheta = Jt * dX
// ..
MultJacobianT(Jt, dX, dTheta );
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Ajout de dTheta aux angles et mise-à-jour des matrices de transformation locales "m1Ti" du squelette
// ..
for (i=0; i<_NO_OF_LINKS_; i++)
Skelet[i].Updateim1Ti(dTheta[i*3+0], dTheta[i*3+1], dTheta[i*3+2]);
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// Estimation de l'erreur : Err² = (ErrVectP)² + (ErrVectO)²
// ..
UpdateZTiMatrices(Skelet);
GetEEPosition(Skelet, PnIter, OnIter);
VectSub(PnIter , Pn , JdThetaP);
VectSub(JdThetaP, dX , ErrVectP);
VectSub(OnIter , On , JdThetaO);
VectSub(JdThetaO, &dX[3], ErrVectO);
Err = sqrt ( ScalProd(ErrVectP,ErrVectP) + ScalProd(ErrVectO,ErrVectO) );
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
//Calcul du nouveau vecteur d'entrée : division par deux pour fluidifier le mouvement **
// ..
dX[0] *=0.5;
dX[1] *=0.5;
dX[2] *=0.5;
dX[3] *=0.5;
dX[4] *=0.5;
dX[5] *=0.5;
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
NoOfIter++;
} while ( (Err > _INNER_EPSILON_) && (NoOfIter < _MAX_INNER_ITER_) );
#ifdef _DEBUG
fprintf(stderr, "INNER loop: %d\n", NoOfIter);
#endif
/* **************************************************************************************************************************** */
}
// 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 ;
}
