vpRobotBiclops::~vpRobotBiclops (void)
{
vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
setRobotState(vpRobot::STATE_STOP) ;
vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
/* wait the end of the control thread */
int code;
vpDEBUG_TRACE (12, "Wait end of control thread");
if (controlThreadCreated == true) {
code = pthread_join(control_thread, NULL);
if (code != 0) {
vpCERROR << "Cannot terminate the control thread: " << code
<< " strErr=" << strerror(errno)
<< " strCode=" << strerror(code)
<< std::endl;
}
}
pthread_mutex_destroy (&vpShm_mutex);
pthread_mutex_destroy (&vpEndThread_mutex);
pthread_mutex_destroy (&vpMeasure_mutex);
vpRobotBiclops::robotAlreadyCreated = false;
vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
return;
}
/*!
Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
\f[\lambda(x) = a * exp(-b*x) + c\f]
\param x : Input value to consider. During a visual servo this value can be the euclidian
norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
\return It returns the value of the computed gain.
*/
double vpAdaptiveGain::value_const (const double x) const
{
vpDEBUG_TRACE (10, "# Entree.");
double res = this ->coeff_a * exp (- this ->coeff_b * x) + this ->coeff_c;
vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
/*!
Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
\f[\lambda(x) = a * exp(-b*x) + c\f]
This value is stored as a parameter of the class.
\param x : Input value to consider. During a visual servo this value can be the euclidian
norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
\return It returns the value of the computed gain.
*/
double vpAdaptiveGain::value (const double x) const
{
vpDEBUG_TRACE (10, "# Entree.");
this ->lambda = this ->value_const (x);
vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
return lambda;
}
/*!
Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$) and stores it
as a parameter of the class.
\return It returns the value of the gain at infinity.
*/
double vpAdaptiveGain:: limitValue (void) const
{
vpDEBUG_TRACE (10, "# Entree.");
this ->lambda = this ->limitValue_const ();
vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
return lambda;
}
/*!
Gets the value of the gain at infinity (ie the value of \f$ \lambda(\infty) = c \f$).
Similar to limitValue() except that here the value is not stored as a parameter of the class.
\return It returns the value of the gain at infinity.
*/
double vpAdaptiveGain::limitValue_const (void) const
{
vpDEBUG_TRACE (10, "# Entree.");
double res = this ->coeff_c;
vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
/*!
Change the state of the robot either to stop them, or to set position or
speed control.
*/
vpRobot::vpRobotStateType
vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
{
switch (newState)
{
case vpRobot::STATE_STOP:
{
if (vpRobot::STATE_STOP != getRobotState ())
{
stopMotion();
}
break;
}
case vpRobot::STATE_POSITION_CONTROL:
{
if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
{
vpDEBUG_TRACE (12, "Speed to position control.");
stopMotion();
}
break;
}
case vpRobot::STATE_VELOCITY_CONTROL:
{
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
{
vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
pthread_mutex_lock(&vpEndThread_mutex);
vpDEBUG_TRACE (12, "Create speed control thread");
int code;
code = pthread_create(&control_thread, NULL,
&vpRobotBiclops::vpRobotBiclopsSpeedControlLoop,
&controller);
if (code != 0) {
vpCERROR << "Cannot create speed biclops control thread: " << code
<< " strErr=" << strerror(errno)
<< " strCode=" << strerror(code)
<< std::endl;
}
controlThreadCreated = true;
vpDEBUG_TRACE (12, "Speed control thread created");
}
break;
}
default:
break ;
}
return vpRobot::setRobotState (newState);
}
/*!
Move the robot in position control.
\warning This method is blocking. That mean that it waits the end of the
positionning.
\param frame : Control frame. This biclops head can only be controlled in
articular.
\param q : The position to set for each axis in radians.
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
void
vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
const vpColVector & q )
{
if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
{
vpERROR_TRACE ("Robot was not in position-based control\n"
"Modification of the robot state");
setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
}
switch(frame)
{
case vpRobot::CAMERA_FRAME:
vpERROR_TRACE ("Cannot move the robot in camera frame: "
"not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot move the robot in camera frame: "
"not implemented");
break;
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot move the robot in reference frame: "
"not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot move the robot in reference frame: "
"not implemented");
break;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot move the robot in mixt frame: "
"not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot move the robot in mixt frame: "
"not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
break ;
}
// test if position reachable
// if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
// (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
// vpERROR_TRACE ("Positionning error.");
// throw vpRobotException (vpRobotException::wrongStateError,
// "Positionning error.");
// }
vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
pthread_mutex_lock(&vpEndThread_mutex);
controller.setPosition( q, positioningVelocity );
vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
return ;
}
/*!
Initializes the parameters with the default value :
- \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
*/
void vpAdaptiveGain::initFromVoid (void)
{
vpDEBUG_TRACE (10, "# Entree.");
this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);
vpDEBUG_TRACE (10, "# Sortie.");
return;
}
/*!
Initializes the parameters to have a constant gain. In that case \f$\lambda(x) = c\f$.
\param c : Value of the constant gain.
*/
void vpAdaptiveGain::initFromConstant (const double c)
{
vpDEBUG_TRACE (10, "# Entree.");
this ->coeff_a = 0;
this ->coeff_b = 1;
this ->coeff_c = c;
vpDEBUG_TRACE (10, "# Sortie.");
return;
}
SoSeparator *
createFrame (float longueurFleche = LONGUEUR_FLECHE ,
float proportionFleche = PROPORTION_FLECHE,
float radiusFleche = RAYON_FLECHE)
{
vpDEBUG_TRACE (15, "# Entree.");
SoSeparator *frame = new SoSeparator;
frame-> ref ();
SoRotationXYZ *rotationY_X = new SoRotationXYZ;
rotationY_X->axis = SoRotationXYZ::Z;
rotationY_X->angle.setValue ((float)(- M_PI / 2));
SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
rotationX_Y->axis = SoRotationXYZ::Z;
rotationX_Y->angle.setValue ((float)(M_PI / 2));
SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
rotationY_Z->axis = SoRotationXYZ::X;
rotationY_Z->angle.setValue ((float)(M_PI / 2));
SoMaterial *rouge = new SoMaterial;
rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
rouge->emissiveColor.setValue(0.5, 0.0, 0.0);
SoMaterial *vert = new SoMaterial;
vert->diffuseColor.setValue(0.0, 1.0, 0.0);
vert->emissiveColor.setValue(0.0, 0.5, 0.0);
SoMaterial *bleu = new SoMaterial;
bleu->diffuseColor.setValue(0.0, 0.0, 1.0);
bleu->emissiveColor.setValue(0.0, 0.0, 0.5);
SoSeparator *fleche = createArrow(longueurFleche,
proportionFleche,
radiusFleche);
frame->addChild (rouge);
frame->addChild (rotationY_X);
frame->addChild (fleche);
frame->addChild (vert);
frame->addChild (rotationX_Y);
frame->addChild (fleche);
frame->addChild (bleu);
frame->addChild (rotationY_Z);
frame->addChild (fleche);
frame-> unrefNoDelete ();
vpDEBUG_TRACE (15, "# Sortie.");
return frame;
}
/*!
Basic constructor which initializes all the parameters with their default value:
- \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
*/
vpAdaptiveGain::vpAdaptiveGain ()
:
coeff_a (),
coeff_b (),
coeff_c (),
lambda(1.)
{
vpDEBUG_TRACE (10, "# Entree constructeur par default.");
this ->initFromVoid ();
vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
return;
}
/*!
Sets the internal parameters \f$a,b,c\f$ in order to obtain a constant gain equal to
the gain in 0 set through the parameter \f$\lambda(0)\f$.
\return It returns the value of the constant gain \f$\lambda(0)\f$.
*/
double vpAdaptiveGain::setConstant (void)
{
vpDEBUG_TRACE (10, "# Entree.");
double res = this ->coeff_a + this ->coeff_c;
this ->coeff_a = 0;
this ->coeff_b = 1;
this ->coeff_c = res;
vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
/*!
Default constructor.
Initialize the ptu-46 pan, tilt head by opening the serial port.
\sa init()
*/
vpRobotPtu46::vpRobotPtu46 (const char *device)
:
vpRobot ()
{
this->device = new char [FILENAME_MAX];
sprintf(this->device, "%s", device);
vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
try
{
init();
}
catch(...)
{
delete [] this->device;
vpERROR_TRACE("Error caught") ;
throw ;
}
try
{
setRobotState(vpRobot::STATE_STOP) ;
}
catch(...)
{
delete [] this->device;
vpERROR_TRACE("Error caught") ;
throw ;
}
positioningVelocity = defaultPositioningVelocity ;
return ;
}
/*!
Set the number of the image to be read.
*/
void
vpDiskGrabber::setImageNumber(long number)
{
image_number = number ;
vpDEBUG_TRACE(2, "image number %ld", image_number);
}
SoSeparator *
createCameraObject (const float zoomFactor = 1.0)
{
vpDEBUG_TRACE (15, "# Entree.");
SoSeparator * cam = new SoSeparator;
cam->ref ();
SoMaterial *myMaterial = new SoMaterial;
myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
myMaterial->emissiveColor.setValue(0.5, 0.0, 0.0);
SoScale *taille = new SoScale;
{
float zoom = 0.1f * zoomFactor;
taille->scaleFactor.setValue (zoom, zoom, zoom);
}
SoMaterial *couleurBlanc = new SoMaterial;
couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
SoDrawStyle * filDeFer = new SoDrawStyle;
filDeFer->style.setValue (SoDrawStyle::LINES);
filDeFer->lineWidth.setValue (1);
SoSeparator * cone = new SoSeparator;
cone->ref();
cone->addChild (makePyramide());
cone->addChild (couleurBlanc);
cone->addChild (filDeFer);
cone->addChild (makePyramide());
cone->unrefNoDelete();
cam->addChild(myMaterial);
cam->addChild(taille);
cam->addChild(cone);
cam->addChild(createFrame(2.0f,0.1f,0.01f));
// cam->unref() ;
vpDEBUG_TRACE (15, "# Sortie.");
return cam;
}
/*!
Change the state of the robot either to stop them, or to set position or
speed control.
*/
vpRobot::vpRobotStateType
vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
{
switch (newState)
{
case vpRobot::STATE_STOP:
{
if (vpRobot::STATE_STOP != getRobotState ())
{
ptu.stop();
}
break;
}
case vpRobot::STATE_POSITION_CONTROL:
{
if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
{
vpDEBUG_TRACE (12, "Passage vitesse -> position.");
ptu.stop();
}
else
{
vpDEBUG_TRACE (1, "Passage arret -> position.");
}
break;
}
case vpRobot::STATE_VELOCITY_CONTROL:
{
if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
{
vpDEBUG_TRACE (10, "Arret du robot...");
ptu.stop();
}
break;
}
default:
break ;
}
return vpRobot::setRobotState (newState);
}
/*!
Set the parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ used to compute \f$\lambda(x)\f$.
\param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
\param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
\param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
*/
void vpAdaptiveGain::initStandard (const double gain_at_zero,
const double gain_at_infinity,
const double slope_at_zero)
{
vpDEBUG_TRACE (10, "# Entree.");
this ->coeff_a = gain_at_zero - gain_at_infinity;
//if (0 == this ->coeff_a)
if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
{
this ->coeff_b = 0;
}
else
{
this ->coeff_b = slope_at_zero / ( this ->coeff_a);
}
this ->coeff_c = gain_at_infinity;
vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
this ->coeff_a, this ->coeff_b, this ->coeff_c);
return;
}
/*!
Open the serial port.
\exception vpRobotException::constructionError : If the device cannot be
oppened.
*/
void
vpRobotPtu46::init ()
{
vpDEBUG_TRACE (12, "Open connection Ptu-46.");
if (0 != ptu.init(device) )
{
vpERROR_TRACE ("Cannot open connexion with ptu-46.");
throw vpRobotException (vpRobotException::constructionError,
"Cannot open connexion with ptu-46");
}
return ;
}
/*!
Default constructor.
Does nothing more than setting the default configuration file
to /usr/share/BiclopsDefault.cfg.
As shown in the following example,the turret need to be initialized
using init() function.
\code
#include <visp/vpConfig.h>
#include <visp/vpRobotBiclops.h>
int main()
{
#ifdef VISP_HAVE_BICLOPS
vpRobotBiclops robot; // Use the default config file in /usr/share/BiclopsDefault.cfg"
// Specify the config file location
robot.setConfigFile("/usr/share/BiclopsDefault.cfg"); // Not mandatory since the file is the default one
// Initialize the head
robot.init();
// Move the robot to a specified pan and tilt
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
vpColVector q(2);
q[0] = vpMath::rad(20); // pan
q[1] = vpMath::rad(40); // tilt
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
#endif
return 0;
}
\endcode
*/
vpRobotBiclops::vpRobotBiclops ()
:
vpRobot ()
{
vpDEBUG_TRACE (12, "Begin default constructor.");
vpRobotBiclops::robotAlreadyCreated = false;
controlThreadCreated = false;
setConfigFile("/usr/share/BiclopsDefault.cfg");
// Initialize the mutex dedicated to she shm protection
pthread_mutex_init (&vpShm_mutex, NULL);
pthread_mutex_init (&vpEndThread_mutex, NULL);
pthread_mutex_init (&vpMeasure_mutex, NULL);
}
void
vpDiskGrabber::open(vpImage<unsigned char> &I)
{
long first_number = getImageNumber();
vpDEBUG_TRACE(2, "first %ld", first_number);
acquire(I);
setImageNumber(first_number);
width = I.getWidth();
height = I.getHeight();
init = true;
}
/*!
Read the fist image of the sequence.
The image number is not incremented.
*/
void
vpDiskGrabber::open(vpImage<float> &I)
{
// First we save the image number, so that it can be reaffected after the
// acquisition. That means that the first image is readed twice
long first_number = getImageNumber();
vpDEBUG_TRACE(2, "first %ld", first_number);
acquire(I);
setImageNumber(first_number);
width = I.getWidth();
height = I.getHeight();
init = true;
}
/*!
Return the position of each axis.
\param frame : Control frame. This head can only be controlled in
articular.
\param q : The position of the axis.
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
void
vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
vpColVector & q)
{
vpDEBUG_TRACE (9, "# Entree.");
switch(frame)
{
case vpRobot::CAMERA_FRAME :
vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
throw vpRobotException (vpRobotException::lowLevelError,
"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::lowLevelError,
"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::lowLevelError,
"Cannot get position in mixt frame: "
"not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
break ;
}
double artpos[2];
if (0 != ptu.getCurrentPosition( artpos ) )
{
vpERROR_TRACE ("Error when calling recup_posit_Afma4.");
throw vpRobotException (vpRobotException::lowLevelError,
"Error when calling recup_posit_Afma4.");
}
q.resize (vpPtu46::ndof);
q[0] = artpos[0];
q[1] = artpos[1];
}
/*!
Acquire an image: read a pgm image from the disk.
After this call, the image number is incremented considering the step.
\param I : The image read from a file.
\param img_number : The number of the desired image.
*/
void
vpDiskGrabber::acquire(vpImage<unsigned char> &I, long img_number)
{
char name[FILENAME_MAX] ;
if(useGenericName)
sprintf(name,genericName,img_number) ;
else
sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
vpDEBUG_TRACE(2, "load: %s\n", name);
vpImageIo::read(I, name) ;
width = I.getWidth();
height = I.getHeight();
}
/*!
Create a new directory.
\param dirname : Directory to create. The directory name
is converted to the current system's format; see path().
\exception vpIoException::invalidDirectoryName : The \e dirname is invalid.
\exception vpIoException::cantCreateDirectory : If the directory cannot be
created.
\sa makeDirectory(const std::string &)
*/
void
vpIoTools::makeDirectory(const char *dirname )
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
struct stat stbuf;
#elif defined(_WIN32)
struct _stat stbuf;
#endif
if ( dirname == NULL || dirname[0] == '\0' ) {
vpERROR_TRACE( "invalid directory name\n");
throw(vpIoException(vpIoException::invalidDirectoryName,
"invalid directory name")) ;
}
std::string _dirname = path(dirname);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
if ( stat( _dirname.c_str(), &stbuf ) != 0 )
#elif defined(_WIN32)
if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
#endif
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
if ( mkdir( _dirname.c_str(), (mode_t)0755 ) != 0 )
#elif defined(_WIN32)
if ( _mkdir( _dirname.c_str()) != 0 )
#endif
{
vpERROR_TRACE("unable to create directory '%s'\n", dirname );
throw(vpIoException(vpIoException::cantCreateDirectory,
"unable to create directory")) ;
}
vpDEBUG_TRACE(2,"has created directory '%s'\n", dirname );
}
if ( checkDirectory( dirname ) == false) {
vpERROR_TRACE("unable to create directory '%s'\n", dirname );
throw(vpIoException(vpIoException::cantCreateDirectory,
"unable to create directory")) ;
}
}
/*!
Default constructor.
Initialize the biclops pan, tilt head by reading the
configuration file provided by Traclabs
and do the homing sequence.
The following example shows how to use the constructor.
\code
#include <visp/vpConfig.h>
#include <visp/vpRobotBiclops.h>
int main()
{
#ifdef VISP_HAVE_BICLOPS
// Specify the config file location and initialize the turret
vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg");
// Move the robot to a specified pan and tilt
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
vpColVector q(2);
q[0] = vpMath::rad(-20); // pan
q[1] = vpMath::rad(10); // tilt
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
#endif
return 0;
}
\endcode
*/
vpRobotBiclops::vpRobotBiclops (const char * filename)
:
vpRobot ()
{
vpDEBUG_TRACE (12, "Begin default constructor.");
vpRobotBiclops::robotAlreadyCreated = false;
controlThreadCreated = false;
setConfigFile(filename);
// Initialize the mutex dedicated to she shm protection
pthread_mutex_init (&vpShm_mutex, NULL);
pthread_mutex_init (&vpEndThread_mutex, NULL);
pthread_mutex_init (&vpMeasure_mutex
, NULL);
init();
return ;
}
/*!
\brief test the coplanarity of the set of points
\return true if points are coplanar
false if not
*/
bool
vpPose::coplanar()
{
if (npt <2)
{
vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
throw(vpPoseException(vpPoseException::notEnoughPointError,
"Not enough points ")) ;
}
if (npt ==3) return true ;
double x1,x2,x3,y1,y2,y3,z1,z2,z3 ;
std::list<vpPoint>::const_iterator it = listP.begin();
vpPoint P1,P2, P3 ;
P1 = *it; ++it;
//if ((P1.get_oX() ==0) && (P1.get_oY() ==0) && (P1.get_oZ() ==0))
if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
&& (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
&& (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon()))
{
P1 = *it; ++it;
}
P2 = *it; ++it;
P3 = *it;
x1 = P1.get_oX() ;
x2 = P2.get_oX() ;
x3 = P3.get_oX() ;
y1 = P1.get_oY() ;
y2 = P2.get_oY() ;
y3 = P3.get_oY() ;
z1 = P1.get_oZ() ;
z2 = P2.get_oZ() ;
z3 = P3.get_oZ() ;
double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
double dist;
for(it=listP.begin(); it != listP.end(); ++it)
{
P1 = *it ;
dist = (a*P1.get_oX() + b*P1.get_oY()+c*P1.get_oZ()+d)/D ;
if (fabs(dist) > distanceToPlaneForCoplanarityTest)
{
vpDEBUG_TRACE(10," points are not coplanar ") ;
// TRACE(" points are not coplanar ") ;
return false ;
}
}
vpDEBUG_TRACE(10," points are coplanar ") ;
// vpTRACE(" points are coplanar ") ;
return true ;
}
static void
computeInteractionMatrixFromList (/*const*/ vpList<vpBasicFeature *> & featureList,
/*const*/ vpList<int> & featureSelectionList,
vpMatrix & L)
{
if (featureList.empty())
{
vpERROR_TRACE("feature list empty, cannot compute Ls") ;
throw(vpServoException(vpServoException::noFeatureError,
"feature list empty, cannot compute Ls")) ;
}
/* The matrix dimension is not known before the affectation loop.
* It thus should be allocated on the flight, in the loop.
* The first assumption is that the size has not changed. A double
* reallocation (realloc(dim*2)) is done if necessary. In particulary,
* [log_2(dim)+1] reallocations are done for the first matrix computation.
* If the allocated size is too large, a correction is done after the loop.
* The algotithmic cost is linear in affectation, logarthmic in allocation
* numbers and linear in allocation size.
*/
/* First assumption: matrix dimensions have not changed. If 0, they are
* initialized to dim 1.*/
int rowL = L .getRows();
const int colL = 6;
if (0 == rowL) { rowL = 1; L .resize(rowL, colL);}
/* vectTmp is used to store the return values of functions get_s() and
* error(). */
vpMatrix matrixTmp;
int rowMatrixTmp, colMatrixTmp;
/* The cursor are the number of the next case of the vector array to
* be affected. A memory reallocation should be done when cursor
* is out of the vector-array range.*/
int cursorL = 0;
for (featureList.front() ,featureSelectionList.front() ;
!featureList.outside();
featureSelectionList.next(),featureList.next() )
{
vpBasicFeature * sPTR = featureList.value() ;
const int select = featureSelectionList.value() ;
/* Get s. */
matrixTmp = sPTR->interaction(select);
rowMatrixTmp = matrixTmp .getRows();
colMatrixTmp = matrixTmp .getCols();
/* Check the matrix L size, and realloc if needed. */
while (rowMatrixTmp + cursorL > rowL)
{ rowL *= 2; L .resize (rowL,colL,false); vpDEBUG_TRACE(15,"Realloc!"); }
/* Copy the temporarily matrix into L. */
for (int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
for (int j = 0; j < colMatrixTmp; ++j)
{ L[cursorL][j] = matrixTmp[k][j]; }
}
L.resize (cursorL,colL,false);
return ;
}
/*!
Compute the error \f$(s - s^*)\f$ between the current set of visual
features \f$s\f$ and the desired set of visual features \f$s^*\f$.
\return The error \f$(s - s^*)\f$. To access to the computed error
you can also use the public vpServo::error class member.
*/
vpColVector
vpServo::computeError()
{
if (featureList.empty())
{
vpERROR_TRACE("feature list empty, cannot compute Ls") ;
throw(vpServoException(vpServoException::noFeatureError,
"feature list empty, cannot compute Ls")) ;
}
if (desiredFeatureList.empty())
{
vpERROR_TRACE("feature list empty, cannot compute Ls") ;
throw(vpServoException(vpServoException::noFeatureError,
"feature list empty, cannot compute Ls")) ;
}
try {
vpBasicFeature *current_s ;
vpBasicFeature *desired_s ;
/* The vector dimensions are not known before the affectation loop.
* They thus should be allocated on the flight, in the loop.
* The first assumption is that the size has not changed. A double
* reallocation (realloc(dim*2)) is done if necessary. In particulary,
* [log_2(dim)+1] reallocations are done for the first error computation.
* If the allocated size is too large, a correction is done after the loop.
* The algotithmic cost is linear in affectation, logarthmic in allocation
* numbers and linear in allocation size.
* No assumptions are made concerning size of each vector: they are
* not said equal, and could be different.
*/
/* First assumption: vector dimensions have not changed. If 0, they are
* initialized to dim 1.*/
int dimError = error .getRows();
int dimS = s .getRows();
int dimSStar = sStar .getRows();
if (0 == dimError) { dimError = 1; error .resize(dimError);}
if (0 == dimS) { dimS = 1; s .resize(dimS);}
if (0 == dimSStar) { dimSStar = 1; sStar .resize(dimSStar);}
/* vectTmp is used to store the return values of functions get_s() and
* error(). */
vpColVector vectTmp;
int dimVectTmp;
/* The cursor are the number of the next case of the vector array to
* be affected. A memory reallocation should be done when cursor
* is out of the vector-array range.*/
int cursorS = 0;
int cursorSStar = 0;
int cursorError = 0;
/* For each cell of the list, recopy value of s, s_star and error. */
for (featureList.front(),
desiredFeatureList.front(),
featureSelectionList.front() ;
!featureList.outside() ;
featureList.next(),
desiredFeatureList.next(),
featureSelectionList.next() )
{
current_s = featureList.value() ;
desired_s = desiredFeatureList.value() ;
int select = featureSelectionList.value() ;
/* Get s, and store it in the s vector. */
vectTmp = current_s->get_s();
dimVectTmp = vectTmp .getRows();
while (dimVectTmp + cursorS > dimS)
{ dimS *= 2; s .resize (dimS,false); vpDEBUG_TRACE(15,"Realloc!"); }
for (int k = 0; k < dimVectTmp; ++k) { s[cursorS++] = vectTmp[k]; }
/* Get s_star, and store it in the s vector. */
vectTmp = desired_s->get_s();
dimVectTmp = vectTmp .getRows();
while (dimVectTmp + cursorSStar > dimSStar)
{ dimSStar *= 2; sStar .resize (dimSStar,false); }
for (int k = 0; k < dimVectTmp; ++k)
{ sStar[cursorSStar++] = vectTmp[k]; }
/* Get error, and store it in the s vector. */
vectTmp = current_s->error(*desired_s, select) ;
dimVectTmp = vectTmp .getRows();
while (dimVectTmp + cursorError > dimError)
{ dimError *= 2; error .resize (dimError,false); }
for (int k = 0; k < dimVectTmp; ++k)
{ error[cursorError++] = vectTmp[k]; }
}
/* If too much memory has been allocated, realloc. */
s .resize(cursorS,false);
sStar .resize(cursorSStar,false);
error .resize(cursorError,false);
/* Final modifications. */
dim_task = error.getRows() ;
errorComputed = true ;
}
catch(vpException me)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
catch(...)
{
throw ;
}
return error ;
}
void
vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
const vpColVector & v)
{
TPtuFrame ptuFrameInterface;
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
{
vpERROR_TRACE ("Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
}
switch(frame)
{
case vpRobot::CAMERA_FRAME :
{
ptuFrameInterface = PTU_CAMERA_FRAME;
if ( v.getRows() != 2) {
vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
throw vpRobotException (vpRobotException::wrongStateError,
"Bad dimension for speed vector "
"in camera frame");
}
break ;
}
case vpRobot::ARTICULAR_FRAME :
{
ptuFrameInterface = PTU_ARTICULAR_FRAME;
if ( v.getRows() != 2) {
vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
throw vpRobotException (vpRobotException::wrongStateError,
"Bad dimension for speed vector "
"in articular frame");
}
break ;
}
case vpRobot::REFERENCE_FRAME :
{
vpERROR_TRACE ("Cannot send a velocity to the robot "
"in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"in the reference frame:"
"functionality not implemented");
break ;
}
case vpRobot::MIXT_FRAME :
{
vpERROR_TRACE ("Cannot send a velocity to the robot "
"in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"in the mixt frame:"
"functionality not implemented");
break ;
}
default:
{
vpERROR_TRACE ("Error in spec of vpRobot. "
"Case not taken in account.");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot ");
}
}
vpDEBUG_TRACE (12, "Velocity limitation.");
bool norm = false; // Flag to indicate when velocities need to be nomalized
double ptuSpeedInterface[2];
switch(frame) {
case vpRobot::ARTICULAR_FRAME :
case vpRobot::CAMERA_FRAME : {
double max = this ->maxRotationVelocity;
for (unsigned int i = 0 ; i < 2; ++ i) // rx and ry of the camera
{
if (fabs (v[i]) > max)
{
norm = true;
max = fabs (v[i]);
vpERROR_TRACE ("Excess velocity: ROTATION "
"(axe nr.%d).", i);
}
}
// Rotations velocities normalisation
if (norm == true) {
max = this ->maxRotationVelocity / max;
for (unsigned int i = 0 ; i < 2; ++ i)
ptuSpeedInterface [i] = v[i]*max;
}
break;
}
default:
// Should never occur
break;
}
vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
<< " " << ptuSpeedInterface[1] << std::endl;
ptu.move(ptuSpeedInterface, ptuFrameInterface);
return;
}
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
\exception if errorHasBeenInitialized is true (that is if
vpGenericFeature::setError have been used) then s_star is useless. In that
since the error HAS TO BE recomputed at each iteration
errorHasBeenInitialized is set to errHasToBeUpdated if
vpGenericFeature::serError is not used in the loop then an exception is
thrown
obviously if vpGenericFeature::setError is not used then s_star is considered
and this warning is meaningless.
\param s_star : Desired visual feature.
\param select : The error can be computed for a selection of a
subset of the possible features.
- To compute the error for all the features use
vpBasicFeature::FEATURE_ALL. In that case the error vector column
vector whose dimension is equal to the number of features.
- To compute the error for only one of the component feature you
have to say which one you want to take into account. If it is the
first one set select to vpBasicFeature::FEATURE_LINE[0], if it is
the second one set select to vpBasicFeature::FEATURE_LINE[1], and
so on. In that case the error vector is a 1 dimension column
vector.
- To compute the error for only two of the component feature you
have to say which ones you want to take into account. If it is the
first one and the second one set select to
vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]. In
that case the error vector is a 2 dimension column vector.
\return The error \f$ (s-s^*)\f$ between the current and the desired
visual feature.
The code below shows how to use this method to manipulate the two
visual features over three:
\code
// Creation of the current feature s
vpGenericFeature s(3);
s.set_s(0, 0, 0);
// Creation of the desired feature s*
vpGenericFeature s_star(3);
s_star.set_s(1, 1, 1);
// Here you have to compute the interaction matrix L
s.setInteractionMatrix(L);
// Compute the error vector (s-s*) for the two first features
s.error(s_star, vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
\endcode
*/
vpColVector
vpGenericFeature::error(const vpBasicFeature &s_star,
const unsigned int select)
{
if (s_star.get_s().getRows() != dim_s)
{
vpERROR_TRACE("size mismatch between s* dimension "
"and feature dimension");
throw(vpFeatureException(vpFeatureException::sizeMismatchError,
"size mismatch between s* dimension "
"and feature dimension"));
}
vpColVector e(0) ;
try
{
if (errorStatus == errorHasToBeUpdated)
{
vpERROR_TRACE("Error has no been updated since last iteration"
"you should have used vpGenericFeature::setError"
"in you visual servoing loop") ;
throw(vpFeatureException(vpFeatureException::badErrorVectorError,
"Error has no been updated since last iteration"));
}
else
if (errorStatus == errorInitialized)
{
vpDEBUG_TRACE(25,"Error init: e=e.");
errorStatus = errorHasToBeUpdated ;
for (unsigned int i=0 ; i < dim_s ; i++)
if (FEATURE_LINE[i] & select )
{
vpColVector ex(1) ;
ex[i] = err[i] ;
e = vpMatrix::stackMatrices(e,ex) ;
}
}
else
{
vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
for (unsigned int i=0 ; i < dim_s ; i++)
if (FEATURE_LINE[i] & select )
{
vpColVector ex(1) ;
ex[0] = s[i] - s_star[i] ;
e = vpMatrix::stackMatrices(e,ex) ;
}
}
}
catch(vpMatrixException me)
{
vpERROR_TRACE("caught a Matric related error") ;
std::cout <<std::endl << me << std::endl ;
throw(me) ;
}
catch(vpException me)
{
vpERROR_TRACE("caught another error") ;
std::cout <<std::endl << me << std::endl ;
throw(me) ;
}
return e ;
}
