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;
}
/*!
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 robot position in the world frame.
\param wMc : Transformation from world frame to camera frame.
*/
void vpSimulatorCamera::setPosition(const vpHomogeneousMatrix &wMc)
{
if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
this->wMc_ = wMc;
}
/*!
Set the robot position as the transformation from camera frame to world frame.
*/
void
vpRobotCamera::setPosition(const vpHomogeneousMatrix &cMw)
{
if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
this->cMw_ = cMw ;
}
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 ;
}
/*!
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 ;
}
bool DynamicsComputations::setRobotState(const VectorDynSize& q,
const VectorDynSize& q_dot,
const VectorDynSize& q_dotdot,
const Twist& world_gravity)
{
Transform world_T_base = Transform::Identity();
Twist base_velocity = SpatialMotionVectorRaw::Zero();
Twist base_acceleration = SpatialMotionVectorRaw::Zero();
return setRobotState(q,q_dot,q_dotdot,
world_T_base,base_velocity,base_acceleration,
world_gravity);
}
/*!
Destructor.
Close the serial connection with the head.
*/
vpRobotPtu46::~vpRobotPtu46 (void)
{
setRobotState(vpRobot::STATE_STOP) ;
if (0 != ptu.close())
{
vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
}
vpRobotPtu46::robotAlreadyCreated = false;
delete [] device;
return;
}
/*!
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 ;
}
}
/*!
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 ;
}
/*!
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 ;
}
}
/*!
Halt all the axis.
*/
void
vpRobotPtu46::stopMotion(void)
{
ptu.stop();
setRobotState (vpRobot::STATE_STOP);
}