Visual Servoing Platform  version 3.6.1 under development (2024-03-18)
vpRobotAfma6 Class Reference

#include <visp3/robot/vpRobotAfma6.h>

+ Inheritance diagram for vpRobotAfma6:

Public Types

enum  vpAfma6ToolType {
  TOOL_CCMOP , TOOL_GRIPPER , TOOL_VACUUM , TOOL_GENERIC_CAMERA ,
  TOOL_INTEL_D435_CAMERA , TOOL_CUSTOM
}
 
enum  vpRobotStateType {
  STATE_STOP , STATE_VELOCITY_CONTROL , STATE_POSITION_CONTROL , STATE_ACCELERATION_CONTROL ,
  STATE_FORCE_TORQUE_CONTROL
}
 
enum  vpControlFrameType {
  REFERENCE_FRAME , ARTICULAR_FRAME , JOINT_STATE = ARTICULAR_FRAME , END_EFFECTOR_FRAME ,
  CAMERA_FRAME , TOOL_FRAME = CAMERA_FRAME , MIXT_FRAME
}
 

Public Member Functions

 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) vp_override
 
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) vp_override
 
void get_fJe (vpMatrix &_fJe) vp_override
 
void init (void)
 
void init (vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc)
 
void init (vpAfma6::vpAfma6ToolType tool, const std::string &filename)
 
void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)
 
void move (const std::string &filename)
 
void move (const std::string &filename, 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) vp_override
 
void setPosition (const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3, double pos4, double pos5, double pos6)
 
void setPosition (const std::string &filename)
 
void setPositioningVelocity (double velocity)
 
void set_eMc (const vpHomogeneousMatrix &eMc)
 
vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState)
 
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
 
void stopMotion ()
 
Inherited functionalities from vpAfma6
void init (const std::string &camera_extrinsic_parameters)
 
void init (const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
 
vpHomogeneousMatrix getForwardKinematics (const vpColVector &q) const
 
int getInverseKinematics (const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
 
vpHomogeneousMatrix get_eMc () 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
 
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
 
void parseConfigFile (const std::string &filename)
 
Inherited functionalities from vpRobot
double getMaxTranslationVelocity (void) const
 
double getMaxRotationVelocity (void) const
 
int getNDof () const
 
vpColVector getPosition (const vpRobot::vpControlFrameType frame)
 
virtual vpRobotStateType getRobotState (void) const
 
void setMaxRotationVelocity (double maxVr)
 
void setMaxTranslationVelocity (double maxVt)
 
void setVerbose (bool verbose)
 

Static Public Member Functions

static bool readPosFile (const std::string &filename, vpColVector &q)
 
static bool savePosFile (const std::string &filename, const vpColVector &q)
 
Static Public Member Functions inherited from vpRobot
static vpColVector saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
 

Static Public Attributes

static const double defaultPositioningVelocity = 15.0
 
static const std::string CONST_AFMA6_FILENAME
 
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
 
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
 
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
 
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
 
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
 
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
 
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
 
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
 
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
 
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
 
static const std::string CONST_CAMERA_AFMA6_FILENAME
 
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 char *const CONST_INTEL_D435_CAMERA_NAME = "Intel-D435"
 
static const vpAfma6ToolType defaultTool = TOOL_CCMOP
 

Protected Member Functions

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

Protected Attributes

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
 

Protected Member Functions Inherited from vpAfma6

static const unsigned int njoint = 6
 
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
 
void setToolType (vpAfma6::vpAfma6ToolType tool)
 

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:

End-effector frame (vpRobot::END_EFFECTOR_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 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 <visp3/robot/vpRobotAfma6.h>
#ifdef VISP_HAVE_AFMA6
int main()
{
vpRobotAfma6 robot;
}
#else
int main() {}
#endif
Control of Irisa's gantry robot named Afma6.
Definition: vpRobotAfma6.h:209

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
@ TOOL_CCMOP
Definition: vpAfma6.h:124
@ perspectiveProjWithDistortion
Perspective projection with distortion model.

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

robot.getCameraParameters(cam, I);
// In cam, you get the intrinsic parameters of the projection model
// with distortion.
Generic class defining intrinsic 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:

// 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
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198

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
while (...) {
// Apply a velocity in the joint space
// Compute new velocities qvel...
}
// Stop the robot
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64

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, servoAfma6AprilTagIBVS.cpp, servoAfma6AprilTagPBVS.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoAfma6FourPoints2DCamVelocityLs_des.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6MegaposePBVS.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoAfma6TwoLines2DCamVelocity.cpp, testRobotAfma6.cpp, testRobotAfma6Pose.cpp, and testVirtuoseAfma6.cpp.

Definition at line 208 of file vpRobotAfma6.h.

Member Enumeration Documentation

◆ vpAfma6ToolType

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.

TOOL_INTEL_D435_CAMERA 

Intel D435 camera

TOOL_CUSTOM 

A user defined tool.

Definition at line 123 of file vpAfma6.h.

◆ vpControlFrameType

Robot control frames.

Enumerator
REFERENCE_FRAME 

Corresponds to a fixed reference frame attached to the robot structure.

ARTICULAR_FRAME 

Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE.

JOINT_STATE 

Corresponds to the joint state.

END_EFFECTOR_FRAME 

Corresponds to robot end-effector frame.

CAMERA_FRAME 

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

TOOL_FRAME 

Corresponds to a frame attached to the tool (camera, gripper...) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME.

MIXT_FRAME 

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

Definition at line 74 of file vpRobot.h.

◆ vpRobotStateType

enum vpRobot::vpRobotStateType
inherited

Robot control states.

Enumerator
STATE_STOP 

Stops robot motion especially in velocity and acceleration control.

STATE_VELOCITY_CONTROL 

Initialize the velocity controller.

STATE_POSITION_CONTROL 

Initialize the position controller.

STATE_ACCELERATION_CONTROL 

Initialize the acceleration controller.

STATE_FORCE_TORQUE_CONTROL 

Initialize the force/torque controller.

Definition at line 62 of file vpRobot.h.

Constructor & Destructor Documentation

◆ vpRobotAfma6()

vpRobotAfma6::vpRobotAfma6 ( bool  verbose = true)
explicit

The only available constructor.

This constructor 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 distortion, use the code below:

// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distortion parameter

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

robot.getCameraParameters(cam, I);
// In cam, you get the intrinsic parameters of the projection model
// with distortion.
See also
vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType)

Definition at line 156 of file vpRobotAfma6.cpp.

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

◆ ~vpRobotAfma6()

vpRobotAfma6::~vpRobotAfma6 ( void  )
virtual

Destructor.

Free allocated resources.

Definition at line 558 of file vpRobotAfma6.cpp.

References setRobotState(), and vpRobot::STATE_STOP.

Member Function Documentation

◆ checkJointLimits()

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

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

◆ closeGripper()

void vpRobotAfma6::closeGripper ( )

Close the pneumatic CCMOP gripper.

See also
openGripper()

Definition at line 2164 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

◆ get_cMe()

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.

Definition at line 815 of file vpRobotAfma6.cpp.

References vpAfma6::get_cMe().

Referenced by setVelocity().

◆ get_cVe()

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.

Definition at line 797 of file vpRobotAfma6.cpp.

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

◆ get_eJe() [1/2]

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 successives rotations expressed in radians.
eJe: Robot jacobian expressed in the end-effector frame.

Definition at line 937 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_long_56, and vpArray2D< Type >::resize().

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

◆ get_eJe() [2/2]

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.

Definition at line 827 of file vpRobotAfma6.cpp.

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

◆ get_eMc()

vpHomogeneousMatrix vpAfma6::get_eMc ( ) const
inherited

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

Returns
Transformation between the end-effector frame and the camera frame.

Definition at line 904 of file vpAfma6.cpp.

References vpAfma6::_eMc.

◆ get_fJe() [1/2]

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 successives rotations expressed in radians.
fJe: Robot jacobian expressed in the robot reference frame.

Definition at line 1007 of file vpAfma6.cpp.

References vpAfma6::_coupl_56, vpAfma6::_long_56, and vpArray2D< Type >::resize().

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

◆ get_fJe() [2/2]

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

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

◆ get_fMc() [1/2]

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

Definition at line 780 of file vpAfma6.cpp.

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

◆ get_fMc() [2/2]

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 successives rotations expressed in radians.
fMcThe homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame (fMc).

Definition at line 807 of file vpAfma6.cpp.

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

◆ 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 successives rotations expressed in radians.
fMeThe 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 840 of file vpAfma6.cpp.

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

Referenced by vpAfma6::get_fMc().

◆ getCameraParameters() [1/3]

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.
Thid method needs also an access to the files containing the camera parameters in XML format. This access is available if VISP_HAVE_AFMA6_DATA macro is defined in include/visp3/core/vpConfig.h file.
  • If VISP_HAVE_AFMA6_DATA is defined, this method gets the camera parameters from const_camera_Afma6.xml config file.
  • If this macro is not defined, this method sets 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 <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
// Acquire an image to update image structure
g.acquire(I) ;
vpRobotAfma6 robot;
// 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_AFMA6_DATA macro is 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
}
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
unsigned int getWidth() const
Definition: vpImage.h:249
unsigned int getHeight() const
Definition: vpImage.h:184
Exceptions
vpRobotException::readingParametersError: If the camera parameters are not found.
Examples
testAfma6.cpp, and testRobotAfma6.cpp.

Definition at line 1261 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_INTEL_D435_CAMERA_NAME, vpAfma6::CONST_VACUUM_CAMERA_NAME, vpAfma6::getToolType(), vpCameraParameters::initPersProjWithDistortion(), vpCameraParameters::initPersProjWithoutDistortion(), vpException::notImplementedError, vpXmlParserCamera::parse(), vpCameraParameters::perspectiveProjWithDistortion, vpCameraParameters::perspectiveProjWithoutDistortion, vpAfma6::projModel, vpCameraParameters::ProjWithKannalaBrandtDistortion, vpRobotException::readingParametersError, vpXmlParserCamera::SEQUENCE_OK, vpAfma6::TOOL_CCMOP, vpAfma6::TOOL_GENERIC_CAMERA, vpAfma6::TOOL_GRIPPER, vpAfma6::TOOL_INTEL_D435_CAMERA, vpAfma6::TOOL_VACUUM, vpERROR_TRACE, and vpTRACE.

Referenced by vpAfma6::getCameraParameters().

◆ getCameraParameters() [2/3]

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 <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
// Acquire an image to update image structure
g.acquire(I) ;
vpRobotAfma6 robot;
// 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 1503 of file vpAfma6.cpp.

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

◆ getCameraParameters() [3/3]

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 <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
#if defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_AFMA6)
// Acquire an image to update image structure
g.acquire(I) ;
vpRobotAfma6 robot;
// 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 1550 of file vpAfma6.cpp.

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

◆ getCameraParametersProjType()

vpCameraParameters::vpCameraParametersProjType vpAfma6::getCameraParametersProjType ( ) const
inlineinherited

Get the current camera model projection type.

Definition at line 168 of file vpAfma6.h.

◆ getCoupl56()

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 1078 of file vpAfma6.cpp.

References vpAfma6::_coupl_56.

◆ getDisplacement()

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
  1. Translations are expressed in meters, rotations in radians.

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

Implements vpRobot.

Definition at line 2194 of file vpRobotAfma6.cpp.

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

◆ getForwardKinematics()

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 successives 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 519 of file vpAfma6.cpp.

References vpAfma6::get_fMc().

◆ getInverseKinematics()

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 successives 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 <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
vpColVector q1(6), q2(6);
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
}
Implementation of an homogeneous matrix and operations on such kind of matrices.
See also
getForwardKinematics()

Definition at line 599 of file vpAfma6.cpp.

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

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

◆ getJointMax()

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 1064 of file vpAfma6.cpp.

References vpAfma6::_joint_max.

◆ getJointMin()

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 1048 of file vpAfma6.cpp.

References vpAfma6::_joint_min.

◆ getLong56()

double vpAfma6::getLong56 ( ) const
inherited

Return the distance between join 5 and 6.

Returns
Distance between join 5 and 6.

Definition at line 1086 of file vpAfma6.cpp.

References vpAfma6::_long_56.

◆ getMaxRotationVelocity()

◆ getMaxTranslationVelocity()

◆ getNDof()

int vpRobot::getNDof ( ) const
inlineinherited

Return robot degrees of freedom number.

Examples
servoPololuPtuPoint2DJointVelocity.cpp.

Definition at line 143 of file vpRobot.h.

◆ getPosition() [1/5]

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

Return the current robot position in the specified frame.

Definition at line 213 of file vpRobot.cpp.

References vpRobot::getPosition().

◆ getPosition() [2/5]

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

Definition at line 1510 of file vpRobotAfma6.cpp.

Referenced by getPosition().

◆ getPosition() [3/5]

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 vpHomogeneousMatrix:
timestamp: Time in second since last robot power on.
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 homogeneous matrix
vpHomogeneousMatrix fMc(ftc, fRc);
@ REFERENCE_FRAME
Definition: vpRobot.h:76
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
Class that consider the case of a translation vector.
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 1426 of file vpRobotAfma6.cpp.

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

◆ getPosition() [4/5]

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

Get the current position of the robot.

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

The difference is here that the timestamp is not used.

Definition at line 1553 of file vpRobotAfma6.cpp.

References getPosition().

◆ getPosition() [5/5]

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

References getPosition().

◆ getPositioningVelocity()

double vpRobotAfma6::getPositioningVelocity ( void  )

Get the maximal velocity percentage used for a position control.

See also
setPositioningVelocity()

Definition at line 930 of file vpRobotAfma6.cpp.

◆ getPowerState()

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

References vpRobotException::lowLevelError, and vpERROR_TRACE.

◆ getRobotFrame()

vpControlFrameType vpRobot::getRobotFrame ( void  ) const
inlineprotectedinherited

◆ getRobotState()

virtual vpRobotStateType vpRobot::getRobotState ( void  ) const
inlinevirtualinherited

Definition at line 153 of file vpRobot.h.

Referenced by vpRobotBiclops::getPosition(), vpRobotBiclops::getVelocity(), vpRobotCamera::setPosition(), vpSimulatorCamera::setPosition(), vpRobotAfma4::setPosition(), setPosition(), vpRobotFranka::setPosition(), vpRobotUniversalRobots::setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotBiclops::setPosition(), vpRobotPololuPtu::setPosition(), vpRobotPtu46::setPosition(), vpSimulatorAfma6::setPosition(), vpSimulatorViper850::setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpSimulatorAfma6::setRobotState(), vpSimulatorViper850::setRobotState(), vpRobotAfma4::setRobotState(), setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), vpRobotPtu46::setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotBiclops::setVelocity(), vpRobotPololuPtu::setVelocity(), vpRobotPtu46::setVelocity(), vpRobotCamera::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), vpRobotFlirPtu::stopMotion(), vpRobotFranka::stopMotion(), vpRobotViper650::stopMotion(), vpRobotViper850::stopMotion(), vpSimulatorAfma6::stopMotion(), and vpSimulatorViper850::stopMotion().

◆ getTime()

double vpRobotAfma6::getTime ( ) const

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

Definition at line 1493 of file vpRobotAfma6.cpp.

◆ getToolType()

vpAfma6ToolType vpAfma6::getToolType ( ) const
inlineinherited

Get the current tool type.

Definition at line 166 of file vpAfma6.h.

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

◆ getVelocity() [1/4]

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

References getVelocity().

◆ getVelocity() [2/4]

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
// Moves the joint in velocity
// 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]
}
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
VISP_EXPORT int wait(double t0, double t)

Definition at line 1914 of file vpRobotAfma6.cpp.

References getVelocity().

◆ getVelocity() [3/4]

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.

Definition at line 1867 of file vpRobotAfma6.cpp.

Referenced by getVelocity().

◆ getVelocity() [4/4]

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
// Moves the joint in velocity
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 1756 of file vpRobotAfma6.cpp.

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

◆ init() [1/6]

void vpAfma6::init ( const std::string &  camera_extrinsic_parameters)
inherited

Read files containing the constant parameters related to the robot tools in order to set the end-effector to tool transformation.

Parameters
camera_extrinsic_parameters: Filename containing the camera extrinsic parameters.

Definition at line 227 of file vpAfma6.cpp.

References vpAfma6::parseConfigFile().

◆ init() [2/6]

void vpAfma6::init ( const std::string &  camera_extrinsic_parameters,
const std::string &  camera_intrinsic_parameters 
)
inherited

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

Parameters
camera_extrinsic_parameters: Filename containing the constant parameters of the robot kinematics $^e{\bf M}_c $ transformation.
camera_intrinsic_parameters: Filename containing the camera extrinsic parameters.

Definition at line 175 of file vpAfma6.cpp.

References vpAfma6::parseConfigFile().

◆ init() [3/6]

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 distortion by calling init(vpAfma6::defaultCameraRobot). If you want to set the extrinsic camera parameters to those obtained with a camera perspective model including the distortion you have to call the init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType) method.
See also
vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType, vpCameraParameters::vpCameraParametersProjType)

Implements vpRobot.

Examples
testRobotAfma6.cpp.

Definition at line 225 of file vpRobotAfma6.cpp.

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

Referenced by vpRobotAfma6().

◆ init() [4/6]

void vpRobotAfma6::init ( vpAfma6::vpAfma6ToolType  tool,
const std::string &  filename 
)

Initialize the robot kinematics (set the eMc homogeneous parameters in the low level controller) from a file and also get the joint limits from the low-level controller.

Parameters
tool: Tool to use.
filename: Path of the configuration file containing the transformation between the end-effector frame and the tool frame.

To set the transformation parameters related to the $^e{\bf M}_c$ matrix, use the code below:

#include <visp3/robot/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
vpRobotAfma6 robot;
// Set the transformation between the end-effector frame
// and the tool frame from a file
std::string filename("./EffectorToolTransformation.cnf");
robot.init(vpAfma6::TOOL_CUSTOM, filename);
#endif
}
@ TOOL_CUSTOM
Definition: vpAfma6.h:129

The configuration file should have the form below:

# Start with any number of consecutive lines
# beginning with the symbol '#'
#
# The 3 following lines contain the name of the camera,
# the rotation parameters of the geometric transformation
# using the Euler angles in degrees with convention XYZ and
# the translation parameters expressed in meters
CAMERA CameraName
eMc_ROT_XYZ 10.0 -90.0 20.0
eMc_TRANS_XYZ 0.05 0.01 0.06
See also
init(), init(vpAfma6::vpAfma6ToolType, vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpAfma6ToolType, const vpHomogeneousMatrix&)

Definition at line 520 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().

◆ init() [5/6]

void vpRobotAfma6::init ( vpAfma6::vpAfma6ToolType  tool,
const vpHomogeneousMatrix eMc 
)

Initialize the robot kinematics with user defined parameters (set the eMc homogeneous parameters in the low level controller) and also get the joint limits from the low-level controller.

Parameters
tool: Tool to use.
eMc: Transformation between the end-effector frame and the tool frame.

To set the transformation parameters related to the $^e{\bf M}_c$ matrix, use the code below:

#include <visp3/robot/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
vpRobotAfma6 robot;
// Set the transformation between the end-effector frame
// and the tool frame.
vpHomogeneousMatrix eMc(0.001, 0.0, 0.1, 0.0, 0.0, M_PI/2);
#endif
}
See also
vpCameraParameters, init(), init(vpAfma6::vpAfma6ToolType, vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpAfma6ToolType, const std::string&)

Definition at line 442 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().

◆ init() [6/6]

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 distortion, use the code below:

// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distortion parameter

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

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

Definition at line 345 of file vpRobotAfma6.cpp.

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

◆ move() [1/2]

void vpRobotAfma6::move ( const std::string &  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 2107 of file vpRobotAfma6.cpp.

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

◆ move() [2/2]

void vpRobotAfma6::move ( const std::string &  filename,
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 2129 of file vpRobotAfma6.cpp.

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

◆ openGripper()

void vpRobotAfma6::openGripper ( )

Open the pneumatic CCMOP gripper.

See also
closeGripper()

Definition at line 2145 of file vpRobotAfma6.cpp.

References vpRobotException::lowLevelError, and vpERROR_TRACE.

◆ parseConfigFile()

void vpAfma6::parseConfigFile ( const std::string &  filename)
inherited

This function gets the robot constant parameters from a file.

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 homogeneous matrix.

Definition at line 1098 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(), and vpRobotException::readingParametersError.

Referenced by vpAfma6::init().

◆ powerOff()

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

References vpRobotException::lowLevelError, and vpERROR_TRACE.

◆ powerOn()

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

References vpRobotException::lowLevelError, and vpERROR_TRACE.

Referenced by setRobotState().

◆ readPosFile()

bool vpRobotAfma6::readPosFile ( const std::string &  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.

vpColVector q; // Joint position
robot.readPosFile("myposition.pos", q); // Set the joint position from the file
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 1988 of file vpRobotAfma6.cpp.

References vpAfma6::njoint, vpMath::rad(), vpColVector::resize(), and vpIoTools::splitChain().

Referenced by move(), and setPosition().

◆ saturateVelocities()

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

Saturate velocities.

Parameters
v_in: Vector of input velocities to saturate. Translation velocities should be expressed in m/s while rotation velocities in rad/s.
v_max: Vector of maximal allowed velocities. Maximal translation velocities should be expressed in m/s while maximal rotation velocities in rad/s.
verbose: Print a message indicating which axis causes the saturation.
Returns
Saturated velocities.
Exceptions
vpRobotException::dimensionError: If the input vectors have different dimensions.

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

#include <iostream>
#include <visp3/robot/vpRobot.h>
int main()
{
// Set a velocity skew vector
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;
}
vpRowVector t() const
static double rad(double deg)
Definition: vpMath.h:127
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160

Definition at line 160 of file vpRobot.cpp.

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

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

◆ savePosFile()

bool vpRobotAfma6::savePosFile ( const std::string &  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 2072 of file vpRobotAfma6.cpp.

References vpMath::deg().

◆ set_eMc()

void vpRobotAfma6::set_eMc ( const vpHomogeneousMatrix eMc)
virtual

Set the geometric transformation between the end-effector frame and the tool frame in the low level controller.

Warning
This function overwrite the transformation parameters that were potentially set using one of the init functions
Parameters
eMc: Transformation between the end-effector frame and the tool frame.

Reimplemented from vpAfma6.

Definition at line 389 of file vpRobotAfma6.cpp.

References vpAfma6::_erc, vpAfma6::_etc, and vpAfma6::set_eMc().

◆ setMaxRotationVelocity()

void vpRobot::setMaxRotationVelocity ( double  w_max)
inherited

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

Parameters
w_max: Maximum rotational velocity expressed in rad/s.
Examples
servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, testFeatureSegment.cpp, testFrankaJointVelocityLimits.cpp, and testRobotFlirPtu.cpp.

Definition at line 257 of file vpRobot.cpp.

References vpRobot::maxRotationVelocity.

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

◆ setMaxTranslationVelocity()

void vpRobot::setMaxTranslationVelocity ( double  v_max)
inherited

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

Parameters
v_max: Maximum translation velocity expressed in m/s.
Examples
servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, and testFeatureSegment.cpp.

Definition at line 236 of file vpRobot.cpp.

References vpRobot::maxTranslationVelocity.

Referenced by vpSimulatorAfma6::setPosition().

◆ setPosition() [1/4]

void vpRobotAfma6::setPosition ( const std::string &  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 <visp3/core/vpColVector.h>
#include <visp3/robot/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
vpRobotAfma6 robot;
vpColVector q; // joint position
robot.readPosFile("MyPositionFilename.pos", q);
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
return 0;
#endif
}
Exceptions
vpRobotException::lowLevelError: vpRobot::MIXT_FRAME and vpRobot::END_EFFECTOR_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
See also
setPositioningVelocity()

Definition at line 1357 of file vpRobotAfma6.cpp.

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

◆ setPosition() [2/4]

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 and vpRobot::END_EFFECTOR_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
#include <visp3/core/vpColVector.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/robot/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;
// 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
}
@ CAMERA_FRAME
Definition: vpRobot.h:82

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) {
std::cout << "The position is out of range" << std::endl;
}
}
int getCode() const
Definition: vpException.cpp:68
Error that can be emitted by the vpRobot class and its derivatives.
@ positionOutOfRangeError
Position is out of range.

Implements vpRobot.

Definition at line 1108 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_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.

◆ setPosition() [3/4]

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, vpRobot::END_EFFECTOR_FRAME and vpRobot::ARTICULAR_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
#include <visp3/core/vpPoseVector.h>
#include <visp3/robot/vpRobotAfma6.h>
int main()
{
#ifdef VISP_HAVE_AFMA6
// 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;
// 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
}
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189

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) {
std::cout << "The position is out of range" << std::endl;
}
}

Definition at line 1006 of file vpRobotAfma6.cpp.

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

Referenced by move(), and setPosition().

◆ setPosition() [4/4]

void vpRobotAfma6::setPosition ( const vpRobot::vpControlFrameType  frame,
double  pos1,
double  pos2,
double  pos3,
double  pos4,
double  pos5,
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 and vpRobot::END_EFFECTOR_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
#include <visp3/robot/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;
// 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 1297 of file vpRobotAfma6.cpp.

References setPosition(), and vpERROR_TRACE.

◆ setPositioningVelocity()

void vpRobotAfma6::setPositioningVelocity ( 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
// 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 923 of file vpRobotAfma6.cpp.

Referenced by move().

◆ setRobotFrame()

◆ setRobotState()

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

Change the robot state.

Parameters
newState: New requested robot state.

Reimplemented from vpRobot.

Definition at line 591 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().

◆ setToolType()

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

Set the current tool type.

Definition at line 191 of file vpAfma6.h.

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

◆ setVelocity()

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, where $\dot{q}_1, \dot{q}_2, \dot{q}_3 $ are the translation velocities in m/s and $\dot{q}_4, \dot{q}_5, \dot{q}_6$ are the rotation velocities in rad/s.
  • In camera frame, $ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c} \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t $ is a velocity twist vector expressed in the camera frame, with translations velocities $ ^{c} v_x, ^{c} v_y, ^{c} v_z $ in m/s and rotation velocities $ ^{c}\omega_x, ^{c} \omega_y, ^{c} \omega_z $ in rad/s.
  • In reference frame, $ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r} \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t $ is a velocity twist vector expressed in the reference frame, with translations velocities $ ^{c} v_x, ^{c} v_y, ^{c} v_z $ in m/s and rotation velocities $ ^{c}\omega_x, ^{c} \omega_y, ^{c} \omega_z $ in rad/s.
  • In mixt frame, $ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t $ is a velocity twist vector where, translations $ ^{r} v_x, ^{r} v_y, ^{r} v_z $ are expressed in the reference frame in m/s and rotations $ ^{c} \omega_x, ^{c} \omega_y, ^{c} \omega_z $ in the camera frame in rad/s.
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 authorized 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
// Moves the joint in velocity

Implements vpRobot.

Definition at line 1622 of file vpRobotAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpArray2D< Type >::data, vpRobot::END_EFFECTOR_FRAME, get_cMe(), 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.

◆ setVerbose()

◆ stopMotion()

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.

Definition at line 641 of file vpRobotAfma6.cpp.

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

Member Data Documentation

◆ _coupl_56

◆ _eMc

◆ _erc

vpRxyzVector vpAfma6::_erc
protectedinherited

◆ _etc

vpTranslationVector vpAfma6::_etc
protectedinherited

◆ _joint_max

◆ _joint_min

◆ _long_56

◆ areJointLimitsAvailable

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 112 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ CONST_AFMA6_FILENAME

const std::string vpAfma6::CONST_AFMA6_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf")

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, transformation eMc between end-effector and camera frame.

Definition at line 82 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_CAMERA_AFMA6_FILENAME

const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml")

Definition at line 93 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

◆ CONST_CCMOP_CAMERA_NAME

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 99 of file vpAfma6.h.

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

◆ CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf")

Definition at line 84 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf")

Definition at line 83 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf")

Definition at line 92 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf")

Definition at line 91 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf")

Definition at line 86 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf")

Definition at line 85 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf")

Definition at line 90 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf")

Definition at line 89 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf")

Definition at line 88 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME

const std::string vpAfma6::CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
staticinherited
Initial value:
=
std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf")

Definition at line 87 of file vpAfma6.h.

Referenced by vpAfma6::init().

◆ CONST_GENERIC_CAMERA_NAME

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 114 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

◆ CONST_GRIPPER_CAMERA_NAME

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 104 of file vpAfma6.h.

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

◆ CONST_INTEL_D435_CAMERA_NAME

const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435"
staticinherited

Name of the Intel D435 camera attached to the robot hand (vpAfma6ToolType::TOOL_INTEL_D435_CAMERA).

Definition at line 120 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

◆ CONST_VACUUM_CAMERA_NAME

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 109 of file vpAfma6.h.

Referenced by vpAfma6::getCameraParameters().

◆ defaultPositioningVelocity

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 249 of file vpRobotAfma6.h.

Referenced by vpRobotAfma6().

◆ defaultTool

const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP
staticinherited

Default tool attached to the robot end effector.

Definition at line 133 of file vpAfma6.h.

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

◆ eJe

◆ eJeAvailable

int vpRobot::eJeAvailable
protectedinherited

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

Definition at line 106 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ fJe

vpMatrix vpRobot::fJe
protectedinherited

◆ fJeAvailable

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 110 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ maxRotationVelocity

◆ maxRotationVelocityDefault

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

Definition at line 99 of file vpRobot.h.

Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().

◆ maxTranslationVelocity

double vpRobot::maxTranslationVelocity
protectedinherited

◆ maxTranslationVelocityDefault

const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

Definition at line 97 of file vpRobot.h.

Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().

◆ nDof

◆ njoint

const unsigned int vpAfma6::njoint = 6
staticinherited

◆ projModel

vpCameraParameters::vpCameraParametersProjType vpAfma6::projModel
protectedinherited

◆ qmax

double* vpRobot::qmax
protectedinherited

Definition at line 114 of file vpRobot.h.

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

◆ qmin

double* vpRobot::qmin
protectedinherited

Definition at line 113 of file vpRobot.h.

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

◆ tool_current

vpAfma6ToolType vpAfma6::tool_current
protectedinherited

Current tool in use.

Definition at line 210 of file vpAfma6.h.

◆ verbose_