/*!
Return the position of each axis.
- In positionning control mode, call vpRobotBiclopsController::getPosition()
- In speed control mode, call vpRobotBiclopsController::getActualPosition()
\param frame : Control frame. This biclops head can only be controlled in
articular.
\param q : The position of the axis in radians.
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
void
vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame,
vpColVector & q)
{
switch(frame)
{
case vpRobot::CAMERA_FRAME :
vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get position in camera frame: "
"not implemented");
break;
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot get position in reference frame: "
"not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get position in reference frame: "
"not implemented");
break;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot get position in mixt frame: "
"not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get position in mixt frame: "
"not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
break ;
}
vpRobot::vpRobotStateType state;
state = vpRobot::getRobotState();
switch (state) {
case STATE_STOP:
case STATE_POSITION_CONTROL:
q = controller.getPosition();
break;
case STATE_VELOCITY_CONTROL:
case STATE_ACCELERATION_CONTROL:
default:
q.resize(vpBiclops::ndof);
vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
q[i] = shm.actual_q[i];
}
vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
break;
}
}
static
void
lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
{
if (DEBUG_LEVEL1)
std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl;
try{
int i,imin;
vpMatrix ata ; // A^T A
ata = a.t()*a ;
vpMatrix btb ; // B^T B
btb = b.t()*b ;
vpMatrix bta ; // B^T A
bta = b.t()*a ;
vpMatrix btb1 ; // (B^T B)^(-1)
if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
else btb1 = btb.pseudoInverse();
if (DEBUG_LEVEL1)
{
std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
}
vpMatrix r ; // (B^T B)^(-1) B^T A
r = btb1*bta ;
vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
e = - (a.t()*b) *r ;
e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
if (DEBUG_LEVEL1)
{
std::cout << " E :" << std::endl << e << std::endl;
}
// vpColVector sv ;
// vpMatrix v ;
e.svd(x1,ata) ;// destructif sur e
// calcul du vecteur propre de E correspondant a la valeur propre min.
/* calcul de SVmax */
imin = 0;
// FC : Pourquoi calculer SVmax ??????
// double svm = 0.0;
// for (i=0;i<x1.getRows();i++)
// {
// if (x1[i] > svm) { svm = x1[i]; imin = i; }
// }
// svm *= EPS; /* pour le rang */
for (i=0;i<x1.getRows();i++)
if (x1[i] < x1[imin]) imin = i;
if (DEBUG_LEVEL1)
{
printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
std::cout << " i_min " << imin << std::endl;
}
for (i=0;i<x1.getRows();i++)
x1[i] = ata[i][imin];
x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
if (DEBUG_LEVEL1)
{
std::cout << " X1 : " << x1.t() << std::endl;
std::cout << " V : " << std::endl << ata << std::endl;
}
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
if (DEBUG_LEVEL1)
std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
}
/*!
Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
Ax = b
\param A : Matrix A from Ax = b.
\param x : Vector x from Ax = b corresponding to the parameters to estimate.
\param b : Vector b from Ax = b.
*/
vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
{
double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) );
return (A.t()*A).pseudoInverse()*sigma2;
}
