/*!
Send to the controller a velocity.
\param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
and camera frame (vpRobot::CAMERA_FRAME) are implemented.
\param v : Velocity to apply to the robot.
- In the camera frame, this velocity is represented by a vector of dimension 6
\f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
{\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
- In articular, this velocity is represented by a 6 dimension vector \f$
\dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
here set to identity.
We use the exponential map (vpExponentialMap) to update the camera location.
Sampling time can be set using setSamplingTime().
\sa setSamplingTime()
*/
void
vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame,
const vpColVector &v)
{
switch (frame)
{
case vpRobot::ARTICULAR_FRAME:
setArticularVelocity(v) ;
break ;
case vpRobot::CAMERA_FRAME:
setCameraVelocity(v) ;
break ;
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the reference frame:"
"functionality not implemented");
break ;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
}
/*!
Waits for the Servolens promt '>'.
\return The prompt character.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
char
vpServolens::wait() const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
ssize_t r;
r = ::write(this->remfd, "\r\n", strlen("\r\n"));
if (r != (ssize_t)(strlen("\r\n"))) {
throw vpRobotException (vpRobotException::communicationError,
"Cannot write on Servolens.");
}
char c;
do {
r = ::read(this->remfd, &c, 1);
c &= 0x7f;
if (r != 1) {
throw vpRobotException (vpRobotException::communicationError,
"Cannot read on Servolens.");
}
}
while (c != '>');
return c;
}
/*!
Write a string to the serial link.
\param s : String to send to the Servolens.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::write(const char *s) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
ssize_t r = 0;
r = ::write(this->remfd,"\r", strlen("\r"));
r += ::write(this->remfd, s, strlen(s));
r += ::write(this->remfd,"\r", strlen("\r"));
if (r != (ssize_t)(2*strlen("\r") + strlen(s))) {
throw vpRobotException (vpRobotException::communicationError,
"Cannot write on Servolens.");
}
/*
* Une petite tempo pour laisser le temps a la liaison serie de
* digerer la commande envoyee. En fait, la liaison serie fonctionne
* a 9600 bauds soit une transmission d'environ 9600 bits pas seconde.
* Les plus longues commandes envoyees sur la liaison serie sont du type:
* SX0842 soit 6 caracteres codes sur 8 bits chacuns = 48 bits pour
* envoyer la commande SX0842.
* Ainsi, le temps necessaire pour envoyer SX0842 est d'environ
* 48 / 9600 = 0,0050 secondes = 5 milli secondes.
* Ici on rajoute une marge pour amener la tempo a 20 ms.
*/
vpTime::wait(20);
}
/*!
Waits the end of motion of the corresponding servo motor.
\param servo : Servolens servo motor.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::wait(vpServoType servo) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char c;
char fin_mvt[3];
bool sortie = false;
switch (servo) {
case ZOOM:
sprintf(fin_mvt, "ZF");
break;
case FOCUS:
sprintf(fin_mvt, "FF");
break;
case IRIS:
default:
sprintf(fin_mvt, "DF");
break;
}
/* lecture des caracteres recus */
do {
/* lecture des caracteres */
if (::read(this->remfd,&c,1) != 1) {
throw vpRobotException (vpRobotException::communicationError,
"Cannot read on Servolens.");
}
c &= 0x7f;
/* tests si fin de mouvement */
if (c == fin_mvt[0]) {
/* lecture du caractere suivant */
if (::read(this->remfd,&c,1) != 1) {
throw vpRobotException (vpRobotException::communicationError,
"Cannot read on Servolens.");
}
c &= 0x7f;
if (c == fin_mvt[1]) {
sortie = true;
}
}
}
while ( !sortie);
/* printf("\nmouvement fini: chaine lue = %s", chaine); */
}
/*!
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 ;
}
void
vpRobotPtu46::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 ;
}
// Interface for the controller
double artpos[2];
artpos[0] = q[0];
artpos[1] = q[1];
if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
{
vpERROR_TRACE ("Positionning error.");
throw vpRobotException (vpRobotException::lowLevelError,
"Positionning error.");
}
return ;
}
/*!
Get the articular velocity.
\param frame : Control frame. This head can only be controlled in articular.
\param q_dot : The measured articular velocity in rad/s.
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
void
vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
vpColVector & q_dot)
{
TPtuFrame ptuFrameInterface = PTU_ARTICULAR_FRAME;
switch (frame)
{
case vpRobot::CAMERA_FRAME:
{
vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the camera frame:"
"functionality not implemented");
break ;
}
case vpRobot::ARTICULAR_FRAME:
{
ptuFrameInterface = PTU_ARTICULAR_FRAME;
break ;
}
case vpRobot::REFERENCE_FRAME:
{
vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the reference frame:"
"functionality not implemented");
break ;
}
case vpRobot::MIXT_FRAME:
{
vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
}
q_dot.resize(vpPtu46::ndof);
double ptuSpeedInterface[2];
ptu.getCurrentSpeed(ptuSpeedInterface, ptuFrameInterface);
q_dot[0] = ptuSpeedInterface[0];
q_dot[1] = ptuSpeedInterface[1];
}
/*!
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
displacement.
\param frame The frame in which the measured displacement is expressed.
\param d The displacement:
- In articular, the dimension of q is 2 (the number of axis of the robot)
with respectively d[0] (pan displacement), d[1] (tilt displacement).
- In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz).
Translations are expressed in meters, rotations in radians with the theta U
representation.
\exception vpRobotException::wrongStateError If a not supported frame type is
given.
\sa getArticularDisplacement(), getCameraDisplacement()
*/
void
vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
vpColVector &d)
{
vpColVector q_current; // current position
getPosition(vpRobot::ARTICULAR_FRAME, q_current);
switch(frame) {
case vpRobot::ARTICULAR_FRAME:
d.resize(vpBiclops::ndof);
d = q_current - q_previous;
break ;
case vpRobot::CAMERA_FRAME: {
d.resize(6);
vpHomogeneousMatrix fMc_current;
vpHomogeneousMatrix fMc_previous;
fMc_current = vpBiclops::get_fMc(q_current);
fMc_previous = vpBiclops::get_fMc(q_previous);
vpHomogeneousMatrix c_previousMc_current;
// fMc_c = fMc_p * c_pMc_c
// => c_pMc_c = (fMc_p)^-1 * fMc_c
c_previousMc_current = fMc_previous.inverse() * fMc_current;
// Compute the instantaneous velocity from this homogeneous matrix.
d = vpExponentialMap::inverse( c_previousMc_current );
break ;
}
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the reference frame:"
"functionality not implemented");
break ;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
q_previous = q_current; // Update for next call of this method
}
/*!
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
displacement.
\param frame The frame in which the measured displacement is expressed.
\param d The displacement:
- In articular, the dimension of q is 2 (the number of axis of the robot)
with respectively d[0] (pan displacement), d[1] (tilt displacement).
- In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
rz). Translations are expressed in meters, rotations in radians.
\exception vpRobotException::wrongStateError If a not supported frame type is
given.
\sa getArticularDisplacement(), getCameraDisplacement()
*/
void
vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
vpColVector &d)
{
double d_[6];
switch (frame)
{
case vpRobot::CAMERA_FRAME:
{
d.resize (6);
ptu.measureDpl(d_, PTU_CAMERA_FRAME);
d[0]=d_[0];
d[1]=d_[1];
d[2]=d_[2];
d[3]=d_[3];
d[4]=d_[4];
d[5]=d_[5];
break ;
}
case vpRobot::ARTICULAR_FRAME:
{
ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
d.resize (vpPtu46::ndof);
d[0]=d_[0]; // pan
d[1]=d_[1]; // tilt
break ;
}
case vpRobot::REFERENCE_FRAME:
{
vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a displacement in the reference frame:"
"functionality not implemented");
break ;
}
case vpRobot::MIXT_FRAME:
{
vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a displacement in the reference frame:"
"functionality not implemented");
break ;
}
}
}
/*!
Set the velocity (frame has to be specified) that will be applied to the robot.
\param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
rotational velocities are implemented.
\param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
- If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
independently.
- If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
Second value is the rotational velocity in rad/s.
Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
\exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
init();
/*
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) ");
} */
if (vel.size() != 2)
{
throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
}
vpColVector vel_max(2);
vpColVector vel_sat;
if (frame == vpRobot::REFERENCE_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxRotationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
this->unlock();
}
else if (frame == vpRobot::ARTICULAR_FRAME)
{
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxTranslationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
//std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
this->unlock();
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send the robot velocity in the specified control frame");
}
}
/*!
Read one character form the serial link.
\param c : The character that was read.
\param timeout_s : Timeout in seconds.
\return true if the reading was successfully achieved, false otherwise.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
bool
vpServolens::read(char *c, long timeout_s) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
fd_set readfds; /* list of fds for select to listen to */
struct timeval timeout = {timeout_s, 0}; // seconde, micro-sec
FD_ZERO(&readfds);
FD_SET(static_cast<unsigned int>(this->remfd), &readfds);
if (select(FD_SETSIZE, &readfds, (fd_set *)NULL,
(fd_set *)NULL, &timeout) > 0) {
int n = ::read(this->remfd, c, 1); /* read one character at a time */
if (n != 1)
return false;
*c &= 0x7f;
//printf("lecture 1 car: %c\n", *c);
return(true);
}
return (false);
}
/*!
Set the controller type.
\param controller : Controller type.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::setController(vpControllerType controller) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
switch(controller) {
case AUTO:
/* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW1");
this->write(commande);
break;
case CONTROLLED:
/* nettoyage : mot d'etat vide 0000 */
sprintf(commande,"SX0842");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
break;
case RELEASED:
sprintf(commande,"SX1084");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
break;
}
}
/*!
Reset the Servolens.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::reset() const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
/* suppression de l'echo */
sprintf(commande, "SE1");
this->write(commande);
/* initialisation de l'objectif, idem qu'a la mise sous tension */
sprintf(commande, "SR0");
this->write(commande);
vpTime::wait(25000);
this->wait();
/* suppression de l'echo */
sprintf(commande, "SE0");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
}
/*!
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];
}
/*!
Send to the controller a velocity.
\param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
and camera frame (vpRobot::CAMERA_FRAME) are implemented.
\param v : Velocity to apply to the robot.
- In the camera frame, this velocity is represented by a vector of dimension 6
\f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
{\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
- In articular, this velocity is represented by a 6 dimension vector \f$
\dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
here set to identity.
We use the exponential map (vpExponentialMap) to update the camera location.
Sampling time can be set using setSamplingTime().
\sa setSamplingTime()
*/
void
vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame,
const vpColVector &v)
{
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
}
switch (frame)
{
case vpRobot::ARTICULAR_FRAME:
case vpRobot::CAMERA_FRAME: {
vpColVector v_max(6);
for (unsigned int i=0; i<3; i++)
v_max[i] = getMaxTranslationVelocity();
for (unsigned int i=3; i<6; i++)
v_max[i] = getMaxRotationVelocity();
vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
setRobotFrame(frame);
break ;
}
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the reference frame:"
"functionality not implemented");
break ;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
}
/*!
Read the content of the position file and moves to head to articular
position.
\param filename : Position filename
\exception vpRobotException::readingParametersError : If the articular
position cannot be read from file.
\sa readPositionFile()
*/
void
vpRobotPtu46::setPosition(const char *filename)
{
vpColVector q ;
if (readPositionFile(filename, q) == false) {
vpERROR_TRACE ("Cannot get ptu-46 position from file");
throw vpRobotException (vpRobotException::readingParametersError,
"Cannot get ptu-46 position from file");
}
setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
}
/*!
Set the velocity used for a position control.
\param velocity : Velocity in % of the maximum velocity between [0,100]. The
maximum velocity is given vpBiclops::speedLimit.
*/
void
vpRobotBiclops::setPositioningVelocity (const double velocity)
{
if (velocity < 0 || velocity > 100) {
vpERROR_TRACE("Bad positionning velocity");
throw vpRobotException (vpRobotException::constructionError,
"Bad positionning velocity");
}
positioningVelocity = velocity;
}
/*!
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 ;
}
/*!
Activates the auto iris mode of the Servolens.
\param enable : true to activate the auto iris.
*/
void
vpServolens::setAutoIris(bool enable) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
if (enable)
sprintf(commande, "DA1");
else
sprintf(commande, "DA0");
this->write(commande);
}
/*!
Get the robot displacement (frame has to be specified).
\param frame : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented.
\param dis : A 6 dimension vector that corresponds to the displacement of the robot since the last call to the function.
\param timestamp : timestamp of the last update of the displacement
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpROSRobot::getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &dis, struct timespec ×tamp){
while(!odom_mutex);
odom_mutex = false;
vpColVector pose_cur(displacement);
timestamp.tv_sec = _sec;
timestamp.tv_nsec = _nsec;
odom_mutex = true;
if(frame == vpRobot::REFERENCE_FRAME){
dis = pose_cur - pose_prev;
pose_prev = pose_cur;
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get robot displacement in the specified control frame");
}
}
/*!
Enable or disable the emission of the Servolens prompt "SERVOLENS>".
\param active : true to activate the emission of the prompy. false
to disable this functionality.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::enablePrompt(bool active) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
/* suppression de l'echo */
if (active == true)
sprintf(commande, "SE1");
else
sprintf(commande, "SE0");
this->write(commande);
}
/*!
Set the velocity (frame has to be specified) that will be applied to the robot.
\param frame : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented.
\param vel : A 6 dimension vector that corresponds to the velocities to apply to the robot.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpROSRobot::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
geometry_msgs::Twist msg;
if (frame == vpRobot::REFERENCE_FRAME)
{
msg.linear.x = vel[0];
msg.linear.y = vel[1];
msg.linear.z = vel[2];
msg.angular.x = vel[3];
msg.angular.y = vel[4];
msg.angular.z = vel[5];
cmdvel.publish(msg);
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send the robot velocity in the specified control frame");
}
}
/*!
Basic initialisation
*/
void vpROSRobot::init(){
if(ros::isInitialized() && ros::master::getURI() != _master_uri){
throw (vpRobotException(vpRobotException::constructionError,
"ROS robot already initialised with a different master_URI (" + ros::master::getURI() +" != " + _master_uri + ")") );
}
if(!isInitialized){
int argc = 2;
char *argv[2];
argv[0] = new char [255];
argv[1] = new char [255];
std::string exe = "ros.exe", arg1 = "__master:=";
strcpy(argv[0], exe.c_str());
arg1.append(_master_uri);
strcpy(argv[1], arg1.c_str());
init(argc, argv);
delete [] argv[0];
delete [] argv[1];
}
}
/*!
These parameters are computed from the Dragonfly2 DR2-COL camera sensor pixel
size (7.4 um) and from the servolens zoom position.
\param I : An image coming from the Dragonfly2 camera attached to the
servolens.
\code
#include <visp3/vs/vpServolens.h>
int main()
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
vpServolens servolens("/dev/ttyS0");
vpImage<unsigned char> I(240, 320);
vpCameraParameters cam = servolens.getCameraParameters(I);
std::cout << "Camera parameters: " << cam << std::endl;
#endif
}
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
\endcode
*/
vpCameraParameters
vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
vpCameraParameters cam;
double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
double px=1000, py=1000, u0=320, v0=240;
// Determine if the image is subsampled.
// Dragonfly2 native images are 640 by 480
double subsample_factor = 1.;
double width = I.getWidth();
double height= I.getHeight();
if (width > 300 && width < 340 && height > 220 && height < 260)
subsample_factor = 2;
else if (width > 140 && width < 1800 && height > 100 && height < 140)
subsample_factor = 4;
unsigned zoom;
getPosition(vpServolens::ZOOM, zoom);
//std::cout << "Actual zoom value: " << zoom << std::endl;
// XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
// aux parametres de calibration de la camera (donnees constructeur) pour des
// tailles d'images CCIR (768x576), donc avec scale = 1.
double focale = zoom * 1.0e-5; // Transformation en metres
px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
u0 = I.getWidth() / 2.;
v0 = I.getHeight() / 2.;
cam.initPersProjWithoutDistortion(px, py, u0, v0);
return cam;
}
/*!
Get the robot position (frame has to be specified).
\param frame : Control frame. For the moment, only vpRobot::REFERENCE_FRAME is implemented.
\param pose : A 6 dimension vector that corresponds to the position of the robot.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
*/
void vpROSRobot::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &pose) {
while(!odom_mutex);
odom_mutex = false;
if (frame == vpRobot::REFERENCE_FRAME)
{
pose.resize(6);
pose[0] = p[0];
pose[1] = p[1];
pose[2] = p[2];
vpRotationMatrix R(q);
vpRxyzVector V(R);
pose[3]=V[0];
pose[4]=V[1];
pose[5]=V[2];
}
else
{
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get the robot position in the specified control frame");
}
odom_mutex = true;
}
/*!
Initialize the Servolens lens.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
\sa open()
*/
void
vpServolens::init() const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
/* suppression de l'echo */
sprintf(commande, "SE0");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
/* L'experience montre qu'une petite tempo est utile. */
vpTime::wait(500);
}
/*!
Set the Biclops config filename.
Check if the config file exists and initialize the head.
\exception vpRobotException::constructionError If the config file cannot be
oppened.
*/
void
vpRobotBiclops::init ()
{
// test if the config file exists
FILE *fd = fopen(configfile, "r");
if (fd == NULL) {
vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
throw vpRobotException (vpRobotException::constructionError,
"Cannot open connexion with biclops");
}
fclose(fd);
// Initialize the controller
controller.init(configfile);
try
{
setRobotState(vpRobot::STATE_STOP) ;
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
vpRobotBiclops::robotAlreadyCreated = true;
positioningVelocity = defaultPositioningVelocity ;
// Initialize previous articular position to manage getDisplacement()
q_previous.resize(vpBiclops::ndof);
q_previous = 0;
controlThreadCreated = false;
return ;
}
/*!
Set or remove the Servolens command complete status at the end of servoing.
\param servo : Servolens servo motor.
\param active : true to activate the emission of a command complete
flag at the end of motion. false to disable this functionality.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
void
vpServolens::enableCmdComplete(vpServoType servo, bool active) const
{
if (!isinit) {
vpERROR_TRACE ("Cannot dial with Servolens.");
throw vpRobotException (vpRobotException::communicationError,
"Cannot dial with Servolens.");
}
char commande[10];
/* Envoie une commande pour qu'en fin de mouvement servolens renvoie
* une information de fin de mouvement (ex: ZF, FF, DF).
*/
switch(servo) {
case ZOOM:
if (active)
sprintf(commande, "ZF1");
else
sprintf(commande, "ZF0");
break;
case FOCUS:
if (active)
sprintf(commande, "FF1");
else
sprintf(commande, "FF0");
break;
case IRIS:
if (active)
sprintf(commande, "DF1");
else
sprintf(commande, "DF0");
break;
}
/* envoie de la commande */
this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
}
/*!
Gets the current translational velocity of the robot.
\param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
rotational velocities are implemented.
\param velocity : A two dimension vector that corresponds to the current velocities applied to the robot.
- If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
second value is the velocity of the right wheel in m/s.
- If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
Second value is the rotational velocity in rad/s.
\exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
\exception vpRobotException::wrongStateError : If the specified control frame is not supported.
\sa getVelocity(const vpRobot::vpControlFrameType)
*/
void vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity)
{
init();
velocity.resize(2);
if (frame == vpRobot::ARTICULAR_FRAME)
{
this->lock();
velocity[0] = this->getLeftVel() / 1000.;
velocity[1] = this->getRightVel() / 1000;
this->unlock();
}
else if (frame == vpRobot::REFERENCE_FRAME)
{
this->lock();
velocity[0] = this->getVel() / 1000.;
velocity[1] = vpMath::rad( this->getRotVel() );
this->unlock();
}
else {
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get the robot volocity in the specified control frame");
}
}
/*!
Send to the controller a velocity.
\param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
\param v : Velocity vector \f$(v_x, w_z)\f$ to apply to the robot.
Depending on the velocity specified as input, the robot position is updated
using the sampling time that can be modified using setSamplingTime().
\sa setSamplingTime()
*/
void
vpSimulatorPioneer::setVelocity(const vpRobot::vpControlFrameType frame,
const vpColVector &v)
{
switch (frame)
{
case vpRobot::ARTICULAR_FRAME: {
if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
}
setRobotFrame(frame);
// v is a 2 dimension vector that contains v,w
if (v.size() != 2) {
vpERROR_TRACE ("Bad dimension of the control vector");
throw vpRobotException (vpRobotException::dimensionError,
"Bad dimension of the control vector");
}
vpColVector v_max(2);
v_max[0] = getMaxTranslationVelocity();
v_max[1] = getMaxRotationVelocity();
vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
xm_ += delta_t_ * v_sat[0] * cos(theta_);
ym_ += delta_t_ * v_sat[0] * sin(theta_);
theta_ += delta_t_ * v_sat[1];
vpRotationMatrix wRe(0, 0, theta_);
vpTranslationVector wte(xm_, ym_, 0);
wMe_.buildFrom(wte, wRe);
wMc_ = wMe_ * cMe_.inverse();
break ;
}
break ;
case vpRobot::CAMERA_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the camera frame:"
"functionality not implemented");
break ;
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the articular frame:"
"functionality not implemented");
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot set a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
}
