/*!
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);
}
/*!
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 ;
}
/*!
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);
}
/*!
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 ;
}
}
/*!
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 ;
}
}
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;
}
/*!
Send a velocity on each axis.
\param frame : Control frame. This biclops head can only be controlled in
articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference
frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are
not implemented.
\param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot
{r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the
camera and \f$ \dot{q}_2\f$ the tilt of the camera.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
velocity control mode is set. For that, call setRobotState(
vpRobot::STATE_VELOCITY_CONTROL) before setVelocity().
\exception vpRobotException::wrongStateError : If a not supported frame type
(vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME or vpRobot::MIXT_FRAME) is
given.
\warning Velocities could be saturated if one of them exceed the maximal
autorized speed (see vpRobot::maxRotationVelocity).
*/
void
vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
const vpColVector & q_dot)
{
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 :
{
vpERROR_TRACE ("Cannot send a velocity to the robot "
"in the camera frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot send a velocity to the robot "
"in the camera frame:"
"functionality not implemented");
break ;
}
case vpRobot::ARTICULAR_FRAME :
{
if ( q_dot.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
// Saturate articular speed
double max = vpBiclops::speedLimit;
vpColVector q_dot_sat(vpBiclops::ndof);
// init q_dot_saturated
q_dot_sat = q_dot;
for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
{
if (fabs (q_dot[i]) > max)
{
norm = true;
max = fabs (q_dot[i]);
vpERROR_TRACE ("Excess velocity: ROTATION "
"(axe nr.%d).", i);
}
}
// Rotations velocities normalisation
if (norm == true) {
max = vpBiclops::speedLimit / max;
q_dot_sat = q_dot * max;
}
vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
for (unsigned int i=0; i < vpBiclops::ndof; i ++)
shm.q_dot[i] = q_dot[i];
controller.writeShm(shm);
vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
return;
}
