#include <vpRobotBiclops.h>

Inheritance diagram for vpRobotBiclops:

Public Types
enum	DenavitHartenbergModel { DH1, DH2 }

enum	vpRobotStateType { STATE_STOP, STATE_VELOCITY_CONTROL, STATE_POSITION_CONTROL, STATE_ACCELERATION_CONTROL, STATE_FORCE_TORQUE_CONTROL }

enum	vpControlFrameType { REFERENCE_FRAME, ARTICULAR_FRAME, JOINT_STATE = ARTICULAR_FRAME, END_EFFECTOR_FRAME, CAMERA_FRAME, TOOL_FRAME = CAMERA_FRAME, MIXT_FRAME }

Public Member Functions
	vpRobotBiclops ()

	vpRobotBiclops (const std::string &filename)

virtual	~vpRobotBiclops ()

void	init ()

void	get_cMe (vpHomogeneousMatrix &_cMe) const

void	get_cVe (vpVelocityTwistMatrix &_cVe) const

void	get_eJe (vpMatrix &_eJe)

void	get_fJe (vpMatrix &_fJe)

void	getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &d)

void	getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q)

double	getPositioningVelocity (void)

void	getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &q_dot)

vpColVector	getVelocity (const vpRobot::vpControlFrameType frame)

bool	readPositionFile (const std::string &filename, vpColVector &q)

void	setConfigFile (const std::string &filename="/usr/share/BiclopsDefault.cfg")

void	setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q)

void	setPosition (const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)

void	setPosition (const char *filename)

void	setPositioningVelocity (double velocity)

vpRobot::vpRobotStateType	setRobotState (const vpRobot::vpRobotStateType newState)

void	setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)

void	stopMotion ()

Inherited functionalities from vpBiclops
void	computeMGD (const vpColVector &q, vpHomogeneousMatrix &fMc) const

vpHomogeneousMatrix	computeMGD (const vpColVector &q) const

void	computeMGD (const vpColVector &q, vpPoseVector &fvc) const

vpHomogeneousMatrix	get_cMe () const

void	get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const

void	get_fMc (const vpColVector &q, vpPoseVector &fvc) const

vpHomogeneousMatrix	get_fMc (const vpColVector &q) const

vpHomogeneousMatrix	get_fMe (const vpColVector &q) const

void	get_eJe (const vpColVector &q, vpMatrix &eJe) const

void	get_fJe (const vpColVector &q, vpMatrix &fJe) const

vpBiclops::DenavitHartenbergModel	getDenavitHartenbergModel () const

void	set_cMe ()

void	set_cMe (const vpHomogeneousMatrix &cMe)

void	setDenavitHartenbergModel (vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1)

Inherited functionalities from vpRobot
double	getMaxTranslationVelocity (void) const

double	getMaxRotationVelocity (void) const

vpColVector	getPosition (const vpRobot::vpControlFrameType frame)

virtual vpRobotStateType	getRobotState (void) const

void	setMaxRotationVelocity (double maxVr)

void	setMaxTranslationVelocity (double maxVt)

void	setVerbose (bool verbose)

Static Public Member Functions
static void *	vpRobotBiclopsSpeedControlLoop (void *arg)

Static Public Member Functions inherited from vpRobot
static vpColVector	saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)

Static Public Attributes
static const double	defaultPositioningVelocity = 10.0

static const unsigned int	ndof = 2

static const float	h = 0.048f

static const float	panJointLimit = (float)(M_PI)

static const float	tiltJointLimit = (float)(M_PI / 4.5)

static const float	speedLimit = (float)(M_PI / 3.0)

Protected Member Functions
Protected Member Functions Inherited from vpRobot
vpControlFrameType	setRobotFrame (vpRobot::vpControlFrameType newFrame)

vpControlFrameType	getRobotFrame (void) const

Protected Attributes
DenavitHartenbergModel	dh_model_

vpHomogeneousMatrix	cMe_

double	maxTranslationVelocity

double	maxRotationVelocity

int	nDof

vpMatrix	eJe

int	eJeAvailable

vpMatrix	fJe

int	fJeAvailable

int	areJointLimitsAvailable

double *	qmin

double *	qmax

bool	verbose_

Static Protected Attributes
static const double	maxTranslationVelocityDefault = 0.2

static const double	maxRotationVelocityDefault = 0.7

Detailed Description

Interface for the biclops, pan, tilt head control.

See http://www.traclabs.com/biclopspt.html for more details.

This class provide a position and a speed control interface for the biclops head. To manage the biclops joint limits in speed control, a control loop is running in a seperate thread implemented in vpRobotBiclopsSpeedControlLoop().

The control of the head is done by vpRobotBiclopsController class.

Warning: Velocity control mode is not exported from the top-level Biclops API class provided by Traclabs. That means that there is no protection in this mode to prevent an axis from striking its hard limit. In position mode, Traclabs put soft limits in that keep any command from driving to a position too close to the hard limits. In velocity mode this protection does not exist in the current API.; With the understanding that hitting the hard limits at full speed/power can damage the unit, damage due to velocity mode commanding is under user responsibility.

Examples:: moveBiclops.cpp, servoBiclopsPoint2DArtVelocity.cpp, and servoPioneerPanSegment3D.cpp.

Definition at line 94 of file vpRobotBiclops.h.

Member Enumeration Documentation

enum vpBiclops::DenavitHartenbergModel

inherited

Two different Denavit Hartenberg representations of the robot are implemented. They differ in the orientation of the tilt axis.

The first representation, vpBiclops::DH1 is given by:

$\begin{tabular}{|c|c|c|c|c|} \hline Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ \hline 1 & 0 & 0 & $-\pi/2$ & $q_1^*$ \\ 2 & 0 & 0 & $ \pi/2$ & $q_2^* + \pi/2$ \\ \hline \end{tabular}$

The second one, vpBiclops::DH2 is given by:

$\begin{tabular}{|c|c|c|c|c|} \hline Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ \hline 1 & 0 & 0 & $ \pi/2$ & $q_1^*$ \\ 2 & 0 & 0 & $ -\pi/2$ & $q_2^* - \pi/2$ \\ \hline \end{tabular}$

where $q_1^*, q_2^*$ are respectively the pan and tilt joint positions.

In those representations, the pan is oriented from left to right, while the tilt is oriented

in vpBiclops::DH1 from down to top,
in vpBiclops::DH2 from top to down.

Enumerator
DH1	First Denavit Hartenberg representation.
DH2	Second Denavit Hartenberg representation.

Definition at line 120 of file vpBiclops.h.

enum vpRobot::vpControlFrameType

inherited

Robot control frames.

Enumerator
REFERENCE_FRAME	Corresponds to a fixed reference frame attached to the robot structure.
ARTICULAR_FRAME	Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE.
JOINT_STATE	Corresponds to the joint state.
END_EFFECTOR_FRAME	Corresponds to robot end-effector frame.
CAMERA_FRAME	Corresponds to a frame attached to the camera mounted on the robot end-effector.
TOOL_FRAME	Corresponds to a frame attached to the tool (camera, gripper...) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME.
MIXT_FRAME	Corresponds to a "virtual" frame where translations are expressed in the reference frame, and rotations in the camera frame.

Definition at line 75 of file vpRobot.h.

enum vpRobot::vpRobotStateType

inherited

Robot control states.

Enumerator
STATE_STOP	Stops robot motion especially in velocity and acceleration control.
STATE_VELOCITY_CONTROL	Initialize the velocity controller.
STATE_POSITION_CONTROL	Initialize the position controller.
STATE_ACCELERATION_CONTROL	Initialize the acceleration controller.
STATE_FORCE_TORQUE_CONTROL	Initialize the force/torque controller.

Definition at line 64 of file vpRobot.h.

Constructor & Destructor Documentation

vpRobotBiclops::vpRobotBiclops ( )

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.

#include <visp3/robot/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;
}

Definition at line 112 of file vpRobotBiclops.cpp.

References setConfigFile(), and vpDEBUG_TRACE.

vpRobotBiclops::vpRobotBiclops ( const std::string & filename )

explicit

Constructor that 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.

#include <visp3/robot/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;
}

Definition at line 159 of file vpRobotBiclops.cpp.

References init(), setConfigFile(), and vpDEBUG_TRACE.

vpRobotBiclops::~vpRobotBiclops ( )

virtual

Destructor. Wait the end of the control thread.

Definition at line 185 of file vpRobotBiclops.cpp.

References setRobotState(), vpRobot::STATE_STOP, vpCERROR, and vpDEBUG_TRACE.

Member Function Documentation

void vpBiclops::computeMGD	(	const vpColVector &	q,
		vpHomogeneousMatrix &	fMc
	)		const

inherited

Compute the direct geometric model of the camera: fMc

Warning: Provided for compatibilty with previous versions. Use rather get_fMc(const vpColVector &, vpHomogeneousMatrix &).

Parameters

q	: Articular position for pan and tilt axis.
fMc	: Homogeneous matrix corresponding to the direct geometric model of the camera. Describes the transformation between the robot reference frame (called fixed) and the camera frame.

See also: get_fMc(const vpColVector &, vpHomogeneousMatrix &)

Definition at line 75 of file vpBiclops.cpp.

References vpBiclops::cMe_, vpBiclops::get_fMe(), vpHomogeneousMatrix::inverse(), and vpCDEBUG.

Referenced by vpBiclops::computeMGD().

vpHomogeneousMatrix vpBiclops::computeMGD ( const vpColVector & q ) const

inherited

Return the direct geometric model of the camera: fMc

Warning: Provided for compatibilty with previous versions. Use rather get_fMc(const vpColVector &).

Parameters

q	: Articular position for pan and tilt axis.

Returns: fMc, the homogeneous matrix corresponding to the direct geometric model of the camera. Describes the transformation between the robot reference frame (called fixed) and the camera frame.

See also: get_fMc(const vpColVector &)

Definition at line 119 of file vpBiclops.cpp.

References vpBiclops::computeMGD().

void vpBiclops::computeMGD	(	const vpColVector &	q,
		vpPoseVector &	fvc
	)		const

inherited

Compute the direct geometric model of the camera in terms of pose vector.

Warning: Provided for compatibilty with previous versions. Use rather get_fMc(const vpColVector &, vpPoseVector &).

Parameters

q	: Articular position for pan and tilt axis.
fvc	: Pose vector corresponding to the transformation between the robot reference frame (called fixed) and the camera frame.

See also: get_fMc(const vpColVector &, vpPoseVector &)

Definition at line 233 of file vpBiclops.cpp.

References vpPoseVector::buildFrom(), vpBiclops::get_fMc(), and vpHomogeneousMatrix::inverse().

void vpRobotBiclops::get_cMe ( vpHomogeneousMatrix & cMe ) const

Get the homogeneous matrix corresponding to the transformation between the camera frame and the end effector frame. The end effector frame is located on the tilt axis.

Parameters

cMe	: Homogeneous matrix between camera and end effector frame.

Definition at line 630 of file vpRobotBiclops.cpp.

References vpBiclops::get_cMe().

vpHomogeneousMatrix vpBiclops::get_cMe ( ) const

inlineinherited

Return the tranformation ${^c}{\bf M}_e$ between the camera frame and the end effector frame.

Definition at line 157 of file vpBiclops.h.

Referenced by get_cMe(), and get_cVe().

void vpRobotBiclops::get_cVe ( vpVelocityTwistMatrix & cVe ) const

Get the twist matrix corresponding to the transformation between the camera frame and the end effector frame. The end effector frame is located on the tilt axis.

Parameters

cVe	: Twist transformation between camera and end effector frame to expess a velocity skew from end effector frame in camera frame.

Definition at line 613 of file vpRobotBiclops.cpp.

References vpVelocityTwistMatrix::buildFrom(), and vpBiclops::get_cMe().

void vpRobotBiclops::get_eJe ( vpMatrix & _eJe )

virtual

Get the robot jacobian expressed in the end-effector frame.

Warning: Re is not the embedded camera frame. It corresponds to the frame associated to the tilt axis (see also get_cMe).

Parameters

_eJe	: Jacobian between end effector frame and end effector frame (on tilt axis).

Implements vpRobot.

Definition at line 642 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpBiclops::get_eJe(), getPosition(), and vpERROR_TRACE.

void vpBiclops::get_eJe	(	const vpColVector &	q,
		vpMatrix &	eJe
	)		const

inherited

Get the robot jacobian expressed in the end-effector frame.

Warning: Re is not the embedded camera frame. It corresponds to the frame associated to the tilt axis (see also get_cMe).

Parameters

q	: Articular position for pan and tilt axis.
eJe	: Jacobian between end effector frame and end effector frame (on tilt axis).

Definition at line 360 of file vpBiclops.cpp.

References vpBiclops::DH1, vpBiclops::dh_model_, vpException::dimensionError, vpArray2D< Type >::getRows(), vpArray2D< Type >::resize(), and vpERROR_TRACE.

Referenced by get_eJe().

void vpRobotBiclops::get_fJe ( vpMatrix & _fJe )

virtual

Get the robot jacobian expressed in the robot reference frame

Parameters

_fJe	: Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis).

Implements vpRobot.

Definition at line 662 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpBiclops::get_fJe(), getPosition(), and vpERROR_TRACE.

void vpBiclops::get_fJe	(	const vpColVector &	q,
		vpMatrix &	fJe
	)		const

inherited

Get the robot jacobian expressed in the robot reference frame

Parameters

q	: Articular position for pan and tilt axis.
fJe	: Jacobian between reference frame (or fix frame) and end effector frame (on tilt axis).

Definition at line 394 of file vpBiclops.cpp.

References vpBiclops::DH1, vpBiclops::dh_model_, vpException::dimensionError, vpArray2D< Type >::getRows(), vpArray2D< Type >::resize(), and vpERROR_TRACE.

Referenced by get_fJe().

void vpBiclops::get_fMc	(	const vpColVector &	q,
		vpHomogeneousMatrix &	fMc
	)		const

inherited

Compute the direct geometric model of the camera: fMc

Parameters

q	: Articular position for pan and tilt axis.
fMc	: Homogeneous matrix corresponding to the direct geometric model of the camera. Describes the transformation between the robot reference frame (called fixed) and the camera frame.

Definition at line 95 of file vpBiclops.cpp.

References vpBiclops::cMe_, vpBiclops::get_fMe(), vpHomogeneousMatrix::inverse(), and vpCDEBUG.

Referenced by vpBiclops::computeMGD(), vpBiclops::get_fMc(), and getDisplacement().

void vpBiclops::get_fMc	(	const vpColVector &	q,
		vpPoseVector &	fvc
	)		const

inherited

Compute the direct geometric model of the camera in terms of pose vector.

Parameters

q	: Articular position for pan and tilt axis.
fvc	: Pose vector corresponding to the transformation between the robot reference frame (called fixed) and the camera frame.

Definition at line 253 of file vpBiclops.cpp.

References vpPoseVector::buildFrom(), vpBiclops::get_fMc(), and vpHomogeneousMatrix::inverse().

vpHomogeneousMatrix vpBiclops::get_fMc ( const vpColVector & q ) const

inherited

Return the direct geometric model of the camera: fMc

Parameters

q	: Articular position for pan and tilt axis.

Returns: fMc, the homogeneous matrix corresponding to the direct geometric model of the camera. Discribes the transformation between the robot reference frame (called fixed) and the camera frame.

Definition at line 138 of file vpBiclops.cpp.

References vpBiclops::get_fMc().

vpHomogeneousMatrix vpBiclops::get_fMe ( const vpColVector & q ) const

inherited

Return the direct geometric model of the end effector: fMe

Parameters

q	: Articular position for pan and tilt axis.

Returns: fMe, the homogeneous matrix corresponding to the direct geometric model of the end effector. Describes the transformation between the robot reference frame (called fixed) and the end effector frame.

Definition at line 157 of file vpBiclops.cpp.

References vpBiclops::DH1, vpBiclops::dh_model_, vpException::dimensionError, vpArray2D< Type >::getRows(), and vpERROR_TRACE.

Referenced by vpBiclops::computeMGD(), and vpBiclops::get_fMc().

vpBiclops::DenavitHartenbergModel vpBiclops::getDenavitHartenbergModel ( ) const

inlineinherited

Return the Denavit Hartenberg representation used to model the head.

See also: vpBiclops::DenavitHartenbergModel

Definition at line 172 of file vpBiclops.h.

void vpRobotBiclops::getDisplacement	(	const vpRobot::vpControlFrameType	frame,
		vpColVector &	d
	)

virtual

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.

Parameters

frame	The frame in which the measured displacement is expressed.
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.

Exceptions

vpRobotException::wrongStateError If a not supported frame type is given.

Implements vpRobot.

Definition at line 1210 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpBiclops::get_fMc(), getPosition(), vpExponentialMap::inverse(), vpHomogeneousMatrix::inverse(), vpRobot::MIXT_FRAME, vpBiclops::ndof, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpRobotException::wrongStateError.

double vpRobot::getMaxRotationVelocity ( void ) const

inherited

Get the maximal rotation velocity that can be sent to the robot during a velocity control.

Returns: Maximum rotation velocity expressed in rad/s.

Definition at line 273 of file vpRobot.cpp.

References vpRobot::maxRotationVelocity.

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorViper850::computeArticularVelocity(), vpSimulatorAfma6::findHighestPositioningSpeed(), vpSimulatorViper850::findHighestPositioningSpeed(), vpSimulatorAfma6::setPosition(), vpRobotTemplate::setVelocity(), vpSimulatorCamera::setVelocity(), vpRobotCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpRobotPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotKinova::setVelocity(), vpRobotFlirPtu::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotFranka::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

double vpRobot::getMaxTranslationVelocity ( void ) const

inherited

Get the maximal translation velocity that can be sent to the robot during a velocity control.

Returns: Maximum translational velocity expressed in m/s.

Definition at line 251 of file vpRobot.cpp.

References vpRobot::maxTranslationVelocity.

void vpRobotBiclops::getPosition	(	const vpRobot::vpControlFrameType	frame,
		vpColVector &	q
	)

virtual

Return the position of each axis.

In positionning control mode, call vpRobotBiclopsController::getPosition()
In speed control mode, call vpRobotBiclopsController::getActualPosition()

Parameters

frame	: Control frame. This biclops head can only be controlled in articular.
q	: The position of the axis in radians.

Exceptions

vpRobotException::wrongStateError : If a not supported frame type is given.

Implements vpRobot.

Examples:: servoPioneerPanSegment3D.cpp.

Definition at line 829 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getRobotState(), vpRobot::MIXT_FRAME, vpBiclops::ndof, vpRobot::REFERENCE_FRAME, vpColVector::resize(), vpRobot::STATE_ACCELERATION_CONTROL, vpRobot::STATE_POSITION_CONTROL, vpRobot::STATE_STOP, vpRobot::STATE_VELOCITY_CONTROL, vpColVector::t(), vpCDEBUG, vpDEBUG_TRACE, and vpRobotException::wrongStateError.

Referenced by get_eJe(), get_fJe(), and getDisplacement().

vpColVector vpRobot::getPosition ( const vpRobot::vpControlFrameType frame )

inherited

Return the current robot position in the specified frame.

Definition at line 216 of file vpRobot.cpp.

References vpRobot::getPosition().

double vpRobotBiclops::getPositioningVelocity ( void )

Get the velocity in % used for a position control.

Returns: Positionning velocity in [0, 100.0]. The maximum positionning velocity is given vpBiclops::speedLimit.

Definition at line 698 of file vpRobotBiclops.cpp.

vpControlFrameType vpRobot::getRobotFrame ( void ) const

inlineprotectedinherited

Definition at line 172 of file vpRobot.h.

Referenced by vpSimulatorAfma6::computeArticularVelocity(), and vpSimulatorViper850::computeArticularVelocity().

virtual vpRobotStateType vpRobot::getRobotState ( void ) const

inlinevirtualinherited

Definition at line 144 of file vpRobot.h.

void vpRobotBiclops::getVelocity	(	const vpRobot::vpControlFrameType	frame,
		vpColVector &	q_dot
	)

Get the articular velocity.

Parameters

frame	: Control frame. This head can only be controlled in articular.
q_dot	: The measured articular velocity in rad/s.

Exceptions

vpRobotException::wrongStateError : If a not supported frame type is given.

Definition at line 1029 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getRobotState(), vpRobot::MIXT_FRAME, vpBiclops::ndof, vpRobot::REFERENCE_FRAME, vpColVector::resize(), vpRobot::STATE_ACCELERATION_CONTROL, vpRobot::STATE_POSITION_CONTROL, vpRobot::STATE_STOP, vpRobot::STATE_VELOCITY_CONTROL, vpColVector::t(), vpCDEBUG, vpDEBUG_TRACE, and vpRobotException::wrongStateError.

Referenced by getVelocity().

vpColVector vpRobotBiclops::getVelocity ( const vpRobot::vpControlFrameType frame )

Return the articular velocity.

Parameters

frame : Control frame. This head can only be controlled in articular.

Returns: The measured articular velocity in rad/s.

Exceptions

vpRobotException::wrongStateError : If a not supported frame type is given.

Definition at line 1103 of file vpRobotBiclops.cpp.

References getVelocity().

void vpRobotBiclops::init ( void )

virtual

Set the Biclops config filename. Check if the config file exists and initialize the head.

Exceptions

vpRobotException::constructionError If the config file cannot be oppened.

Implements vpRobot.

Definition at line 238 of file vpRobotBiclops.cpp.

References vpRobotException::constructionError, vpBiclops::ndof, vpColVector::resize(), setRobotState(), vpRobot::STATE_STOP, vpCERROR, and vpERROR_TRACE.

Referenced by vpRobotBiclops().

bool vpRobotBiclops::readPositionFile	(	const std::string &	filename,
		vpColVector &	q
	)

Get an articular position from the position file.

Parameters

filename	: Position file.
q	: The articular position read in the file.

# Example of biclops position file
# The axis positions must be preceed by R:
# First value : pan  articular position in degrees
# Second value: tilt articular position in degrees
R: 15.0 5.0

Returns: true if a position was found, false otherwise.

Definition at line 1130 of file vpRobotBiclops.cpp.

References vpColVector::deg2rad(), vpBiclops::ndof, vpColVector::resize(), and vpIoTools::splitChain().

Referenced by setPosition().

vpColVector vpRobot::saturateVelocities	(	const vpColVector &	v_in,
		const vpColVector &	v_max,
		bool	verbose = `false`
	)

staticinherited

Saturate velocities.

Parameters

v_in	: Vector of input velocities to saturate. Translation velocities should be expressed in m/s while rotation velocities in rad/s.
v_max	: Vector of maximal allowed velocities. Maximal translation velocities should be expressed in m/s while maximal rotation velocities in rad/s.
verbose	: Print a message indicating which axis causes the saturation.

Returns: Saturated velocities.

Exceptions

vpRobotException::dimensionError : If the input vectors have different dimensions.

The code below shows how to use this static method in order to saturate a velocity skew vector.

#include <iostream>
#include <visp3/robot/vpRobot.h>
int main()
{
  // Set a velocity skew vector
  vpColVector v(6);
  v[0] = 0.1;               // vx in m/s
  v[1] = 0.2;               // vy
  v[2] = 0.3;               // vz
  v[3] = vpMath::rad(10);   // wx in rad/s
  v[4] = vpMath::rad(-10);  // wy
  v[5] = vpMath::rad(20);   // wz
  // Set the maximal allowed velocities
  vpColVector v_max(6);
  for (int i=0; i<3; i++)
    v_max[i] = 0.3;             // in translation (m/s)
  for (int i=3; i<6; i++)
    v_max[i] = vpMath::rad(10); // in rotation (rad/s)
  // Compute the saturated velocity skew vector
  vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
  std::cout << "v    : " << v.t() << std::endl;
  std::cout << "v max: " << v_max.t() << std::endl;
  std::cout << "v sat: " << v_sat.t() << std::endl;
  return 0;
}

Definition at line 163 of file vpRobot.cpp.

References vpException::dimensionError, and vpArray2D< Type >::size().

Referenced by vpRobotTemplate::setVelocity(), vpSimulatorCamera::setVelocity(), vpRobotCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpRobotPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotKinova::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotFranka::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

void vpBiclops::set_cMe ( )

inherited

Set the default homogeneous matrix corresponding to the transformation between the camera frame and the end effector frame. The end effector frame is located on the tilt axis.

Definition at line 321 of file vpBiclops.cpp.

References vpBiclops::cMe_, vpBiclops::h, and vpHomogeneousMatrix::inverse().

Referenced by vpBiclops::init().

void vpBiclops::set_cMe ( const vpHomogeneousMatrix & cMe )

inlineinherited

Set the transformation between the camera frame and the end effector frame.

Definition at line 179 of file vpBiclops.h.

void vpRobotBiclops::setConfigFile ( const std::string & filename = "/usr/share/BiclopsDefault.cfg" )

Set the Biclops config filename.

Definition at line 227 of file vpRobotBiclops.cpp.

Referenced by vpRobotBiclops().

void vpBiclops::setDenavitHartenbergModel ( vpBiclops::DenavitHartenbergModel m = vpBiclops::DH1 )

inlineinherited

Set the Denavit Hartenberg representation used to model the head.

See also: vpBiclops::DenavitHartenbergModel

Examples:: servoPioneerPanSegment3D.cpp.

Definition at line 185 of file vpBiclops.h.

void vpRobot::setMaxRotationVelocity ( double w_max )

inherited

Set the maximal rotation velocity that can be sent to the robot during a velocity control.

Parameters

w_max : Maximum rotational velocity expressed in rad/s.

Examples:: servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, testFeatureSegment.cpp, testFrankaJointVelocityLimits.cpp, and testRobotFlirPtu.cpp.

Definition at line 260 of file vpRobot.cpp.

References vpRobot::maxRotationVelocity.

Referenced by vpRobotViper650::getControlMode(), vpRobotViper850::getControlMode(), vpRobotViper650::setMaxRotationVelocity(), vpRobotViper850::setMaxRotationVelocity(), vpSimulatorAfma6::setPosition(), vpRobotCamera::vpRobotCamera(), and vpSimulatorCamera::vpSimulatorCamera().

void vpRobot::setMaxTranslationVelocity ( double v_max )

inherited

Set the maximal translation velocity that can be sent to the robot during a velocity control.

Parameters

v_max : Maximum translation velocity expressed in m/s.

Examples:: servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, and testFeatureSegment.cpp.

Definition at line 239 of file vpRobot.cpp.

References vpRobot::maxTranslationVelocity.

Referenced by vpSimulatorAfma6::setPosition(), vpRobotCamera::vpRobotCamera(), and vpSimulatorCamera::vpSimulatorCamera().

void vpRobotBiclops::setPosition	(	const vpRobot::vpControlFrameType	frame,
		const vpColVector &	q
	)

virtual

Move the robot in position control.

Warning: This method is blocking. That mean that it waits the end of the positionning.

Parameters

frame	: Control frame. This biclops head can only be controlled in articular.
q	: The position to set for each axis in radians.

Exceptions

vpRobotException::wrongStateError : If a not supported frame type is given.

Implements vpRobot.

Examples:: servoPioneerPanSegment3D.cpp.

Definition at line 715 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getRobotState(), vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, setRobotState(), vpRobot::STATE_POSITION_CONTROL, vpDEBUG_TRACE, vpERROR_TRACE, and vpRobotException::wrongStateError.

Referenced by setPosition().

void vpRobotBiclops::setPosition	(	const vpRobot::vpControlFrameType	frame,
		const double &	q1,
		const double &	q2
	)

Move the robot in position control.

Warning: This method is blocking. That mean that it wait the end of the positionning.

Parameters

frame	: Control frame. This biclops head can only be controlled in articular.
q1	: The pan position to set in radians.
q2	: The tilt position to set in radians.

Exceptions

vpRobotException::wrongStateError : If a not supported frame type is given.

Definition at line 777 of file vpRobotBiclops.cpp.

References setPosition(), and vpERROR_TRACE.

void vpRobotBiclops::setPosition ( const char * filename )

Read the content of the position file and moves to head to articular position.

Parameters

filename : Position filename

Exceptions

vpRobotException::readingParametersError : If the articular position cannot be read from file.

See also: readPositionFile()

Definition at line 804 of file vpRobotBiclops.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobotException::readingParametersError, readPositionFile(), setPosition(), and vpERROR_TRACE.

void vpRobotBiclops::setPositioningVelocity ( double velocity )

Set the velocity used for a position control.

Parameters

velocity : Velocity in % of the maximum velocity between [0,100]. The maximum velocity is given vpBiclops::speedLimit.

Definition at line 682 of file vpRobotBiclops.cpp.

References vpRobotException::constructionError, and vpERROR_TRACE.

vpRobot::vpControlFrameType vpRobot::setRobotFrame ( vpRobot::vpControlFrameType newFrame )

protectedinherited

Definition at line 207 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::init(), vpSimulatorViper850::init(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpSimulatorAfma6::setVelocity(), and vpSimulatorViper850::setVelocity().

vpRobot::vpRobotStateType vpRobotBiclops::setRobotState ( const vpRobot::vpRobotStateType newState )

virtual

Change the state of the robot either to stop them, or to set position or speed control.

Reimplemented from vpRobot.

Examples:: servoPioneerPanSegment3D.cpp.

Definition at line 544 of file vpRobotBiclops.cpp.

References vpRobot::getRobotState(), vpRobot::setRobotState(), vpRobot::STATE_POSITION_CONTROL, vpRobot::STATE_STOP, vpRobot::STATE_VELOCITY_CONTROL, stopMotion(), vpCERROR, vpDEBUG_TRACE, and vpRobotBiclopsSpeedControlLoop().

Referenced by init(), setPosition(), and ~vpRobotBiclops().

void vpRobotBiclops::setVelocity	(	const vpRobot::vpControlFrameType	frame,
		const vpColVector &	q_dot
	)

virtual

Send a velocity on each axis.

Parameters

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), end-effector frame (vpRobot::END_EFFECTOR_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are not implemented.
q_dot	: The desired articular velocity of the axis in rad/s. $\dot {r} = [\dot{q}_1, \dot{q}_2]^t$ with $\dot{q}_1$ the pan of the camera and $\dot{q}_2$ the tilt of the camera.

Exceptions

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().
vpRobotException::wrongStateError	: If a not supported frame type (vpRobot::CAMERA_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::END_EFFECTOR_FRAME or vpRobot::MIXT_FRAME) is given.