#include <vpRobotAfma6.h>

Inheritance diagram for vpRobotAfma6:

Public Types
enum	vpAfma6ToolType { TOOL_CCMOP, TOOL_GRIPPER, TOOL_VACUUM, TOOL_GENERIC_CAMERA }

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
	vpRobotAfma6 (bool verbose=true)

virtual	~vpRobotAfma6 (void)

bool	checkJointLimits (vpColVector &jointsStatus)

void	closeGripper ()

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)

void	getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position)

void	getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &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_eJe (vpMatrix &_eJe)

void	get_fJe (vpMatrix &_fJe)

void	init (void)

void	init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)

void	move (const char *filename)

void	move (const char *filename, const double velocity)

void	openGripper ()

void	powerOn ()

void	powerOff ()

void	setPosition (const vpRobot::vpControlFrameType frame, const vpPoseVector &pose)

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

void	setPosition (const vpRobot::vpControlFrameType frame, const double pos1, const double pos2, const double pos3, const double pos4, const double pos5, const double pos6)

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 ()

void	init (const char paramAfma6, const char paramCamera)

vpHomogeneousMatrix	getForwardKinematics (const vpColVector &q) const

int	getInverseKinematics (const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) 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_eJe (const vpColVector &q, vpMatrix &eJe) const

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

void	parseConfigFile (const char *filename)

vpAfma6ToolType	getToolType () const

vpCameraParameters::vpCameraParametersProjType	getCameraParametersProjType () const

void	getCameraParameters (vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const

void	getCameraParameters (vpCameraParameters &cam, const vpImage< unsigned char > &I) const

void	getCameraParameters (vpCameraParameters &cam, const vpImage< vpRGBa > &I) const

vpColVector	getJointMin () const

vpColVector	getJointMax () const

double	getCoupl56 () const

double	getLong56 () 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 char *const	CONST_AFMA6_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf"

static const char *const	CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf"

static const char *const	CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf"

static const char *const	CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf"

static const char *const	CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf"

static const char *const	CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf"

static const char *const	CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf"

static const char *const	CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf"

static const char *const	CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf"

static const char *const	CONST_CAMERA_AFMA6_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml"

static const char *const	CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop"

static const char *const	CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper"

static const char *const	CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum"

static const char *const	CONST_GENERIC_CAMERA_NAME = "Generic-camera"

static const vpAfma6ToolType	defaultTool = TOOL_CCMOP

static const unsigned int	njoint = 6

Protected Member Functions
void	setToolType (vpAfma6::vpAfma6ToolType tool)

vpControlFrameType	setRobotFrame (vpRobot::vpControlFrameType newFrame)

vpControlFrameType	getRobotFrame (void) const

Protected Attributes
double	_coupl_56

double	_long_56

double	_joint_max [6]

double	_joint_min [6]

vpTranslationVector	_etc

vpRxyzVector	_erc

vpHomogeneousMatrix	_eMc

vpAfma6ToolType	tool_current

vpCameraParameters::vpCameraParametersProjType	projModel

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 gantry robot named Afma6.

Implementation of the vpRobot class in order to control Irisa's Afma6 robot. This robot is a gantry robot with six degrees of freedom manufactured in 1992 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 ring light is attached around the camera. The control of this ring light is possible throw the vpRingLight class. A CCMOP gripper is also mounted on the end-effector. The pneumatic control of this gripper is possible throw the openGripper() or closeGripper() member functions.

This class allows to control the Afma6 gantry robot in position and velocity:

in the joint space (vpRobot::ARTICULAR_FRAME),
in the fixed reference frame (vpRobot::REFERENCE_FRAME),
in the camera frame (vpRobot::CAMERA_FRAME),
or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed in the reference frame and rotations in the camera frame.

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 direct and inverse kinematics models are implemented in the vpAfma6 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:

#include <visp/vpConfig.h>
#include <visp/vpRobotAfma6.h>
#ifdef VISP_HAVE_AFMA6
int main()
{
  vpRobotAfma6 robot;
}
#else
int main() {}
#endif

This initialize the robot kinematics with the eMc extrinsic camera parameters obtained with a projection model without distortion. To set the robot kinematics with the eMc matrix obtained with a camera perspective model including distortion you need to initialize the robot with:

// Set the extrinsic camera parameters obtained with a perpective 
// projection model including a distortion parameter
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);

You can get the intrinsic camera parameters of the image I acquired with the camera, with:

vpCameraParameters cam;
robot.getCameraParameters(cam, I);
// In cam, you get the intrinsic parameters of the projection model 
// with distortion.

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(6);
// Set a joint position
q[0] = 0.1; // x axis, in meter
q[1] = 0.2; // y axis, in meter
q[2] = 0.3; // z axis, in meter
q[3] = M_PI/8; // rotation around A axis, in rad
q[4] = M_PI/4; // rotation around B axis, in rad
q[5] = M_PI;   // rotation around C axis, in rad
// 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 vpRobotAfma6::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] = 0.1;    // x axis, in m/s
qvel[1] = 0.2;    // y axis, in m/s
qvel[2] = 0;      // z axis, in m/s
qvel[3] = M_PI/8; // rotation around A axis, in rad/s
qvel[4] = 0;      // rotation around B axis, in rad/s
qvel[5] = 0;      // rotation around C 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:: servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoAfma6TwoLines2DCamVelocity.cpp, testRobotAfma6.cpp, and testRobotAfma6Pose.cpp.

Definition at line 214 of file vpRobotAfma6.h.

Member Enumeration Documentation

enum vpAfma6::vpAfma6ToolType

inherited

List of possible tools that can be attached to the robot end-effector.

Enumerator
TOOL_CCMOP	Pneumatic CCMOP gripper.
TOOL_GRIPPER	Pneumatic gripper with 2 fingers.
TOOL_VACUUM	Pneumatic vaccum gripper.
TOOL_GENERIC_CAMERA	A generic camera.

Definition at line 108 of file vpAfma6.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 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

vpRobotAfma6::vpRobotAfma6 ( bool verbose = true )

The only available constructor.

This contructor calls init() to initialise the connection with the MotionBox or low level controller, send the default eMc homogeneous matrix, 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.

To set the extrinsic camera parameters related to the eMc matrix obtained with a camera perspective projection model including the distorsion, use the code below:

vpRobotAfma6 robot;
// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distorsion parameter
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);

Now, you can get the intrinsic camera parameters of the image I acquired with the camera, with:

vpCameraParameters cam;
robot.getCameraParameters(cam, I);
// In cam, you get the intrinsic parameters of the projection model
// with distorsion.

See also: vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType)

Definition at line 158 of file vpRobotAfma6.cpp.

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

vpRobotAfma6::~vpRobotAfma6 ( void )

virtual

Destructor.

Free allocated resources.

Definition at line 397 of file vpRobotAfma6.cpp.

References setRobotState(), and vpRobot::STATE_STOP.

Member Function Documentation

bool vpRobotAfma6::checkJointLimits ( vpColVector & jointsStatus )

Test the joints of the robot to detect if one or more is at its limit.

Parameters

jointsStatus : A vector (size 6) of the status of the joints. For each joint, the value is equal to 1 if the joint is at its maximal limit, -1 if the joint is at its minimal value and 0 otherwise.

Returns: false if at least one joint is at one of its limit.

Definition at line 2208 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, vpAfma6::njoint, vpColVector::resize(), and vpERROR_TRACE.

void vpRobotAfma6::closeGripper ( )

Close the pneumatic CCMOP gripper.

See also: openGripper()

Definition at line 2049 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

void vpRobotAfma6::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.

Examples:: servoAfma6Points2DCamVelocityEyeToHand.cpp.

Definition at line 671 of file vpRobotAfma6.cpp.

References vpAfma6::get_cMe().

void vpRobotAfma6::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.

Examples:: servoAfma6FourPoints2DArtVelocity.cpp, and servoAfma6Point2DArtVelocity.cpp.

Definition at line 652 of file vpRobotAfma6.cpp.

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

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

inherited

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

Parameters

q	: Articular joint position of the robot. q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.
eJe	: Robot jacobian expressed in the end-effector frame.

Definition at line 899 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_long_56, and vpMatrix::resize().

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorAfma6::get_eJe(), get_eJe(), and vpSimulatorAfma6::getVelocity().

void vpRobotAfma6::get_eJe ( vpMatrix & eJe )

virtual

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

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.

Implements vpRobot.

Examples:: servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, and servoAfma6Points2DCamVelocityEyeToHand.cpp.

Definition at line 688 of file vpRobotAfma6.cpp.

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

void vpAfma6::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}{cccccc} 1 & 0 & 0 & -Ls4 & 0 & 0 \\ 0 & 1 & 0 & Lc4 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & c4+\gamma s4c5 & -s4c5 \\ 0 & 0 & 0 & 0 & s4-\gamma c4c5 & c4c5 \\ 0 & 0 & 0 & 1 & -gamma s5 & s5 \\ \end{array} \right)$

where $\gamma$ is the coupling factor between join 5 and 6.

Parameters

q	: Articular joint position of the robot. q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.
fJe	: Robot jacobian expressed in the robot reference frame.

Definition at line 968 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_long_56, and vpMatrix::resize().

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorAfma6::get_fJe(), get_fJe(), and vpSimulatorAfma6::getVelocity().

void vpRobotAfma6::get_fJe ( vpMatrix & fJe )

virtual

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

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

${^f}J_e = \left(\begin{array}{cccccc} 1 & 0 & 0 & -Ls4 & 0 & 0 \\ 0 & 1 & 0 & Lc4 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & c4 & -s4c5 \\ 0 & 0 & 0 & 0 & s4 & c4c5 \\ 0 & 0 & 0 & 1 & 0 & s5 \\ \end{array} \right)$

Parameters

fJe	: Robot jacobian expressed in the reference frame.

Implements vpRobot.

Definition at line 736 of file vpRobotAfma6.cpp.

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

vpHomogeneousMatrix vpAfma6::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 six joints.

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

Parameters

q	: Articular position of the six joints: q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.

Returns: The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the base frame and the camera frame (fMc).

See also: getForwardKinematics(const vpColVector & q)

Examples:: testRobotAfma6Pose.cpp.

Definition at line 745 of file vpAfma6.cpp.

Referenced by vpSimulatorAfma6::compute_fMi(), getDisplacement(), vpAfma6::getForwardKinematics(), vpSimulatorAfma6::getPosition(), getPosition(), getVelocity(), vpSimulatorAfma6::setPosition(), and setPosition().

void vpAfma6::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 six joints.

Parameters

q	: Articular joint position of the robot. q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.
fMc	The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame (fMc).

Definition at line 773 of file vpAfma6.cpp.

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

void vpAfma6::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 six joints.

Parameters

q	: Articular joint position of the robot. q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.
fMe	The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the end effector frame (fMe).

Definition at line 807 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, and vpAfma6::_long_56.

Referenced by vpAfma6::get_fMc().

void vpAfma6::getCameraParameters	(	vpCameraParameters &	cam,
		const unsigned int &	image_width,
		const unsigned int &	image_height
	)		const

inherited

Get the current intrinsic camera parameters obtained by calibration.

Warning: This method needs XML library to parse the file defined in vpAfma6::CONST_CAMERA_AFMA6_FILENAME and containing the camera parameters. If XML is detected by ViSP, VISP_HAVE_XML2 macro is defined in include/visp/vpConfig.h file.; Thid method needs also an access to the file located on Inria's NAS server and containing the camera parameters in XML format. This access is available if VISP_HAVE_ACCESS_TO_NAS macro is defined in include/visp/vpConfig.h file.

If VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined, this method gets the camera parameters from /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml config file.
If these two macros are not defined, this method set the camera parameters to default one.

Parameters

cam	: In output, camera parameters to fill.
image_width	: Image width used to compute camera calibration.
image_height	: Image height used to compute camera calibration.

The code below shows how to get the camera parameters of the camera attached to the robot.

#include <visp/vpImage.h>
#include <visp/vp1394TwoGrabber.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpCameraParameters.h>
int main()
{
#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
  vpImage<unsigned char> I;
  vp1394TwoGrabber g;
  // Acquire an image to update image structure
  g.acquire(I) ;
  vpRobotAfma6 robot;
  vpCameraParameters cam ;
  // Get the intrinsic camera parameters depending on the image size
  // Camera parameters are read from
  // /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml
  // if VISP_HAVE_ACCESS_TO_NAS and VISP_HAVE_XML2 macros are defined in vpConfig.h file
  try {
    robot.getCameraParameters (cam, I.getWidth(), I.getHeight());
  }
  catch(...) {
    std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
  }
  std::cout << "Camera parameters: " << cam << std::endl;
#endif
}

Exceptions

vpRobotException::readingParametersError : If the camera parameters are not found.

Examples:: servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoAfma6TwoLines2DCamVelocity.cpp, testAfma6.cpp, testRobotAfma6.cpp, and testRobotAfma6Pose.cpp.

Definition at line 1248 of file vpAfma6.cpp.

References vpAfma6::CONST_CAMERA_AFMA6_FILENAME, vpAfma6::CONST_CCMOP_CAMERA_NAME, vpAfma6::CONST_GENERIC_CAMERA_NAME, vpAfma6::CONST_GRIPPER_CAMERA_NAME, vpAfma6::CONST_VACUUM_CAMERA_NAME, vpAfma6::getToolType(), vpCameraParameters::initPersProjWithDistortion(), vpCameraParameters::initPersProjWithoutDistortion(), vpXmlParserCamera::parse(), vpCameraParameters::perspectiveProjWithDistortion, vpCameraParameters::perspectiveProjWithoutDistortion, vpAfma6::projModel, vpRobotException::readingParametersError, vpXmlParserCamera::SEQUENCE_OK, vpAfma6::TOOL_CCMOP, vpAfma6::TOOL_GENERIC_CAMERA, vpAfma6::TOOL_GRIPPER, vpAfma6::TOOL_VACUUM, vpERROR_TRACE, and vpTRACE.

Referenced by vpAfma6::getCameraParameters().

void vpAfma6::getCameraParameters	(	vpCameraParameters &	cam,
		const vpImage< unsigned char > &	I
	)		const

inherited

Get the current intrinsic camera parameters obtained by calibration.

Camera parameters are read from /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml

Parameters

cam	: In output, camera parameters to fill.
I	: A B&W image send by the current camera in use.

#include <visp/vpImage.h>
#include <visp/vp1394TwoGrabber.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpCameraParameters.h>
int main()
{
#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
  vpImage<unsigned char> I;
  vp1394TwoGrabber g;
  // Acquire an image to update image structure
  g.acquire(I) ;
  vpRobotAfma6 robot;
  vpCameraParameters cam ;
  // Get the intrinsic camera parameters depending on the image size
  try {
    robot.getCameraParameters (cam, I);
  }
  catch(...) {
    std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
  }
  std::cout << "Camera parameters: " << cam << std::endl;
#endif
}

Exceptions

vpRobotException::readingParametersError : If the camera parameters are not found.

Definition at line 1467 of file vpAfma6.cpp.

References vpAfma6::getCameraParameters(), vpImage< Type >::getHeight(), and vpImage< Type >::getWidth().

void vpAfma6::getCameraParameters	(	vpCameraParameters &	cam,
		const vpImage< vpRGBa > &	I
	)		const

inherited

Get the current intrinsic camera parameters obtained by calibration.

Camera parameters are read from /udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml

Parameters

cam	: In output, camera parameters to fill.
I	: A color image send by the current camera in use.

#include <visp/vpConfig.h>
#include <visp/vpImage.h>
#include <visp/vp1394TwoGrabber.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpCameraParameters.h>
int main()
{
#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_AFMA6)
  vpImage<vpRGBa> I;
  vp1394TwoGrabber g;
  // Acquire an image to update image structure
  g.acquire(I) ;
  vpRobotAfma6 robot;
  vpCameraParameters cam ;
  // Get the intrinsic camera parameters depending on the image size
  try {
    robot.getCameraParameters (cam, I);
  }
  catch(...) {
    std::cout << "Cannot get camera parameters for image: " << I.getWidth() << " x " << I.getHeight() << std::endl;
  }
  std::cout << "Camera parameters: " << cam << std::endl;
#endif
}

Exceptions

vpRobotException::readingParametersError : If the camera parameters are not found.

Definition at line 1516 of file vpAfma6.cpp.

References vpAfma6::getCameraParameters(), vpImage< Type >::getHeight(), and vpImage< Type >::getWidth().

vpCameraParameters::vpCameraParametersProjType vpAfma6::getCameraParametersProjType ( ) const

inlineinherited

Get the current camera model projection type.

Definition at line 154 of file vpAfma6.h.

double vpAfma6::getCoupl56 ( ) const

inherited

Return the coupling factor between join 5 and 6.

Returns: Coupling factor between join 5 and 6.

Definition at line 1042 of file vpAfma6.cpp.

References vpAfma6::_coupl_56.

void vpRobotAfma6::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 reference and mixt frames. It is only available in the joint space (vpRobot::ARTICULAR_FRAME) and in the camera frame (vpRobot::CAMERA_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 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 2118 of file vpRobotAfma6.cpp.

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

vpHomogeneousMatrix vpAfma6::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 six joints.

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

Parameters

q	: Articular position of the six joints: q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.

Returns: The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the base frame and the camera frame (fMc).

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

Definition at line 482 of file vpAfma6.cpp.

References vpAfma6::get_fMc().

int vpAfma6::getInverseKinematics	(	const vpHomogeneousMatrix &	fMc,
		vpColVector &	q,
		const bool &	nearest = `true`,
		const bool &	verbose = `false`
	)		const

inherited

Compute the inverse kinematics (inverse geometric model).

By inverse kinematics we mean here the six articular values of the joint positions given the position and the orientation of the camera frame relative to the base frame.

Parameters

fMc	: Homogeneous matrix describing the transformation from base frame to the camera frame.
q	: In input, the current articular joint position of the robot. In output, the solution of the inverse kinematics. Articular position of the six joints: q[0], q[1], q[2] correspond to the first 3 translations expressed in meter, while q[3], q[4] and q[5] correspond to the 3 succesives rotations expressed in radians.
nearest	: true to return the nearest solution to q. false to return the farest.
verbose	: Activates printings when no solution is found.

Returns: The number of solutions (1 or 2) of the inverse geometric model. O, if no solution can be found.

The code below shows how to compute the inverse geometric model:

#include <visp/vpConfig.h>
#include <visp/vpColVector.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
  vpColVector q1(6), q2(6);
  vpHomogeneousMatrix fMc;
  vpRobotAfma6 robot;
  // Get the current articular position of the robot
  robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
  // Compute the pose of the camera in the reference frame using the
  // direct geometric model
  fMc = robot.getForwardKinematics(q1);
  // this is similar to  fMc = robot.get_fMc(q1);
  // or robot.get_fMc(q1, fMc);
  // Compute the inverse geometric model
  int nbsol; // number of solutions (0, 1 or 2) of the inverse geometric model
  // get the nearest solution to the current articular position
  nbsol = robot.getInverseKinematics(fMc, q1, true);
  if (nbsol == 0)
    std::cout << "No solution of the inverse geometric model " << std::endl;
  else if (nbsol >= 1)
    std::cout << "First solution: " << q1 << std::endl;
  if (nbsol == 2) {
    // Compute the other solution of the inverse geometric model
    q2 = q1;
    robot.getInverseKinematics(fMc, q2, false);
    std::cout << "Second solution: " << q2 << std::endl;
  }
#endif
}

See also: getForwardKinematics()

Definition at line 564 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_eMc, vpAfma6::_joint_max, vpAfma6::_joint_min, vpAfma6::_long_56, vpMath::deg(), vpMatrix::getRows(), vpHomogeneousMatrix::inverse(), vpAfma6::njoint, vpMath::rad(), vpColVector::resize(), and vpTRACE.

Referenced by vpSimulatorAfma6::initialiseCameraRelativeToObject(), vpSimulatorAfma6::setPosition(), and setPosition().

vpColVector vpAfma6::getJointMax ( ) const

inherited

Get max joint values.

Returns: Maximal joint values for the 6 dof X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation A,B,C in radians.

Definition at line 1027 of file vpAfma6.cpp.

References vpAfma6::_joint_max.

vpColVector vpAfma6::getJointMin ( ) const

inherited

Get min joint values.

Returns: Minimal joint values for the 6 dof X,Y,Z,A,B,C. Translation X,Y,Z are expressed in meters. Rotation A,B,C in radians.

Definition at line 1010 of file vpAfma6.cpp.

References vpAfma6::_joint_min.

double vpAfma6::getLong56 ( ) const

inherited

Return the distance between join 5 and 6.

Returns: Distance between join 5 and 6.

Definition at line 1054 of file vpAfma6.cpp.

References vpAfma6::_long_56.

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(), vpRobotAfma4::setVelocity(), 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(), vpRobotAfma4::setVelocity(), 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 vpRobotAfma6::getPosition	(	const vpRobot::vpControlFrameType	frame,
		vpColVector &	position
	)

virtual

Get the current position of the robot.

The difference is here that the timestamp is not used.

Implements vpRobot.

Examples:: servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, and testRobotAfma6Pose.cpp.

Definition at line 1400 of file vpRobotAfma6.cpp.

Referenced by getPosition().

void vpRobotAfma6::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 6 dimension vector corresponding to the articular position of each dof, first the 3 translations, then the 3 articular rotation positions represented by a vpRxyzVector. 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.

vpRobotAfma6 robot;
vpColVector position;
robot.getPosition(vpRobot::REFERENCE_FRAME, position);
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(ftc, fRc);

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 1317 of file vpRobotAfma6.cpp.

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

void vpRobotAfma6::getPosition	(	const vpRobot::vpControlFrameType	frame,
		vpPoseVector &	position
	)

Get the current position of the robot.

The difference is here that the timestamp is not used.

Definition at line 1445 of file vpRobotAfma6.cpp.

References getPosition().

void vpRobotAfma6::getPosition	(	const vpRobot::vpControlFrameType	frame,
		vpPoseVector &	position,
		double &	timestamp
	)

Get the current position of the robot. Similar as : void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position) as the difference the position is returned using a ThetaU representation.

Definition at line 1416 of file vpRobotAfma6.cpp.

References getPosition().

double vpRobotAfma6::getPositioningVelocity ( void )

Get the maximal velocity percentage used for a position control.

See also: setPositioningVelocity()

Definition at line 801 of file vpRobotAfma6.cpp.

bool vpRobotAfma6::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 619 of file vpRobotAfma6.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(), vpRobotCamera::setPosition(), vpSimulatorCamera::setPosition(), vpRobotPtu46::setPosition(), vpRobotBiclops::setPosition(), vpSimulatorAfma6::setPosition(), vpSimulatorViper850::setPosition(), vpRobotAfma4::setPosition(), setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotPtu46::setRobotState(), vpRobotBiclops::setRobotState(), vpSimulatorAfma6::setRobotState(), vpSimulatorViper850::setRobotState(), vpRobotAfma4::setRobotState(), setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotPtu46::setVelocity(), vpRobotBiclops::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), vpRobotAfma4::setVelocity(), setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpSimulatorAfma6::stopMotion(), vpSimulatorViper850::stopMotion(), vpRobotViper650::stopMotion(), and vpRobotViper850::stopMotion().

double vpRobotAfma6::getTime ( ) const

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

Definition at line 1384 of file vpRobotAfma6.cpp.

vpAfma6ToolType vpAfma6::getToolType ( ) const

inlineinherited

Get the current tool type.

Definition at line 150 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters(), vpSimulatorAfma6::getCameraParameters(), and vpSimulatorAfma6::initArms().

void vpRobotAfma6::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:: servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Point2DArtVelocity.cpp, and servoAfma6Point2DCamVelocity.cpp.

Definition at line 1751 of file vpRobotAfma6.cpp.

Referenced by getVelocity().

void vpRobotAfma6::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(6);
q_dot[0] = 0.1;    // X axis, in meter/s
q_dot[1] = 0.2;    // Y axis, in meter/s
q_dot[2] = 0.3;    // Z axis, in meter/s
q_dot[3] = M_PI/8; // A axis, in rad/s
q_dot[4] = M_PI/4; // B axis, in rad/s
q_dot[5] = M_PI/16;// C axis, in rad/s
vpRobotAfma6 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 6, 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 [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
}

Definition at line 1641 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpThetaUVector::buildFrom(), vpRobot::CAMERA_FRAME, vpHomogeneousMatrix::extract(), vpAfma6::get_fMc(), vpExponentialMap::inverse(), vpHomogeneousMatrix::inverse(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma6::njoint, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpERROR_TRACE.

vpColVector vpRobotAfma6::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 1817 of file vpRobotAfma6.cpp.

References getVelocity().

vpColVector vpRobotAfma6::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(6);
q_dot[0] = 0.1;    // X axis, in meter/s
q_dot[1] = 0.2;    // Y axis, in meter/s
q_dot[2] = 0.3;    // Z axis, in meter/s
q_dot[3] = M_PI/8; // A axis, in rad/s
q_dot[4] = M_PI/4; // B axis, in rad/s
q_dot[5] = M_PI/16;// C axis, in rad/s
vpRobotAfma6 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 6, 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 [0.1, 0.2, 0.3, M_PI/8, M_PI/4, M_PI/16]
}

Definition at line 1801 of file vpRobotAfma6.cpp.

References getVelocity().

void vpAfma6::init	(	const char *	paramAfma6,
		const char *	paramCamera
	)

inherited

Read files containing the constant parameters related to the robot kinematics and to the end-effector to camera transformation.

Warning: This function is only available if the macro VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.

Parameters

paramAfma6	: Filename containing the constant parameters of the robot kinematics.
paramCamera	: Filename containing the camera extrinsic parameters.

Definition at line 221 of file vpAfma6.cpp.

References vpAfma6::parseConfigFile().

void vpRobotAfma6::init ( void )

virtual

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

Warning: This method sets the camera extrinsic parameters (matrix eMc) to the one obtained by calibration with a camera projection model without distorsion by calling init(vpAfma6::defaultCameraRobot). If you want to set the extrinsic camera parameters to those obtained with a camera perspective model including the distorsion you have to call the init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType) method.

See also: vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType)

Implements vpRobot.

Examples:: servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, testRobotAfma6.cpp, and testRobotAfma6Pose.cpp.

Definition at line 232 of file vpRobotAfma6.cpp.

References vpAfma6::_joint_max, vpAfma6::_joint_min, vpRobotException::constructionError, vpAfma6::defaultTool, vpColVector::resize(), and vpRobot::verbose_.

Referenced by vpRobotAfma6().

void vpRobotAfma6::init	(	vpAfma6::vpAfma6ToolType	tool,
		vpCameraParameters::vpCameraParametersProjType	projModel = `vpCameraParameters::perspectiveProjWithoutDistortion`
	)

Initialize the robot kinematics with the extrinsic calibration parameters associated to a specific camera (set the eMc homogeneous parameters in the low level controller) and also get the joint limits from the low-level controller.

The eMc parameters depend on the camera and the projection model in use.

Parameters

tool	: Tool to use.
projModel	: Projection model associated to the camera.

To set the extrinsic camera parameters related to the eMc matrix obtained with a camera perspective projection model including the distorsion, use the code below:

vpRobotAfma6 robot;
// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distorsion parameter
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);

Now, you can get the intrinsic camera parameters of the image I acquired with the camera, with:

vpCameraParameters cam;
robot.getCameraParameters(cam, I);
// In cam, you get the intrinsic parameters of the projection model
// with distorsion.

See also: vpCameraParameters, init()

Definition at line 353 of file vpRobotAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_erc, vpAfma6::_etc, vpAfma6::_joint_max, vpAfma6::_joint_min, vpAfma6::_long_56, vpAfma6::init(), and vpAfma6::setToolType().

void vpRobotAfma6::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

filename : File containing a joint position to reach.

See also: readPosFile(), move(const char *, const double)

Definition at line 1988 of file vpRobotAfma6.cpp.

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

void vpRobotAfma6::move	(	const char *	filename,
		const double	velocity
	)

Moves the robot to the joint position specified in the filename with a specified positioning velocity.

Parameters

filename	: File containing a joint position to reach.
velocity	: Percentage of the maximal velocity. Values should be in ]0:100].

See also: readPosFile(), move(const char *)

Definition at line 2011 of file vpRobotAfma6.cpp.

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

void vpRobotAfma6::openGripper ( )

Open the pneumatic CCMOP gripper.

See also: closeGripper()

Definition at line 2028 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

void vpAfma6::parseConfigFile ( const char * filename )

inherited

This function gets the robot constant parameters from a file.

Warning: This function is only available if the macro VISP_HAVE_ACCESS_TO_NAS is defined in vpConfig.h.

Parameters

filename : File name containing the robot constant parameters, like max/min joint values, distance between 5 and 6 axis, coupling factor between axis 5 and 6, and the hand-to-eye homogenous matrix.

Definition at line 1074 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_eMc, vpAfma6::_erc, vpAfma6::_etc, vpAfma6::_joint_max, vpAfma6::_joint_min, vpAfma6::_long_56, vpHomogeneousMatrix::buildFrom(), vpRobotException::readingParametersError, and vpERROR_TRACE.

Referenced by vpAfma6::init().

void vpRobotAfma6::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 582 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

void vpRobotAfma6::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 510 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

Referenced by setRobotState().

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

static

Read joint positions in a specific Afma6 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:

#AFMA6 - Position - Version 2.01
# file: "myposition.pos "
#
# R: X Y Z A B C
# Joint position: X, Y, Z: translations in meters
#                 A, B, C: rotations in degrees
#
R: 0.1 0.3 -0.25 -80.5 80 0

Parameters

filename	: Name of the position file to read.
q	: Joint positions: X,Y,Z,A,B,C. Translations X,Y,Z are expressed in meters, while joint rotations A,B,C 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.

vpRobotAfma6 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 1875 of file vpRobotAfma6.cpp.

References vpAfma6::njoint, 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(), vpRobotAfma4::setVelocity(), setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

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

static

Save joint (articular) positions in a specific Afma6 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,Z,A,B,C] to save in the filename. Translations X,Y,Z are expressed in meters, while rotations A,B,C in radians.

Warning: The joint rotations A,B,C 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 1947 of file vpRobotAfma6.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 vpRobotAfma6::setPosition	(	const vpRobot::vpControlFrameType	frame,
		const vpPoseVector &	pose
	)

Move the robot to an absolute cartesian position with a given percent of max velocity. The percent of max velocity is to set with setPositioningVelocity(). The position to reach can only be specified in camera frame or in the reference frame. In joint, an exception is thrown.

Warning: This method is blocking. It returns only when the position is reached by the robot.

Parameters

pose	: A six dimension pose vector corresponding to the position to reach. The three first parameters are the translations in meter, the three last parameters are the rotations expressed as a theta u vector in radians. If the position is out of range, an exception is provided.
frame	: Frame in which the position is expressed.

In the camera and the reference frame, rotations are represented by a vpThetaUVector.
Mixt frame of joint frame is not implemented.

Exceptions

vpRobotException::lowLevelError	: vpRobot::MIXT_FRAME and vpRobot::ARTICULAR_FRAME not implemented.
vpRobotException::positionOutOfRangeError	: The requested position is out of range.

#include <visp/vpConfig.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpPoseVector.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
  vpPoseVector pose;
  // Set positions in the reference frame
  pose[0] = 0.1;    // x axis, in meter
  pose[1] = 0.;     // y axis, in meter
  pose[2] = 0.3;    // z axis, in meter
  pose[3] = M_PI/8; // ThetaU rotation around x axis, in rad
  pose[4] = M_PI/4; // ThetaU rotation around y axis, in rad
  pose[5] = 0.;     // ThetaU rotation around z axis, in rad
  vpRobotAfma6 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::REFERENCE_FRAME, pose);
  return 0;
#endif
}

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

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

Definition at line 881 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRotationMatrix::buildFrom(), vpRxyzVector::buildFrom(), and vpRobotException::lowLevelError.

Referenced by move(), and setPosition().

void vpRobotAfma6::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(). The position to reach can be specified in joint coordinates, in the camera frame or in the reference frame.

Warning: This method is blocking. It returns only when the position is reached by the robot.

Parameters

position	: A six dimension vector corresponding to the position to reach. All the positions are expressed in meters for the translations and radians for the rotations. If the position is out of range, an exception is provided.
frame	: Frame in which the position is expressed.

In the joint space, positions are respectively X, Y, Z, A, B, C, with X,Y,Z the translations, and A,B,C the rotations of the end-effector.
In the camera and the reference frame, rotations are represented by a vpRxyzVector.
Mixt frame is not implemented. By mixt frame we mean, translations expressed in the reference frame, and rotations in the camera frame.

Exceptions

vpRobotException::lowLevelError	: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError	: The requested position is out of range.

#include <visp/vpConfig.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpColVector.h>
#include <visp/vpRobotException.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
  vpColVector position(6);
  // Set positions in the camera frame
  position[0] = 0.1;    // x axis, in meter
  position[1] = 0.2;    // y axis, in meter
  position[2] = 0.3;    // z axis, in meter
  position[3] = M_PI/8; // rotation around x axis, in rad
  position[4] = M_PI/4; // rotation around y axis, in rad
  position[5] = M_PI/10;// rotation around z axis, in rad
  vpRobotAfma6 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::CAMERA_FRAME, position);
  
  return 0;
#endif
}

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

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

Implements vpRobot.

Definition at line 986 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpAfma6::get_fMc(), vpAfma6::getInverseKinematics(), vpRobot::getRobotState(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpAfma6::njoint, vpRobotException::positionOutOfRangeError, vpRobot::REFERENCE_FRAME, setRobotState(), vpRobot::STATE_POSITION_CONTROL, and vpERROR_TRACE.

void vpRobotAfma6::setPosition	(	const vpRobot::vpControlFrameType	frame,
		const double	pos1,
		const double	pos2,
		const double	pos3,
		const double	pos4,
		const double	pos5,
		const double	pos6
	)

Move to an absolute position with a given percent of max velocity. The percent of max velocity is to set with setPositioningVelocity(). The position to reach can be specified in joint coordinates, in the camera frame or in the reference frame.

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

pos1,pos2,pos3,pos4,pos5,pos6	: The six coordinates of the position to reach. All the positions are expressed in meters for the translations and radians for the rotations.
frame	: Frame in which the position is expressed.

In the joint space, positions are respectively X (pos1), Y (pos2), Z (pos3), A (pos4), B (pos5), C (pos6), with X,Y,Z the translations, and A,B,C the rotations of the end-effector.
In the camera and the reference frame, rotations [pos4, pos5, pos6] are represented by a vpRxyzVector.
Mixt frame is not implemented. By mixt frame we mean, translations expressed in the reference frame, and rotations in the camera frame.

Exceptions

vpRobotException::lowLevelError	: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError	: The requested position is out of range.

#include <visp/vpConfig.h>
#include <visp/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
  // Set positions in the camera frame
  double pos1 = 0.1;    // x axis, in meter
  double pos2 = 0.2;    // y axis, in meter
  double pos3 = 0.3;    // z axis, in meter
  double pos4 = M_PI/8; // rotation around x axis, in rad
  double pos5 = M_PI/4; // rotation around y axis, in rad
  double pos6 = M_PI;   // rotation around z axis, in rad
  vpRobotAfma6 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::CAMERA_FRAME, pos1, pos2, pos3, pos4, pos5, pos6);
  return 0;
#endif
}

See also: setPosition()

Definition at line 1180 of file vpRobotAfma6.cpp.

References setPosition(), and vpERROR_TRACE.

void vpRobotAfma6::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;

#include <visp/vpConfig.h>
#include <visp/vpRobotAfma6.h>
#include <visp/vpColVector.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
  vpRobotAfma6 robot;
  vpColVector q; // joint position
  robot.readPosFile("MyPositionFilename.pos", q);
  robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
  robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
  return 0;
#endif
}

Exceptions

vpRobotException::lowLevelError	: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError	: The requested position is out of range.

See also: setPositioningVelocity()

Definition at line 1247 of file vpRobotAfma6.cpp.

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

void vpRobotAfma6::setPositioningVelocity ( const double velocity )

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

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

Parameters

velocity : Percentage of the maximal velocity. Values should be in ]0:100].

vpColVector position(6);
position = 0; // position in meter and rad
vpRobotAfma6 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,0,0]
robot.setPosition(vpRobot::ARTICULAR_FRAME, position);

See also: getPositioningVelocity()

Definition at line 790 of file vpRobotAfma6.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 vpRobotAfma6::setRobotState ( vpRobot::vpRobotStateType newState )

virtual

Change the robot state.

Parameters

newState : New requested robot state.

Reimplemented from vpRobot.

Examples:: servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, and servoAfma6TwoLines2DCamVelocity.cpp.

Definition at line 435 of file vpRobotAfma6.cpp.

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

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

void vpAfma6::setToolType ( vpAfma6::vpAfma6ToolType tool )

inlineprotectedinherited

Set the current tool type.

Definition at line 174 of file vpAfma6.h.

Referenced by vpAfma6::init(), vpSimulatorAfma6::init(), and init().

void vpRobotAfma6::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 articular, camera frame, reference frame or mixt frame.
vel	: Velocity vector. Translation velocities are expressed in m/s while rotation velocities in rad/s. The size of this vector is always 6.

In articular, $vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4, \dot{q}_5, \dot{q}_6]^t$ correspond to joint velocities.
In camera frame, $vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c} \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t$ is expressed in the camera frame.
In reference frame, $vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r} \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t$ is expressed in the reference frame.
In mixt frame, $vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t$ . In mixt frame, translations are expressed in the reference frame and rotations $\omega_x, \omega_y, \omega_z$ in the camera frame.

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(6);
q_dot[0] = 0.1;    // X axis, in meter/s
q_dot[1] = 0.2;    // Y axis, in meter/s
q_dot[2] = 0.3;    // Z axis, in meter/s
q_dot[3] = M_PI/8; // A axis, in rad/s
q_dot[4] = M_PI/4; // B axis, in rad/s
q_dot[5] = M_PI/16;// C axis, in rad/s
vpRobotAfma6 robot;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);

Implements vpRobot.

Examples:: servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, and servoAfma6TwoLines2DCamVelocity.cpp.

Definition at line 1507 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpMatrix::data, vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpRobot::getRobotState(), vpRobot::MIXT_FRAME, vpAfma6::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::vpRobotAfma4(), vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().

void vpRobotAfma6::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.

Examples:: servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6SquareLines2DCamVelocity.cpp, and servoAfma6TwoLines2DCamVelocity.cpp.

Definition at line 486 of file vpRobotAfma6.cpp.

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

Member Data Documentation

double vpAfma6::_coupl_56

protectedinherited

Definition at line 184 of file vpAfma6.h.

Referenced by vpAfma6::get_eJe(), vpAfma6::get_fJe(), vpAfma6::get_fMe(), vpAfma6::getCoupl56(), vpAfma6::getInverseKinematics(), init(), vpAfma6::parseConfigFile(), and vpAfma6::vpAfma6().

vpHomogeneousMatrix vpAfma6::_eMc

protectedinherited

Definition at line 192 of file vpAfma6.h.

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpAfma6::get_cMe(), vpAfma6::get_fMc(), vpAfma6::getInverseKinematics(), vpSimulatorAfma6::getVelocity(), vpAfma6::init(), vpSimulatorAfma6::init(), vpAfma6::parseConfigFile(), and vpAfma6::vpAfma6().

vpRxyzVector vpAfma6::_erc

protectedinherited

Definition at line 190 of file vpAfma6.h.

Referenced by vpAfma6::init(), vpSimulatorAfma6::init(), init(), and vpAfma6::parseConfigFile().

vpTranslationVector vpAfma6::_etc

protectedinherited

Definition at line 189 of file vpAfma6.h.

Referenced by vpAfma6::init(), vpSimulatorAfma6::init(), init(), and vpAfma6::parseConfigFile().

double vpAfma6::_joint_max[6]

protectedinherited

Definition at line 186 of file vpAfma6.h.

Referenced by vpAfma6::getInverseKinematics(), vpAfma6::getJointMax(), vpSimulatorAfma6::init(), init(), vpSimulatorAfma6::isInJointLimit(), vpAfma6::parseConfigFile(), vpSimulatorAfma6::setJointLimit(), vpSimulatorAfma6::updateArticularPosition(), and vpAfma6::vpAfma6().

double vpAfma6::_joint_min[6]

protectedinherited

Definition at line 187 of file vpAfma6.h.

Referenced by vpAfma6::getInverseKinematics(), vpAfma6::getJointMin(), vpSimulatorAfma6::init(), init(), vpSimulatorAfma6::isInJointLimit(), vpAfma6::parseConfigFile(), vpSimulatorAfma6::setJointLimit(), vpSimulatorAfma6::updateArticularPosition(), and vpAfma6::vpAfma6().

double vpAfma6::_long_56

protectedinherited

Definition at line 185 of file vpAfma6.h.

Referenced by vpSimulatorAfma6::compute_fMi(), vpAfma6::get_eJe(), vpAfma6::get_fJe(), vpAfma6::get_fMe(), vpAfma6::getInverseKinematics(), vpAfma6::getLong56(), init(), vpAfma6::parseConfigFile(), and vpAfma6::vpAfma6().

int vpRobot::areJointLimitsAvailable

protectedinherited

Definition at line 111 of file vpRobot.h.

Referenced by vpRobot::operator=().

const char *const vpAfma6::CONST_AFMA6_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_Afma6.cnf"

staticinherited

File where constant parameters in relation with the robot are stored: joint max, min, coupling factor between 4 ant 5 joint, distance between 5 and 6 joint, tranformation eMc between end-effector and camera frame.

Definition at line 76 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_CAMERA_AFMA6_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_camera_Afma6.xml"

staticinherited

Definition at line 85 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop"

staticinherited

Name of the camera attached to the CCMOP tool (vpAfma6ToolType::TOOL_CCMOP).

Definition at line 90 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters(), and vpSimulatorAfma6::getCameraParameters().

const char *const vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_with_distortion_Afma6.cnf"

staticinherited

Definition at line 78 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_ccmop_without_distortion_Afma6.cnf"

staticinherited

Definition at line 77 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_with_distortion_Afma6.cnf"

staticinherited

Definition at line 84 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_generic_without_distortion_Afma6.cnf"

staticinherited

Definition at line 83 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_with_distortion_Afma6.cnf"

staticinherited

Definition at line 80 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_gripper_without_distortion_Afma6.cnf"

staticinherited

Definition at line 79 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_with_distortion_Afma6.cnf"

staticinherited

Definition at line 82 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME = "/udd/fspindle/robot/Afma6/current/include/const_eMc_vacuum_without_distortion_Afma6.cnf"

staticinherited

Definition at line 81 of file vpAfma6.h.

Referenced by vpAfma6::init().

const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera"

staticinherited

Name of the generic camera attached to the robot hand (vpAfma6ToolType::TOOL_GENERIC_CAMERA).

Definition at line 105 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper"

staticinherited

Name of the camera attached to the 2 fingers gripper tool (vpAfma6ToolType::TOOL_GRIPPER).

Definition at line 95 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters(), and vpSimulatorAfma6::getCameraParameters().

const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum"

staticinherited

Name of the camera attached to the vacuum gripper tool (vpAfma6ToolType::TOOL_VACUUM).

Definition at line 100 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

const double vpRobotAfma6::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 262 of file vpRobotAfma6.h.

Referenced by vpRobotAfma6().

const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP

staticinherited

Default tool attached to the robot end effector.

Definition at line 117 of file vpAfma6.h.

Referenced by vpAfma6::init(), and init().

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 vpRobotAfma4::setVelocity().