ViSP  2.10.0
Public Types | Public Member Functions | Static Public Member Functions | Static Public Attributes | Protected Member Functions | Protected Attributes | Static Protected Attributes | List of all members
vpRobotAfma4 Class Reference
ViSP capabilities » Working with robots » Real robots » Afma4 cylindrical robotViSP capabilities » Drivers » | Robot drivers

#include <vpRobotAfma4.h>

+ Inheritance diagram for vpRobotAfma4:

Public Types

enum  vpRobotStateType { STATE_STOP, STATE_VELOCITY_CONTROL, STATE_POSITION_CONTROL, STATE_ACCELERATION_CONTROL }
 
enum  vpControlFrameType { REFERENCE_FRAME, ARTICULAR_FRAME, CAMERA_FRAME, MIXT_FRAME }
 

Public Member Functions

 vpRobotAfma4 (bool verbose=true)
 
virtual ~vpRobotAfma4 (void)
 
void getDisplacement (vpRobot::vpControlFrameType frame, vpColVector &displacement)
 
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position)
 
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position, double &timestamp)
 
double getPositioningVelocity (void)
 
bool getPowerState ()
 
double getTime () const
 
void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &velocity)
 
void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &velocity, double &timestamp)
 
vpColVector getVelocity (const vpRobot::vpControlFrameType frame)
 
vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double &timestamp)
 
void get_cMe (vpHomogeneousMatrix &cMe) const
 
void get_cVe (vpVelocityTwistMatrix &cVe) const
 
void get_cVf (vpVelocityTwistMatrix &cVf) const
 
void get_eJe (vpMatrix &eJe)
 
void get_fJe (vpMatrix &fJe)
 
void init (void)
 
void move (const char *filename)
 
void powerOn ()
 
void powerOff ()
 
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &position)
 
void setPosition (const vpRobot::vpControlFrameType frame, const double q1, const double q2, const double q4, const double q5)
 
void setPosition (const char *filename)
 
void setPositioningVelocity (const double velocity)
 
vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState)
 
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
 
void stopMotion ()
 
vpHomogeneousMatrix getForwardKinematics (const vpColVector &q) const
 
vpHomogeneousMatrix get_fMc (const vpColVector &q) const
 
void get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
 
void get_fMe (const vpColVector &q, vpHomogeneousMatrix &fMe) const
 
void get_cVf (const vpColVector &q, vpVelocityTwistMatrix &cVf) const
 
void get_eJe (const vpColVector &q, vpMatrix &eJe) const
 
void get_fJe (const vpColVector &q, vpMatrix &fJe) const
 
void get_fJe_inverse (const vpColVector &q, vpMatrix &fJe_inverse) const
 
vpColVector getJointMin () const
 
vpColVector getJointMax () const
 
double getMaxTranslationVelocity (void) const
 
double getMaxRotationVelocity (void) const
 
vpColVector getPosition (const vpRobot::vpControlFrameType frame)
 
virtual vpRobotStateType getRobotState (void) const
 
void setMaxRotationVelocity (const double maxVr)
 
void setMaxTranslationVelocity (const double maxVt)
 
void setVerbose (bool verbose)
 

Static Public Member Functions

static bool readPosFile (const char *filename, vpColVector &q)
 
static bool savePosFile (const char *filename, const vpColVector &q)
 
static vpColVector saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
 

Static Public Attributes

static const double defaultPositioningVelocity = 15.0
 
static const unsigned int njoint = 4
 

Protected Member Functions

vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame)
 
vpControlFrameType getRobotFrame (void) const
 

Protected Attributes

double _a1
 
double _d3
 
double _d4
 
double _joint_max [4]
 
double _joint_min [4]
 
vpTranslationVector _etc
 
vpRxyzVector _erc
 
vpHomogeneousMatrix _eMc
 
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

Control of Irisa's cylindrical robot named Afma4.

Implementation of the vpRobot class in order to control Irisa's Afma4 robot. This robot is a cylindrical robot with five degrees of freedom but only four are actuated (see vpAfma4 documentation). It was manufactured in 1995 by the french Afma-Robots company. In 2008, the low level controller change for a more recent Adept technology based on the MotionBlox controller. A firewire camera is mounted on the end-effector to allow eye-in-hand visual servoing. The control of this camera is achieved by the vp1394TwoGrabber class. A Servolens lens is attached to the camera. It allows to control the focal lens, the iris and the focus throw a serial link. The control of the lens is possible using the vpServolens class.

This class allows to control the Afma4 cylindrical robot in position and velocity:

Mixed frame (vpRobot::MIXT_FRAME) where translations are expressed in the reference frame and rotations in the camera frame is not implemented.

All the translations are expressed in meters for positions and m/s for the velocities. Rotations are expressed in radians for the positions, and rad/s for the rotation velocities.

The Denavit-Hartenberg representation as well as the direct and inverse kinematics models are given and implemented in the vpAfma4 class.

Warning
A Ctrl-C, a segmentation fault or other system errors are catched by this class to stop the robot.

To communicate with the robot, you may first create an instance of this class by calling the default constructor:

vpRobotAfma4 robot;

This initialize the robot kinematics with the eMc extrinsic camera parameters.

To control the robot in position, you may set the controller to position control and than send the position to reach in a specific frame like here in the joint space:

vpColVector q(4);
// Set a joint position
q[0] = M_PI/2; // X axis, in radian
q[1] = 0.2; // Y axis, in meter
q[2] = -M_PI/2; // A axis, in radian
q[3] = M_PI/8; // B axis, in radian
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);

The robot moves to the specified position with the default positioning velocity vpRobotAfma4::defaultPositioningVelocity. The setPositioningVelocity() method allows to change the maximal velocity used to reach the desired position.

// Set the max velocity to 40%
robot.setPositioningVelocity(40);
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);

To control the robot in velocity, you may set the controller to velocity control and than send the velocities. To end the velocity control and stop the robot you have to set the controller to the stop state. Here is an example of a velocity control in the joint space:

vpColVector qvel(6);
// Set a joint velocity
qvel[0] = M_PI/8; // X axis, in rad/s
qvel[1] = 0.2; // Y axis, in m/s
qvel[2] = 0; // A axis, in rad/s
qvel[3] = M_PI/8; // B axis, in rad/s
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
while (...) {
// Apply a velocity in the joint space
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qvel);
// Compute new velocities qvel...
}
// Stop the robot
robot.setRobotState(vpRobot::STATE_STOP)

There is also possible to measure the robot current position with getPosition() method and the robot current velocities with the getVelocity() method.

For convenience, there is also the ability to read/write joint positions from a position file with readPosFile() and writePosFile() methods.

Examples:
moveAfma4.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.cpp, servoAfma4Point2DCamVelocityKalman.cpp, and testRobotAfma4.cpp.

Definition at line 181 of file vpRobotAfma4.h.

Member Enumeration Documentation

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 space.

CAMERA_FRAME 

Corresponds to a frame attached to the camera mounted on the robot end-effector.

MIXT_FRAME 

Corresponds to a "virtual" frame where translations are expressed in the reference frame, and rotations in the camera frame.

Definition at line 78 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.

Definition at line 66 of file vpRobot.h.

Constructor & Destructor Documentation

vpRobotAfma4::vpRobotAfma4 ( bool  verbose = true)

The only available constructor.

This contructor calls init() to initialise the connection with the MotionBox or low level controller, power on the robot and wait 1 sec before returning to be sure the initialisation is done.

It also set the robot state to vpRobot::STATE_STOP.

Definition at line 134 of file vpRobotAfma4.cpp.

References defaultPositioningVelocity, init(), setRobotState(), vpRobot::setVerbose(), vpRobot::STATE_STOP, and vpRobot::verbose_.

vpRobotAfma4::~vpRobotAfma4 ( void  )
virtual

Destructor.

Free allocated resources.

Definition at line 301 of file vpRobotAfma4.cpp.

References setRobotState(), and vpRobot::STATE_STOP.

Member Function Documentation

void vpRobotAfma4::get_cMe ( vpHomogeneousMatrix &  cMe) const

Get the geometric transformation between the camera frame and the end-effector frame. This transformation is constant and correspond to the extrinsic camera parameters estimated by calibration.

Parameters
cMe: Transformation between the camera frame and the end-effector frame.

Definition at line 607 of file vpRobotAfma4.cpp.

References vpAfma4::get_cMe().

void vpRobotAfma4::get_cVe ( vpVelocityTwistMatrix &  cVe) const

Get the twist transformation from camera frame to end-effector frame. This transformation allows to compute a velocity expressed in the end-effector frame into the camera frame.

Parameters
cVe: Twist transformation.

Definition at line 557 of file vpRobotAfma4.cpp.

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

void vpAfma4::get_cVf ( const vpColVector &  q,
vpVelocityTwistMatrix &  cVf 
) const
inherited

Get the twist transformation from camera frame to the reference frame. This transformation allows to compute a velocity expressed in the reference frame into the camera frame.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
cVf: Twist transformation.

Definition at line 368 of file vpAfma4.cpp.

References vpVelocityTwistMatrix::buildFrom(), vpAfma4::get_fMc(), and vpHomogeneousMatrix::inverse().

Referenced by get_cVf().

void vpRobotAfma4::get_cVf ( vpVelocityTwistMatrix &  cVf) const

Get the twist transformation from camera frame to the reference frame. This transformation allows to compute a velocity expressed in the reference frame into the camera frame.

Parameters
cVf: Twist transformation.

Definition at line 575 of file vpRobotAfma4.cpp.

References vpAfma4::get_cVf(), vpAfma4::njoint, and vpERROR_TRACE.

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

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

\[ {^e}J_e = \left[\begin{array}{cccc} -c_5(a_1c_4+d_3s_4) & -s_5 & 0 & 0 \\ s_5(a_1c_4+d_3s_4) & -c_5 & 0 & 0 \\ a_1s_4-d_3c_4 & 0 & 0 & 0 \\ -s_5 & 0 & -s_5 & 0 \\ -c_5 & 0 & -c_5 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
eJe: Robot jacobian expressed in the end-effector frame, with:

\[ {^e}J_e = \left[\begin{array}{cc} {^f}R_e^T & 0_{3 \times 3} \\ 0_{3 \times 3} & {^f}R_e^T \\ \end{array} \right] {^f}J_e \]

See also
get_fJe()

Definition at line 416 of file vpAfma4.cpp.

References vpAfma4::_a1, vpAfma4::_d3, and vpMatrix::resize().

Referenced by get_eJe().

void vpRobotAfma4::get_eJe ( vpMatrix &  eJe)
virtual

Get the robot jacobian expressed in the end-effector frame. To have acces to the analytic form of this jacobian see vpAfma4::get_eJe().

To compute eJe, we communicate with the low level controller to get the articular joint position of the robot.

Parameters
eJe: Robot jacobian expressed in the end-effector frame.
See also
vpAfma4::get_eJe()

Implements vpRobot.

Definition at line 625 of file vpRobotAfma4.cpp.

References vpAfma4::get_eJe(), vpAfma4::njoint, and vpERROR_TRACE.

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

Get the robot jacobian expressed in the robot reference frame also called fix frame:

\[ {^f}J_e = \left[\begin{array}{cccc} -a_1s_1-d_3c_1 & 0 & 0 & 0 \\ a_1c_1-d_3s_1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & c_{14} \\ 0 & 0 & 0 & s_{14} \\ 1 & 0 & 1 & 0 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fJe: Robot jacobian expressed in the robot reference frame.
See also
get_eJe() and get_fJe_inverse()

Definition at line 470 of file vpAfma4.cpp.

References vpAfma4::_a1, vpAfma4::_d3, and vpMatrix::resize().

Referenced by get_fJe().

void vpRobotAfma4::get_fJe ( vpMatrix &  fJe)
virtual

Get the robot jacobian expressed in the robot reference frame also called fix frame. To have acces to the analytic form of this jacobian see vpAfma4::get_fJe().

To compute fJe, we communicate with the low level controller to get the articular joint position of the robot.

Parameters
fJe: Robot jacobian expressed in the reference frame.
See also
vpAfma4::get_fJe()

Implements vpRobot.

Definition at line 662 of file vpRobotAfma4.cpp.

References vpAfma4::get_fJe(), vpAfma4::njoint, and vpERROR_TRACE.

void vpAfma4::get_fJe_inverse ( const vpColVector &  q,
vpMatrix &  fJe_inverse 
) const
inherited

Get the inverse jacobian.

\[ {^f}J_e^+ = \left[\begin{array}{cccccc} -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\ 0 & 0 & 0 & c_{14} & s_{14} & 0 \\ \end{array} \right] \]

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fJe_inverse: Inverse robot jacobian expressed in the robot reference frame.
See also
get_eJe() and get_fJe()

Definition at line 526 of file vpAfma4.cpp.

References vpAfma4::_a1, vpAfma4::_d3, and vpMatrix::resize().

Referenced by setVelocity().

vpHomogeneousMatrix vpAfma4::get_fMc ( const vpColVector &  q) const
inherited

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

This method is the same than getForwardKinematics(const vpColVector & q).

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
Returns
The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

See also
getForwardKinematics(const vpColVector & q)

Definition at line 190 of file vpAfma4.cpp.

Referenced by vpAfma4::get_cVf(), vpAfma4::getForwardKinematics(), getPosition(), and getVelocity().

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

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fMc: The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

Definition at line 225 of file vpAfma4.cpp.

References vpAfma4::_eMc, and vpAfma4::get_fMe().

void vpAfma4::get_fMe ( const vpColVector &  q,
vpHomogeneousMatrix &  fMe 
) const
inherited

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the end effector with respect to the base frame given the articular positions of all the four variable joints.

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
fMeThe homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the end effector frame ( ${^f}M_e$) with

\[ {^f}M_e = \left[\begin{array}{cccc} c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\ -s_5 & -c_5 & d_4+q_2 \\ 0 & 0 & 0 & 1 \\ \end{array} \right] \]

Definition at line 273 of file vpAfma4.cpp.

References vpAfma4::_a1, vpAfma4::_d3, and vpAfma4::_d4.

Referenced by vpAfma4::get_fMc(), and setVelocity().

void vpRobotAfma4::getDisplacement ( vpRobot::vpControlFrameType  frame,
vpColVector &  displacement 
)
virtual

Get the robot displacement since the last call of this method.

Warning
This functionnality is not implemented for the moment in the cartesian space. It is only available in the joint space (vpRobot::ARTICULAR_FRAME).
Parameters
frame: The frame in which the measured displacement is expressed.
displacement: The measured displacement since the last call of this method. The dimension of displacement is always 4, the number of joints. Translations are expressed in meters, rotations in radians.

In camera or reference frame, rotations are expressed with the Euler Rxyz representation.

See also
getArticularDisplacement(), getCameraDisplacement()

Implements vpRobot.

Definition at line 1801 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma4::njoint, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpERROR_TRACE.

vpHomogeneousMatrix vpAfma4::getForwardKinematics ( const vpColVector &  q) const
inherited

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the articular positions of all the four joints.

This method is the same than get_fMc(const vpColVector & q).

Parameters
q: Articular position of the four joints: q[0] corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q[1] corresponds to the vertical translation (joint 2 with value $q_2$), while q[2] and q[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q[0], q[2] and q[3] are expressed in radians. The translation q[1] is expressed in meters.
Returns
The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame ( ${^f}M_c$) with:

\[ {^f}M_c = {^f}M_e * {^e}M_c \]

See also
get_fMc(const vpColVector & q)
getInverseKinematics()

Definition at line 152 of file vpAfma4.cpp.

References vpAfma4::get_fMc().

vpColVector vpAfma4::getJointMax ( ) const
inherited

Get max joint values.

Returns
Maximal joint values for the 4 dof X, Y, A, B. Translation Y is expressed in meters. Rotations X, A and B in radians.

Definition at line 579 of file vpAfma4.cpp.

References vpAfma4::_joint_max.

vpColVector vpAfma4::getJointMin ( ) const
inherited

Get min joint values.

Returns
Minimal joint values for the 4 dof X, Y, A, B. Translation Y is expressed in meters. Rotations X,A and B in radians.

Definition at line 562 of file vpAfma4.cpp.

References vpAfma4::_joint_min.

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 279 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorViper850::computeArticularVelocity(), vpSimulatorAfma6::findHighestPositioningSpeed(), vpSimulatorViper850::findHighestPositioningSpeed(), vpSimulatorAfma6::setPosition(), vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpRobotPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), setVelocity(), vpRobotAfma6::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 translation velocity expressed in m/s.

Definition at line 254 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::setPosition(), vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpRobotPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

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

Return the current robot position in the specified frame.

Definition at line 222 of file vpRobot.cpp.

void vpRobotAfma4::getPosition ( const vpRobot::vpControlFrameType  frame,
vpColVector &  position 
)
virtual

Get the current position of the robot.

Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).

The difference is here that the timestamp is not used.

Implements vpRobot.

Examples:
moveAfma4.cpp.

Definition at line 1130 of file vpRobotAfma4.cpp.

Referenced by setVelocity().

void vpRobotAfma4::getPosition ( const vpRobot::vpControlFrameType  frame,
vpColVector &  position,
double &  timestamp 
)

Get the current position of the robot.

Parameters
frame: Control frame type in which to get the position, either :
  • in the camera cartesien frame,
  • joint (articular) coordinates of each axes
  • in a reference or fixed cartesien frame attached to the robot base
  • in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
position: Measured position of the robot:
  • in camera cartesien frame, a 6 dimension vector, set to 0.
  • in articular, a 4 dimension vector corresponding to the joint position of each dof. position[0] corresponds to the first rotation of the turret around the vertical axis (joint 1 with value $q_1$), while position[1] corresponds to the vertical translation (joint 2 with value $q_2$), while position[2] and position[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations position[0], position[2] and position[3] are expressed in radians. The translation q[1] is expressed in meters.
  • in reference frame, a 6 dimension vector, the first 3 values correspond to the translation tx, ty, tz in meters (like a vpTranslationVector), and the last 3 values to the rx, ry, rz rotation (like a vpRxyzVector). The code below show how to convert this position into a vpHomogenousMatrix:
timestamp: Time in second since last robot power on.
vpRobotAfma4 robot;
vpColVector r;
robot.getPosition(vpRobot::REFERENCE_FRAME, r);
vpTranslationVector ftc; // reference frame to camera frame translations
vpRxyzVector frc; // reference frame to camera frame rotations
// Update the transformation between reference frame and camera frame
for (int i=0; i < 3; i++) {
ftc[i] = position[i]; // tx, ty, tz
frc[i] = position[i+3]; // ry, ry, rz
}
// Create a rotation matrix from the Rxyz rotation angles
vpRotationMatrix fRc(frc); // reference frame to camera frame rotation matrix
// Create the camera to fix frame pose in terms of a homogenous matrix
vpHomogeneousMatrix fMc(fRc, ftc);
Exceptions
vpRobotException::lowLevelError: If the position cannot be get from the low level controller.
See also
setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)

Definition at line 1056 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, vpRxyzVector::buildFrom(), vpRobot::CAMERA_FRAME, vpHomogeneousMatrix::extract(), vpAfma4::get_fMc(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma4::njoint, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpERROR_TRACE.

double vpRobotAfma4::getPositioningVelocity ( void  )

Get the maximal velocity percentage used for a position control.

See also
setPositioningVelocity()

Definition at line 723 of file vpRobotAfma4.cpp.

bool vpRobotAfma4::getPowerState ( void  )

Get the robot power state indication if power is on or off.

Returns
true if power is on. false if power is off
Exceptions
vpRobotException::lowLevelError: If the low level controller returns an error.
See also
powerOn(), powerOff()

Definition at line 524 of file vpRobotAfma4.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

vpControlFrameType vpRobot::getRobotFrame ( void  ) const
inlineprotectedinherited

Definition at line 162 of file vpRobot.h.

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

virtual vpRobotStateType vpRobot::getRobotState ( void  ) const
inlinevirtualinherited

Definition at line 140 of file vpRobot.h.

Referenced by vpRobotBiclops::getPosition(), vpRobotBiclops::getVelocity(), vpSimulatorCamera::setPosition(), vpRobotCamera::setPosition(), vpRobotPtu46::setPosition(), vpRobotBiclops::setPosition(), vpSimulatorAfma6::setPosition(), vpSimulatorViper850::setPosition(), setPosition(), vpRobotAfma6::setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotPtu46::setRobotState(), vpRobotBiclops::setRobotState(), vpSimulatorAfma6::setRobotState(), vpSimulatorViper850::setRobotState(), setRobotState(), vpRobotAfma6::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotPtu46::setVelocity(), vpRobotBiclops::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpSimulatorAfma6::stopMotion(), vpSimulatorViper850::stopMotion(), vpRobotViper650::stopMotion(), and vpRobotViper850::stopMotion().

double vpRobotAfma4::getTime ( ) const

Returns the robot controller current time (in second) since last robot power on.

Definition at line 989 of file vpRobotAfma4.cpp.

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

Get robot velocities.

The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &) except that the timestamp is not returned.

Examples:
servoAfma4Point2DCamVelocityKalman.cpp.

Definition at line 1498 of file vpRobotAfma4.cpp.

Referenced by getVelocity().

void vpRobotAfma4::getVelocity ( const vpRobot::vpControlFrameType  frame,
vpColVector &  velocity,
double &  timestamp 
)

Get the robot velocities.

Parameters
frame: Frame in wich velocities are mesured.
velocity: Measured velocities. Translations are expressed in m/s and rotations in rad/s.
timestamp: Time in second since last robot power on.
Warning
In camera frame, reference frame and mixt frame, the representation of the rotation is ThetaU. In that cases, $velocity = [\dot x, \dot y, \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]$.
The first time this method is called, velocity is set to 0. The first call is used to intialise the velocity computation for the next call.
// Set joint velocities
vpColVector q_dot(4);
q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
vpColVector q_dot_mes; // Measured velocities
// Initialisation of the velocity measurement
robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
// q_dot_mes is resized to 4, the number of joint
double timestamp;
while (1) {
robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes, timestamp);
vpTime::wait(40); // wait 40 ms
// here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
}

Definition at line 1392 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpAfma4::get_fMc(), vpExponentialMap::inverse(), vpHomogeneousMatrix::inverse(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma4::njoint, vpRobot::REFERENCE_FRAME, vpColVector::resize(), vpERROR_TRACE, and vpRobotException::wrongStateError.

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

Get robot velocities.

The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &) except that the timestamp is not returned.

Definition at line 1563 of file vpRobotAfma4.cpp.

References getVelocity().

vpColVector vpRobotAfma4::getVelocity ( const vpRobot::vpControlFrameType  frame,
double &  timestamp 
)

Get the robot velocities.

Parameters
frame: Frame in wich velocities are mesured.
timestamp: Time in second since last robot power on.
Returns
Measured velocities. Translations are expressed in m/s and rotations in rad/s.
// Set joint velocities
vpColVector q_dot(4);
q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
q_dot[3] = M_PI/16; // Joint 5 velocity, in rad/s
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
// Initialisation of the velocity measurement
robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes); // q_dot_mes =0
// q_dot_mes is resized to 4, the number of joint
vpColVector q_dot_mes; // Measured velocities
double timestamp;
while (1) {
q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME, timestamp);
vpTime::wait(40); // wait 40 ms
// here q_dot_mes is equal to [M_PI/8, 0.2, M_PI/4, M_PI/16]
}

Definition at line 1547 of file vpRobotAfma4.cpp.

References getVelocity().

void vpRobotAfma4::init ( void  )
virtual

Initialise the connection with the MotionBox or low level controller, power on the robot and wait 1 sec before returning to be sure the initialisation is done.

Implements vpRobot.

Definition at line 197 of file vpRobotAfma4.cpp.

References vpAfma4::_joint_max, vpAfma4::_joint_min, vpRobotException::constructionError, vpColVector::resize(), vpRobot::verbose_, and vpERROR_TRACE.

Referenced by vpRobotAfma4().

void vpRobotAfma4::move ( const char *  filename)

Moves the robot to the joint position specified in the filename. The positioning velocity is set to 10% of the robot maximal velocity.

Parameters
filenameFile containing a joint position.
See also
readPosFile

Definition at line 1735 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, readPosFile(), setPosition(), setPositioningVelocity(), setRobotState(), and vpRobot::STATE_POSITION_CONTROL.

void vpRobotAfma4::powerOff ( void  )

Power off the robot.

Exceptions
vpRobotException::lowLevelError: If the low level controller returns an error during robot stopping.
See also
powerOn(), getPowerState()

Definition at line 487 of file vpRobotAfma4.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

void vpRobotAfma4::powerOn ( void  )

Power on the robot.

Exceptions
vpRobotException::lowLevelError: If the low level controller returns an error during robot power on.
See also
powerOff(), getPowerState()

Definition at line 415 of file vpRobotAfma4.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

Referenced by setRobotState().

bool vpRobotAfma4::readPosFile ( const char *  filename,
vpColVector &  q 
)
static

Read joint positions in a specific Afma4 position file.

This position file has to start with a header. The six joint positions are given after the "R:" keyword. The first 3 values correspond to the joint translations X,Y,Z expressed in meters. The 3 last values correspond to the joint rotations A,B,C expressed in degres to be more representative for the user. Theses values are then converted in radians in q. The character "#" starting a line indicates a comment.

A typical content of such a file is given below:

#AFMA4 - Position - Version 2.01
# file: "myposition.pos "
#
# R: X Y A B
# Joint position: X : rotation of the turret in degrees (joint 1)
# Y : vertical translation in meters (joint 2)
# A : pan rotation of the camera in degrees (joint 4)
# B : tilt rotation of the camera in degrees (joint 5)
#
R: 45 0.3 -20 30
Parameters
filename: Name of the position file to read.
q: Joint positions: X,Y,A,B. Translations Y is expressed in meters, while joint rotations X,A,B in radians.
Returns
true if the positions were successfully readen in the file. false, if an error occurs.

The code below shows how to read a position from a file and move the robot to this position.

vpRobotAfma4 robot;
vpColVector q; // Joint position
robot.readPosFile("myposition.pos", q); // Set the joint position from the file
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
robot.setPositioningVelocity(5); // Positioning velocity set to 5%
robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
See also
savePosFile()

Definition at line 1623 of file vpRobotAfma4.cpp.

References vpMath::rad(), and vpColVector::resize().

Referenced by move(), and 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 <visp/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 167 of file vpRobot.cpp.

References vpException::dimensionError, and vpColVector::size().

Referenced by vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpRobotPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

bool vpRobotAfma4::savePosFile ( const char *  filename,
const vpColVector &  q 
)
static

Save joint (articular) positions in a specific Afma4 position file.

This position file starts with a header on the first line. After convertion of the rotations in degrees, the joint position q is written on a line starting with the keyword "R: ". See readPosFile() documentation for an example of such a file.

Parameters
filename: Name of the position file to create.
q: Joint positions: X,Y,A,B. Translations Y is expressed in meters, while joint rotations X,A,B in radians.
Warning
The joint rotations X,A,B written in the file are converted in degrees to be more representative for the user.
Returns
true if the positions were successfully saved in the file. false, if an error occurs.
See also
readPosFile()

Definition at line 1695 of file vpRobotAfma4.cpp.

References vpMath::deg().

void vpRobot::setMaxRotationVelocity ( const double  w_max)
inherited

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

Parameters
w_max: Maximum rotation velocity expressed in rad/s.
Examples:
servoMomentPoints.cpp, servoSimu4Points.cpp, and servoSimuSphere.cpp.

Definition at line 266 of file vpRobot.cpp.

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

void vpRobot::setMaxTranslationVelocity ( const 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, and simulateFourPoints2DPolarCamVelocity.cpp.

Definition at line 242 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::setPosition().

void vpRobotAfma4::setPosition ( const vpRobot::vpControlFrameType  frame,
const vpColVector &  position 
)
virtual

Move to an absolute position with a given percent of max velocity. The percent of max velocity is to set with setPositioningVelocity().

Warning
The position to reach can only be specified in joint space coordinates.

This method owerloads setPosition(const vpRobot::vpControlFrameType, const vpColVector &).

Warning
This method is blocking. It returns only when the position is reached by the robot.
Parameters
position: The joint positions to reach. position[0] corresponds to the first rotation of the turret around the vertical axis (joint 1 with value $q_1$), while position[1] corresponds to the vertical translation (joint 2 with value $q_2$), while position[2] and position[3] correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations position[0], position[2] and position[3] are expressed in radians. The translation q[1] is expressed in meters.
frame: Frame in which the position is expressed.
Exceptions
vpRobotException::lowLevelError: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
// Set positions in the joint space
vpColVector q[4];
double q[0] = M_PI/8; // Joint 1, in radian
double q[1] = 0.2; // Joint 2, in meter
double q[2] = M_PI/4; // Joint 4, in radian
double q[3] = M_PI/8; // Joint 5, in radian
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Set the max velocity to 20%
robot.setPositioningVelocity(20);
// Moves the robot in the camera frame
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
Exceptions
vpRobotException::lowLevelError: If the requested frame (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, or vpRobot::MIXT_FRAME) are requested since they are not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.

To catch the exception if the position is out of range, modify the code like:

try {
robot.setPosition(vpRobot::CAMERA_FRAME, q);
}
catch (vpRobotException e) {
if (e.getCode() == vpRobotException::positionOutOfRangeError) {
std::cout << "The position is out of range" << std::endl;
}

Implements vpRobot.

Examples:
moveAfma4.cpp.

Definition at line 801 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpMatrix::data, vpRobot::getRobotState(), vpMatrix::getRows(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma4::njoint, vpRobotException::positionOutOfRangeError, vpRobot::REFERENCE_FRAME, setRobotState(), vpRobot::STATE_POSITION_CONTROL, and vpERROR_TRACE.

Referenced by move(), and setPosition().

void vpRobotAfma4::setPosition ( const vpRobot::vpControlFrameType  frame,
const double  q1,
const double  q2,
const double  q4,
const double  q5 
)

Move to an absolute position with a given percent of max velocity. The percent of max velocity is to set with setPositioningVelocity().

Warning
The position to reach can only be specified in joint space coordinates.

This method owerloads setPosition(const vpRobot::vpControlFrameType, const vpColVector &).

Warning
This method is blocking. It returns only when the position is reached by the robot.
Parameters
q1,q2,q4,q5: The four joint positions to reach. q1 corresponds to the first rotation (joint 1 with value $q_1$) of the turret around the vertical axis, while q2 corresponds to the vertical translation (joint 2 with value $q_2$), while q4 and q5 correspond to the pan and tilt of the camera (respectively joint 4 and 5 with values $q_4$ and $q_5$). Rotations q1, q4 and q5 are expressed in radians. The translation q2 is expressed in meters.
frame: Frame in which the position is expressed.
Exceptions
vpRobotException::lowLevelError: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
// Set positions in the camera frame
double q1 = M_PI/8; // Joint 1, in radian
double q2 = 0.2; // Joint 2, in meter
double q4 = M_PI/4; // Joint 4, in radian
double q5 = M_PI/8; // Joint 5, in radian
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Set the max velocity to 20%
robot.setPositioningVelocity(20);
// Moves the robot in the camera frame
robot.setPosition(vpRobot::ARTICULAR_FRAME, q1, q2, q4, q5);
See also
setPosition()

Definition at line 917 of file vpRobotAfma4.cpp.

References vpAfma4::njoint, setPosition(), and vpERROR_TRACE.

void vpRobotAfma4::setPosition ( const char *  filename)

Move to an absolute joint position with a given percent of max velocity. The robot state is set to position control. The percent of max velocity is to set with setPositioningVelocity(). The position to reach is defined in the position file.

Parameters
filename: Name of the position file to read. The readPosFile() documentation shows a typical content of such a position file.

This method has the same behavior than the sample code given below;

vpColVector q;
robot.readPosFile("MyPositionFilename.pos", q);
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL)
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
Exceptions
vpRobotException::lowLevelError: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
See also
setPositioningVelocity()

Definition at line 970 of file vpRobotAfma4.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobotException::lowLevelError, readPosFile(), setPosition(), setRobotState(), vpRobot::STATE_POSITION_CONTROL, and vpERROR_TRACE.

void vpRobotAfma4::setPositioningVelocity ( const double  velocity)

Set the maximal velocity percentage to use for a position control.

The default positioning velocity is defined by vpRobotAfma4::defaultPositioningVelocity. This method allows to change this default positioning velocity

Parameters
velocity: Percentage of the maximal velocity. Values should be in ]0:100].
vpColVector q[4]);
q = 0; // position in meter and rad
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Set the max velocity to 20%
robot.setPositioningVelocity(20);
// Moves the robot to the joint position [0,0,0,0]
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
See also
getPositioningVelocity()

Definition at line 712 of file vpRobotAfma4.cpp.

Referenced by move().

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

Definition at line 212 of file vpRobot.cpp.

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

vpRobot::vpRobotStateType vpRobotAfma4::setRobotState ( vpRobot::vpRobotStateType  newState)
virtual

Change the robot state.

Parameters
newState: New requested robot state.

Reimplemented from vpRobot.

Examples:
moveAfma4.cpp, and servoAfma4Point2DCamVelocityKalman.cpp.

Definition at line 340 of file vpRobotAfma4.cpp.

References vpRobot::getRobotState(), powerOn(), vpRobot::setRobotState(), vpRobot::STATE_POSITION_CONTROL, vpRobot::STATE_STOP, and vpRobot::STATE_VELOCITY_CONTROL.

Referenced by move(), setPosition(), stopMotion(), vpRobotAfma4(), and ~vpRobotAfma4().

void vpRobotAfma4::setVelocity ( const vpRobot::vpControlFrameType  frame,
const vpColVector &  vel 
)
virtual

Apply a velocity to the robot.

Parameters
frame: Control frame in which the velocity is expressed. Velocities could be expressed in the joint space or in the camera frame. The reference frame and mixt frame are not implemented.
vel: Velocity vector. Translation velocities are expressed in m/s while rotation velocities in rad/s. The size of this vector may be 4 (the number of dof) if frame is set to vpRobot::ARTICULAR_FRAME. The size of this vector is 2 when the control is in the camera frame (frame is than set to vpRobot::CAMERA_FRAME).
  • In articular, $ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t $ correspond to joint velocities.
  • In camera frame, $ vel = [^{c} t_x, ^{c} t_y, ^{c} t_z,^{c} \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t$ is expressed in the camera frame. Since only four dof are available, in camera frame we control control only the camera pan and tilt.
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().
Warning
Velocities could be saturated if one of them exceed the maximal autorized speed (see vpRobot::maxTranslationVelocity and vpRobot::maxRotationVelocity). To change these values use setMaxTranslationVelocity() and setMaxRotationVelocity().
// Set joint velocities
vpColVector q_dot(4);
q_dot[0] = M_PI/8; // Joint 1 velocity, in rad/s
q_dot[1] = 0.2; // Joint 2 velocity, in meter/s
q_dot[2] = M_PI/4; // Joint 4 velocity, in rad/s
q_dot[3] = M_PI/8; // Joint 5 velocity, in rad/s
vpRobotAfma4 robot;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);

Implements vpRobot.

Examples:
moveAfma4.cpp, and servoAfma4Point2DCamVelocityKalman.cpp.

Definition at line 1184 of file vpRobotAfma4.cpp.

References vpAfma4::_eMc, vpRobot::ARTICULAR_FRAME, vpVelocityTwistMatrix::buildFrom(), vpRobot::CAMERA_FRAME, vpMatrix::data, vpRobot::eJe, vpHomogeneousMatrix::extract(), vpAfma4::get_fJe_inverse(), vpAfma4::get_fMe(), vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), getPosition(), vpRobot::getRobotState(), vpMatrix::getRows(), vpRobot::MIXT_FRAME, vpAfma4::njoint, vpRobot::REFERENCE_FRAME, vpRobot::saturateVelocities(), vpRobot::STATE_VELOCITY_CONTROL, vpERROR_TRACE, and vpRobotException::wrongStateError.

void vpRobot::setVerbose ( bool  verbose)
inlineinherited
Examples:
tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, and tutorial-ibvs-4pts-wireframe-robot-viper.cpp.

Definition at line 158 of file vpRobot.h.

Referenced by vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().

void vpRobotAfma4::stopMotion ( void  )

Stop the robot and set the robot state to vpRobot::STATE_STOP.

Exceptions
vpRobotException::lowLevelError: If the low level controller returns an error during robot stopping.

Definition at line 390 of file vpRobotAfma4.cpp.

References vpRobotException::lowLevelError, setRobotState(), vpRobot::STATE_STOP, and vpERROR_TRACE.

Member Data Documentation

double vpAfma4::_a1
protectedinherited

Definition at line 146 of file vpAfma4.h.

Referenced by vpAfma4::get_eJe(), vpAfma4::get_fJe(), vpAfma4::get_fJe_inverse(), vpAfma4::get_fMe(), and vpAfma4::vpAfma4().

double vpAfma4::_d3
protectedinherited

Definition at line 147 of file vpAfma4.h.

Referenced by vpAfma4::get_eJe(), vpAfma4::get_fJe(), vpAfma4::get_fJe_inverse(), vpAfma4::get_fMe(), and vpAfma4::vpAfma4().

double vpAfma4::_d4
protectedinherited

Definition at line 148 of file vpAfma4.h.

Referenced by vpAfma4::get_fMe(), and vpAfma4::vpAfma4().

vpHomogeneousMatrix vpAfma4::_eMc
protectedinherited

Definition at line 156 of file vpAfma4.h.

Referenced by vpAfma4::get_cMe(), vpAfma4::get_fMc(), setVelocity(), and vpAfma4::vpAfma4().

vpRxyzVector vpAfma4::_erc
protectedinherited

Definition at line 154 of file vpAfma4.h.

Referenced by vpAfma4::vpAfma4().

vpTranslationVector vpAfma4::_etc
protectedinherited

Definition at line 153 of file vpAfma4.h.

Referenced by vpAfma4::vpAfma4().

double vpAfma4::_joint_max[4]
protectedinherited

Definition at line 149 of file vpAfma4.h.

Referenced by vpAfma4::getJointMax(), init(), and vpAfma4::vpAfma4().

double vpAfma4::_joint_min[4]
protectedinherited

Definition at line 150 of file vpAfma4.h.

Referenced by vpAfma4::getJointMin(), init(), and vpAfma4::vpAfma4().

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 111 of file vpRobot.h.

Referenced by vpRobot::operator=().

const double vpRobotAfma4::defaultPositioningVelocity = 15.0
static

Default positioning velocity in percentage of the maximum velocity. This value is set to 15. The member function setPositioningVelocity() allows to change this value.

Definition at line 227 of file vpRobotAfma4.h.

Referenced by vpRobotAfma4().

vpMatrix vpRobot::eJe
protectedinherited

robot Jacobian expressed in the end-effector frame

Definition at line 103 of file vpRobot.h.

Referenced by vpRobotCamera::get_eJe(), vpSimulatorCamera::get_eJe(), vpRobot::operator=(), and setVelocity().

int vpRobot::eJeAvailable
protectedinherited

is the robot Jacobian expressed in the end-effector frame available

Definition at line 105 of file vpRobot.h.

Referenced by vpRobot::operator=().

vpMatrix vpRobot::fJe
protectedinherited

robot Jacobian expressed in the robot reference frame available

Definition at line 107 of file vpRobot.h.

Referenced by vpRobot::operator=().

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 109 of file vpRobot.h.

Referenced by vpRobot::operator=().

double vpRobot::maxRotationVelocity
protectedinherited

Definition at line 97 of file vpRobot.h.

Referenced by vpRobot::operator=(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

Definition at line 98 of file vpRobot.h.

double vpRobot::maxTranslationVelocity
protectedinherited

Definition at line 95 of file vpRobot.h.

Referenced by vpRobot::operator=().

const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

Definition at line 96 of file vpRobot.h.

int vpRobot::nDof
protectedinherited

number of degrees of freedom

Definition at line 101 of file vpRobot.h.

Referenced by vpRobot::operator=().

const unsigned int vpAfma4::njoint = 4
staticinherited

Number of joint.

Examples:
moveAfma4.cpp.

Definition at line 142 of file vpAfma4.h.

Referenced by get_cVf(), get_eJe(), get_fJe(), getDisplacement(), getPosition(), getVelocity(), setPosition(), and setVelocity().

double* vpRobot::qmax
protectedinherited

Definition at line 113 of file vpRobot.h.

Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().

double* vpRobot::qmin
protectedinherited

Definition at line 112 of file vpRobot.h.

Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().

bool vpRobot::verbose_
protectedinherited

Definition at line 115 of file vpRobot.h.

Referenced by init(), vpRobotAfma6::init(), vpRobotViper650::init(), vpRobotViper850::init(), vpRobot::operator=(), vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().