Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpSimulatorAfma6 Class Reference

#include <vpSimulatorAfma6.h>

Inheritance diagram for vpSimulatorAfma6:

Public Types

enum  vpDisplayRobotType { MODEL_3D , MODEL_DH }
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
}
enum  vpAfma6ToolType {
  TOOL_CCMOP , TOOL_GRIPPER , TOOL_VACUUM , TOOL_GENERIC_CAMERA ,
  TOOL_INTEL_D435_CAMERA , TOOL_CUSTOM
}

Public Member Functions

 vpSimulatorAfma6 ()
VP_EXPLICIT vpSimulatorAfma6 (bool display)
virtual ~vpSimulatorAfma6 () VP_OVERRIDE
void getCameraParameters (vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getCameraParameters (vpCameraParameters &cam, const vpImage< unsigned char > &I)
void getCameraParameters (vpCameraParameters &cam, const vpImage< vpRGBa > &I)
void getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &displacement) VP_OVERRIDE
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp)
void getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position)
void getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
double getPositioningVelocity (void)
void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &q)
void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp)
vpColVector getVelocity (const vpRobot::vpControlFrameType frame)
vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double &timestamp)
void get_cMe (vpHomogeneousMatrix &cMe)
void get_cVe (vpVelocityTwistMatrix &cVe)
void get_eJe (vpMatrix &eJe) VP_OVERRIDE
void get_fJe (vpMatrix &fJe) VP_OVERRIDE
void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion)
bool initialiseCameraRelativeToObject (const vpHomogeneousMatrix &cMo)
void initialiseObjectRelativeToCamera (const vpHomogeneousMatrix &cMo)
void move (const char *filename)
void setCameraParameters (const vpCameraParameters &cam)
void setJointLimit (const vpColVector &limitMin, const vpColVector &limitMax)
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q) VP_OVERRIDE
void setPosition (const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3, double pos4, double pos5, double pos6)
void setPosition (const char *filename)
void setPositioningVelocity (double vel)
bool setPosition (const vpHomogeneousMatrix &cdMo, vpImage< unsigned char > *Iint=nullptr, const double &errMax=0.001)
vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState) VP_OVERRIDE
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &velocity) VP_OVERRIDE
void stopMotion ()
Inherited functionalities from vpRobotWireFrameSimulator
vpCameraParameters getExternalCameraParameters () const
vpHomogeneousMatrix getExternalCameraPosition () const
void getInternalView (vpImage< vpRGBa > &I)
void getInternalView (vpImage< unsigned char > &I)
vpHomogeneousMatrix get_cMo ()
vpHomogeneousMatrix get_fMo () const
void initScene (const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void initScene (const char *obj, const char *desiredObject)
void initScene (const vpSceneObject &obj)
void initScene (const char *obj)
void setCameraColor (const vpColor &col)
void setConstantSamplingTimeMode (const bool _constantSamplingTimeMode)
void setCurrentViewColor (const vpColor &col)
void setDesiredViewColor (const vpColor &col)
void setDesiredCameraPosition (const vpHomogeneousMatrix &cdMo_)
void setDisplayRobotType (const vpDisplayRobotType dispType)
void setExternalCameraPosition (const vpHomogeneousMatrix &camMf_)
void setGraphicsThickness (unsigned int thickness)
void setSamplingTime (const double &delta_t) VP_OVERRIDE
void setSingularityManagement (bool sm)
void setVerbose (bool verbose)
void set_fMo (const vpHomogeneousMatrix &fMo_)
Inherited functionalities from vpRobotSimulator
double getSamplingTime () const
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)
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)
void init (vpAfma6::vpAfma6ToolType tool, const std::string &filename)
void init (vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc_)
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_cMe (vpHomogeneousMatrix &cMe) const
void get_cVe (vpVelocityTwistMatrix &cVe) 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)
virtual void set_eMc (const vpHomogeneousMatrix &eMc)

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)

Public Attributes

vpImage< vpRGBaI

Static Public Attributes

static const double defaultPositioningVelocity = 25.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 Types

enum  vpSceneObject {
  THREE_PTS , CUBE , PLATE , SMALL_PLATE ,
  RECTANGLE , SQUARE_10CM , DIAMOND , TRAPEZOID ,
  THREE_LINES , ROAD , TIRE , PIPE ,
  CIRCLE , SPHERE , CYLINDER , PLAN ,
  POINT_CLOUD
}
enum  vpSceneDesiredObject { D_STANDARD , D_CIRCLE , D_TOOL }
enum  vpCameraTrajectoryDisplayType { CT_LINE , CT_POINT }

Protected Member Functions

Protected Member Functions Inherited from vpSimulatorAfma6
void computeArticularVelocity () VP_OVERRIDE
void compute_fMi ()
void findHighestPositioningSpeed (vpColVector &q)
void getExternalImage (vpImage< vpRGBa > &I)
void get_fMi (vpHomogeneousMatrix *fMit) VP_OVERRIDE
void init () VP_OVERRIDE
void initArms () VP_OVERRIDE
void initDisplay ()
int isInJointLimit () VP_OVERRIDE
bool singularityTest (const vpColVector &q, vpMatrix &J)
void updateArticularPosition () VP_OVERRIDE
Protected Member Functions Inherited from vpWireFrameSimulator
void display_scene (Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
void display_scene (Matrix mat, Bound_scene &sc, const vpImage< unsigned char > &I, const vpColor &color)
vpHomogeneousMatrix navigation (const vpImage< vpRGBa > &I, bool &changed)
vpHomogeneousMatrix navigation (const vpImage< unsigned char > &I, bool &changed)
vpImagePoint projectCameraTrajectory (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
vpImagePoint projectCameraTrajectory (const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo)
vpImagePoint projectCameraTrajectory (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf)
vpImagePoint projectCameraTrajectory (const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf)
Protected Member Functions Inherited from vpRobot
vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame)
vpControlFrameType getRobotFrame (void) const

Protected Attributes

double tcur
double tprev
Bound_scene * robotArms
unsigned int size_fMi
vpHomogeneousMatrixfMi
vpColVector artCoord
vpColVector artVel
vpColVector velocity
std::thread * m_thread
std::mutex m_mutex_fMi
std::mutex m_mutex_eMc
std::mutex m_mutex_artVel
std::mutex m_mutex_artCoord
std::mutex m_mutex_velocity
std::mutex m_mutex_display
std::mutex m_mutex_robotStop
std::mutex m_mutex_frame
std::mutex m_mutex_setVelocityCalled
std::mutex m_mutex_scene
bool displayBusy
bool robotStop
bool jointLimit
unsigned int jointLimitArt
bool singularityManagement
vpCameraParameters cameraParam
vpDisplayX display
vpDisplayRobotType displayType
bool displayAllowed
bool constantSamplingTimeMode
bool setVelocityCalled
Bound_scene scene
Bound_scene desiredScene
Bound_scene camera
std::list< vpImageSimulatorobjectImage
vpHomogeneousMatrix fMo
vpHomogeneousMatrix fMc
vpHomogeneousMatrix camMf
vpHomogeneousMatrix refMo
vpHomogeneousMatrix cMo
vpHomogeneousMatrix cdMo
vpSceneObject object
vpSceneDesiredObject desiredObject
vpColor camColor
vpColor camTrajColor
vpColor curColor
vpColor desColor
bool sceneInitialized
bool displayCameraTrajectory
std::list< vpImagePointcameraTrajectory
std::list< vpHomogeneousMatrixposeList
std::list< vpHomogeneousMatrixfMoList
unsigned int nbrPtLimit
vpImagePoint old_iPr
vpImagePoint old_iPz
vpImagePoint old_iPt
bool blockedr
bool blockedz
bool blockedt
bool blocked
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
double px_int
double py_int
double px_ext
double py_ext
bool displayObject
bool displayDesiredObject
bool displayCamera
bool displayImageSimulator
float cameraFactor
vpCameraTrajectoryDisplayType camTrajType
bool extCamChanged
vpHomogeneousMatrix rotz
unsigned int thickness_
double delta_t_
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
void setToolType (vpAfma6::vpAfma6ToolType tool)
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

Inherited functionalities from vpWireFrameSimulator

void getExternalImage (vpImage< unsigned char > &I)
void getExternalImage (vpImage< unsigned char > &I, const vpHomogeneousMatrix &camMf)
void getExternalImage (vpImage< vpRGBa > &I, const vpHomogeneousMatrix &camMf)
vpCameraParameters getExternalCameraParameters (const vpImage< unsigned char > &I) const
vpCameraParameters getExternalCameraParameters (const vpImage< vpRGBa > &I) const
vpHomogeneousMatrix get_cMo () const
void initScene (const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list< vpImageSimulator > &imObj)
void initScene (const char *obj, const char *desiredObject, const std::list< vpImageSimulator > &imObj)
void initScene (const vpSceneObject &obj, const std::list< vpImageSimulator > &imObj)
void initScene (const char *obj, const std::list< vpImageSimulator > &imObj)
void deleteCameraPositionHistory ()
void displayTrajectory (const vpImage< unsigned char > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
void displayTrajectory (const vpImage< vpRGBa > &I, const std::list< vpHomogeneousMatrix > &list_cMo, const std::list< vpHomogeneousMatrix > &list_fMo, const vpHomogeneousMatrix &camMf)
vpCameraParameters getInternalCameraParameters (const vpImage< unsigned char > &I) const
vpCameraParameters getInternalCameraParameters (const vpImage< vpRGBa > &I) const
void getInternalImage (vpImage< unsigned char > &I)
void getInternalImage (vpImage< vpRGBa > &I)
void get_cMo_History (std::list< vpHomogeneousMatrix > &cMo_history)
void get_fMo_History (std::list< vpHomogeneousMatrix > &fMo_history)
void setCameraPositionRelObj (const vpHomogeneousMatrix &cMo_)
void setCameraPositionRelWorld (const vpHomogeneousMatrix &fMc_)
void setCameraSizeFactor (float factor)
void setCameraTrajectoryColor (const vpColor &col)
void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType &camTraj_type)
void setDisplayCameraTrajectory (const bool &do_display)
void setExternalCameraParameters (const vpCameraParameters &cam)
void setInternalCameraParameters (const vpCameraParameters &cam)
void setNbPtTrajectory (unsigned int nbPt)

Protected Member Functions Inherited from vpRobotWireFrameSimulator

vpColVector get_artCoord ()
void set_artCoord (const vpColVector &coord)
vpColVector get_artVel ()
void set_artVel (const vpColVector &vel)
vpColVector get_velocity ()
void set_velocity (const vpColVector &vel)
void set_displayBusy (const bool &status)
bool get_displayBusy ()
static void launcher (vpRobotWireFrameSimulator &simulator)

Detailed Description

Simulator of Irisa's gantry robot named Afma6.

Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's Afma6 robot. This robot is a gantry robot with six degrees of freedom manufactured in 1992 by the french Afma-Robots company.

Warning
This class uses threading capabilities. Thus on Unix-like platforms, the libpthread third-party library need to be installed. On Windows, we use the native threading capabilities.

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.

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

#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
// Set a joint position
q[0] = 0.1; // Joint 1 position, in meter
q[1] = 0.2; // Joint 2 position, in meter
q[2] = 0.3; // Joint 3 position, in meter
q[3] = M_PI/8; // Joint 4 position, in rad
q[4] = M_PI/4; // Joint 5 position, in rad
q[5] = M_PI; // Joint 6 position, 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);
return 0;
}
@ TOOL_CCMOP
Definition vpAfma6.h:125
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
@ ARTICULAR_FRAME
Definition vpRobot.h:77
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65

To control the robot in velocity, you may set the controller to velocity control and then 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:

#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
vpColVector qvel(6);
// Set a joint velocity
qvel[0] = 0.1; // Joint 1 velocity in m/s
qvel[1] = 0.1; // Joint 2 velocity in m/s
qvel[2] = 0.1; // Joint 3 velocity in m/s
qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
qvel[4] = 0; // Joint 5 velocity in rad/s
qvel[5] = 0; // Joint 6 velocity in rad/s
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
for ( ; ; ) {
// 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);
return 0;
}
@ TOOL_GRIPPER
Definition vpAfma6.h:126
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63

It 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 savePosFile() methods.

Tutorials & Examples

Tutorials
To know how this class can be used to achieve a visual servoing simulation, you can follow the tutorial:

Examples
servoSimuAfma6FourPoints2DCamVelocity.cpp, and tutorial-ibvs-4pts-wireframe-robot-afma6.cpp.

Definition at line 185 of file vpSimulatorAfma6.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.

◆ vpCameraTrajectoryDisplayType

Enumerator
CT_LINE 
CT_POINT 

Definition at line 228 of file vpWireFrameSimulator.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 73 of file vpRobot.h.

◆ vpDisplayRobotType

Enumerator
MODEL_3D 
MODEL_DH 

Definition at line 84 of file vpRobotWireFrameSimulator.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 61 of file vpRobot.h.

◆ vpSceneDesiredObject

Type of scene used to display the object at the desired pose (in the internal view).

  • D_STANDARD will use the vpSceneObject used to be the object at the current position.
  • D_OUTIL will display a tool which is attached to the camera.
Enumerator
D_STANDARD 

The object displayed at the desired position is the same than the scene object defined in vpSceneObject.

D_CIRCLE 

The object displayed at the desired position is a circle.

D_TOOL 

A cylindrical tool is attached to the camera.

Definition at line 220 of file vpWireFrameSimulator.h.

◆ vpSceneObject

Type of scene used to display the object at the current position.

Enumerator
THREE_PTS 

A 40cm by 40cm plate with 3 points at coordinates (0,0,0), (0.1,0,0), (0,0.1,0). Each point is represented by a circle with 2cm radius.

CUBE 

A 12.5cm size cube.

PLATE 

A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.

SMALL_PLATE 

4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0), (0.03,0.03,0), (0.03,0.03,0). Each point is represented by a circle with 1cm radius.

RECTANGLE 

A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0.07,-0.05,0), (-0.07,-0.05,0). Each point is represented by a circle with 2cm radius.

SQUARE_10CM 

A 40cm by 40cm plate with 4 points at coordinates (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0), (-0.05,-0.05,0). Each point is represented by a circle with 2cm radius.

DIAMOND 

A 40cm by 40cm plate with 4 points at coordinates (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is represented by a circle with 2cm radius.

TRAPEZOID 

A 40cm by 40cm plate with 4 points at coordinates (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0), (0.025,-0.05,0). Each point is represented by a circle with 2cm radius.

THREE_LINES 

Three parallel lines with equation y=-5, y=0, y=5.

ROAD 

Three parallel lines representing a road.

TIRE 

A tire represented by 2 circles with radius 10cm and 15cm.

PIPE 

A pipe represented by a cylinder of 25 cm length and 15cm radius.

CIRCLE 

A 10cm radius circle.

SPHERE 

A 15cm radius sphere.

CYLINDER 

A cylinder of 80cm length and 10cm radius.

PLAN 

A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.

POINT_CLOUD 

A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.

Definition at line 168 of file vpWireFrameSimulator.h.

Constructor & Destructor Documentation

◆ vpSimulatorAfma6() [1/2]

◆ vpSimulatorAfma6() [2/2]

vpSimulatorAfma6::vpSimulatorAfma6 ( bool do_display)

Constructor used to enable or disable the external view of the robot.

Parameters
do_display: When true, enables the display of the external view.

Definition at line 79 of file vpSimulatorAfma6.cpp.

References compute_fMi(), defaultPositioningVelocity, init(), initDisplay(), vpRobotWireFrameSimulator::launcher(), vpRobotWireFrameSimulator::m_thread, vpTime::measureTimeMs(), vpRobotWireFrameSimulator::tcur, and vpRobotWireFrameSimulator::vpRobotWireFrameSimulator().

◆ ~vpSimulatorAfma6()

Member Function Documentation

◆ compute_fMi()

void vpSimulatorAfma6::compute_fMi ( )
protected

Compute the pose between the robot reference frame and the frames used used to compute the Denavit-Hartenberg representation. The last element of the table corresponds to the pose between the reference frame and the camera frame.

To compute the diferent homogeneous matrices, this function needs the articular coordinates as input.

Finally the output is a table of 8 elements : $ fM1 $, $ fM2 $, $fM3 $, $ fM4 $, $ fM5 $, $ fM6 = fMw $, $ fM7 = fMe $ and $fMc $ - where w is for wrist and e for effector-.

Definition at line 585 of file vpSimulatorAfma6.cpp.

References vpAfma6::_long_56, vpRobotWireFrameSimulator::fMi, vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::get_fMc(), vpRobotWireFrameSimulator::m_mutex_eMc, and vpRobotWireFrameSimulator::m_mutex_fMi.

Referenced by initialiseCameraRelativeToObject(), updateArticularPosition(), vpSimulatorAfma6(), and vpSimulatorAfma6().

◆ computeArticularVelocity()

◆ deleteCameraPositionHistory()

void vpWireFrameSimulator::deleteCameraPositionHistory ( )
inlineinherited

Delete the history of the main camera position which are displayed in the external views.

Definition at line 303 of file vpWireFrameSimulator.h.

References cameraTrajectory, fMoList, and poseList.

◆ display_scene() [1/2]

void vpWireFrameSimulator::display_scene ( Matrix mat,
Bound_scene & sc,
const vpImage< unsigned char > & I,
const vpColor & color )
protectedinherited

Definition at line 119 of file vpWireFrameSimulator.cpp.

References vpDisplay::displayLine(), and thickness_.

◆ display_scene() [2/2]

◆ displayTrajectory() [1/2]

void vpWireFrameSimulator::displayTrajectory ( const vpImage< unsigned char > & I,
const std::list< vpHomogeneousMatrix > & list_cMo,
const std::list< vpHomogeneousMatrix > & list_fMo,
const vpHomogeneousMatrix & cMf )
inherited

Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.

The two lists must have the same size of homogeneous matrices must have the same size.

Parameters
I: The image where the trajectory is displayed.
list_cMo: The homogeneous matrices list containing the position of the camera relative to the object.
list_fMo: The homogeneous matrices list containing the position of the object relative to the world reference frame.
cMf: A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.

Definition at line 1278 of file vpWireFrameSimulator.cpp.

References camTrajColor, camTrajType, CT_LINE, CT_POINT, vpException::dimensionError, vpDisplay::displayLine(), vpDisplay::displayPoint(), projectCameraTrajectory(), rotz, and thickness_.

◆ displayTrajectory() [2/2]

void vpWireFrameSimulator::displayTrajectory ( const vpImage< vpRGBa > & I,
const std::list< vpHomogeneousMatrix > & list_cMo,
const std::list< vpHomogeneousMatrix > & list_fMo,
const vpHomogeneousMatrix & cMf )
inherited

Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.

The two lists must have the same size of homogeneous matrices must have the same size.

Parameters
I: The image where the trajectory is displayed.
list_cMo: The homogeneous matrices list containing the position of the camera relative to the object.
list_fMo: The homogeneous matrices list containing the position of the object relative to the world reference frame.
cMf: A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.

Definition at line 1325 of file vpWireFrameSimulator.cpp.

References camTrajColor, camTrajType, CT_LINE, CT_POINT, vpException::dimensionError, vpDisplay::displayLine(), vpDisplay::displayPoint(), projectCameraTrajectory(), rotz, and thickness_.

◆ findHighestPositioningSpeed()

void vpSimulatorAfma6::findHighestPositioningSpeed ( vpColVector & q)
protected

◆ get_artCoord()

◆ get_artVel()

vpColVector vpRobotWireFrameSimulator::get_artVel ( )
inlineprotectedinherited

◆ get_cMe() [1/2]

void vpAfma6::get_cMe ( vpHomogeneousMatrix & cMe) const
inherited

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

References _eMc.

Referenced by vpRobotAfma6::get_cMe(), vpSimulatorAfma6::get_cMe(), get_cVe(), vpRobotAfma6::get_cVe(), and vpSimulatorAfma6::get_cVe().

◆ get_cMe() [2/2]

void vpSimulatorAfma6::get_cMe ( vpHomogeneousMatrix & cMe)

Get the geometric transformation $^c{\bf M}_e$ 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 2080 of file vpSimulatorAfma6.cpp.

References vpAfma6::get_cMe().

Referenced by getExternalImage().

◆ get_cMo() [1/2]

vpHomogeneousMatrix vpRobotWireFrameSimulator::get_cMo ( )
inherited

Get the pose between the object and the robot's camera.

Returns
The pose between the object and the fixed world frame.

Definition at line 345 of file vpRobotWireFrameSimulator.cpp.

References vpWireFrameSimulator::fMo, get_fMi(), vpHomogeneousMatrix::inverse(), and size_fMi.

Referenced by vpSimulatorAfma6::setPosition().

◆ get_cMo() [2/2]

vpHomogeneousMatrix vpWireFrameSimulator::get_cMo ( ) const
inlineinherited

Get the pose between the object and the camera.

Returns
The pose between between the object and the camera.

Definition at line 414 of file vpWireFrameSimulator.h.

References cMo, and rotz.

◆ get_cMo_History()

void vpWireFrameSimulator::get_cMo_History ( std::list< vpHomogeneousMatrix > & cMo_history)
inlineinherited

Get the homogeneous matrices cMo stored to display the camera trajectory.

Parameters
cMo_history: The list of the homogeneous matrices cMo.

Definition at line 422 of file vpWireFrameSimulator.h.

References poseList, and rotz.

◆ get_cVe() [1/2]

void vpAfma6::get_cVe ( vpVelocityTwistMatrix & cVe) const
inherited

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

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

◆ get_cVe() [2/2]

void vpSimulatorAfma6::get_cVe ( vpVelocityTwistMatrix & cVe)

Get the twist transformation $^c{\bf V}_e$ 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 2089 of file vpSimulatorAfma6.cpp.

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

◆ get_displayBusy()

bool vpRobotWireFrameSimulator::get_displayBusy ( )
inlineprotectedinherited

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

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

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

◆ get_eJe() [2/2]

void vpSimulatorAfma6::get_eJe ( vpMatrix & eJe_)
virtual

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

To compute $^e{\bf J}_e$, we communicate with the low level controller to get the joint position of the robot.

Parameters
eJe_: Robot jacobian $^e{\bf J}_e$ expressed in the end-effector frame.

Implements vpRobot.

Definition at line 2106 of file vpSimulatorAfma6.cpp.

References vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::get_eJe(), 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 901 of file vpAfma6.cpp.

References _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 1004 of file vpAfma6.cpp.

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

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

◆ get_fJe() [2/2]

void vpSimulatorAfma6::get_fJe ( vpMatrix & fJe_)
virtual

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

To compute $^f{\bf J}_e$, we communicate with the low level controller to get the joint position of the robot.

Parameters
fJe_: Robot jacobian $^f{\bf J}_e$ expressed in the reference frame.

Implements vpRobot.

Definition at line 2127 of file vpSimulatorAfma6.cpp.

References vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::get_fJe(), 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 777 of file vpAfma6.cpp.

References get_fMc().

Referenced by vpSimulatorAfma6::compute_fMi(), get_fMc(), vpRobotAfma6::getDisplacement(), getForwardKinematics(), vpRobotAfma6::getPosition(), vpSimulatorAfma6::getPosition(), vpRobotAfma6::getVelocity(), vpRobotAfma6::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 804 of file vpAfma6.cpp.

References _eMc, and 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 837 of file vpAfma6.cpp.

References _coupl_56, and _long_56.

Referenced by get_fMc().

◆ get_fMi()

void vpSimulatorAfma6::get_fMi ( vpHomogeneousMatrix * fMit)
inlineprotectedvirtual

Get a table of poses between the reference frame and the frames you used to compute the Denavit-Hartenberg representation

Implements vpRobotWireFrameSimulator.

Definition at line 258 of file vpSimulatorAfma6.h.

References vpRobotWireFrameSimulator::fMi, get_fMi(), and vpRobotWireFrameSimulator::m_mutex_fMi.

Referenced by get_fMi(), getExternalImage(), initialiseObjectRelativeToCamera(), and updateArticularPosition().

◆ get_fMo()

vpHomogeneousMatrix vpRobotWireFrameSimulator::get_fMo ( ) const
inlineinherited

Get the pose between the object and the fixed world frame.

Returns
The pose between the object and the fixed world frame.

Definition at line 208 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::fMo.

◆ get_fMo_History()

void vpWireFrameSimulator::get_fMo_History ( std::list< vpHomogeneousMatrix > & fMo_history)
inlineinherited

Get the homogeneous matrices fMo stored to display the camera trajectory.

Parameters
fMo_history: The list of the homogeneous matrices fMo.

Definition at line 443 of file vpWireFrameSimulator.h.

References fMoList.

◆ get_velocity()

vpColVector vpRobotWireFrameSimulator::get_velocity ( )
inlineprotectedinherited

◆ getCameraParameters() [1/6]

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.
Third 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>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
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)
Generic class defining intrinsic camera parameters.
Definition of the vpImage class member functions.
Definition vpImage.h:131
Control of Irisa's gantry robot named Afma6.
Exceptions
vpRobotException::readingParametersError: If the camera parameters are not found.
Examples
testAfma6.cpp, and testRobotAfma6.cpp.

Definition at line 1262 of file vpAfma6.cpp.

References CONST_CAMERA_AFMA6_FILENAME, CONST_CCMOP_CAMERA_NAME, CONST_GENERIC_CAMERA_NAME, CONST_GRIPPER_CAMERA_NAME, CONST_INTEL_D435_CAMERA_NAME, CONST_VACUUM_CAMERA_NAME, getToolType(), vpException::notImplementedError, vpCameraParameters::perspectiveProjWithDistortion, vpCameraParameters::perspectiveProjWithoutDistortion, projModel, vpCameraParameters::ProjWithKannalaBrandtDistortion, vpRobotException::readingParametersError, vpXmlParserCamera::SEQUENCE_OK, TOOL_CCMOP, TOOL_GENERIC_CAMERA, TOOL_GRIPPER, TOOL_INTEL_D435_CAMERA, TOOL_VACUUM, vpERROR_TRACE, and vpTRACE.

Referenced by getCameraParameters(), and getCameraParameters().

◆ getCameraParameters() [2/6]

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>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
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 getCameraParameters().

◆ getCameraParameters() [3/6]

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>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
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 1554 of file vpAfma6.cpp.

References getCameraParameters().

◆ getCameraParameters() [4/6]

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

Get the current intrinsic camera parameters obtained by calibration.

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.
Warning
The image size must be : 640x480 !

Definition at line 340 of file vpSimulatorAfma6.cpp.

References vpAfma6::CONST_CCMOP_CAMERA_NAME, vpAfma6::CONST_GRIPPER_CAMERA_NAME, vpAfma6::getToolType(), vpWireFrameSimulator::px_int, vpWireFrameSimulator::py_int, vpAfma6::TOOL_CCMOP, vpAfma6::TOOL_CUSTOM, vpAfma6::TOOL_GENERIC_CAMERA, vpAfma6::TOOL_GRIPPER, vpAfma6::TOOL_INTEL_D435_CAMERA, vpAfma6::TOOL_VACUUM, vpERROR_TRACE, and vpTRACE.

Referenced by getCameraParameters(), getCameraParameters(), and initDisplay().

◆ getCameraParameters() [5/6]

void vpSimulatorAfma6::getCameraParameters ( vpCameraParameters & cam,
const vpImage< unsigned char > & I_ )

Get the current intrinsic camera parameters obtained by calibration.

Parameters
cam: In output, camera parameters to fill.
I_: A B&W image send by the current camera in use.
Warning
The image size must be : 640x480 !

Definition at line 405 of file vpSimulatorAfma6.cpp.

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

◆ getCameraParameters() [6/6]

void vpSimulatorAfma6::getCameraParameters ( vpCameraParameters & cam,
const vpImage< vpRGBa > & I_ )

Get the current intrinsic camera parameters obtained by calibration.

Parameters
cam: In output, camera parameters to fill.
I_: A B&W image send by the current camera in use.
Warning
The image size must be : 640x480 !

Definition at line 418 of file vpSimulatorAfma6.cpp.

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

References projModel.

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

References _coupl_56.

◆ getDisplacement()

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

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

Warning
This functionnality is not implemented for the moment in the cartesian space. It is only available in the joint space (vpRobot::ARTICULAR_FRAME).
Parameters
frame: The frame in which the measured displacement is expressed.
displacement: The measured displacement since the last call of this method. The dimension of displacement is always
  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 1857 of file vpSimulatorAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobotWireFrameSimulator::get_artCoord(), vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, and vpColVector::resize().

◆ getExternalCameraParameters() [1/3]

vpCameraParameters vpRobotWireFrameSimulator::getExternalCameraParameters ( ) const
inlineinherited

Get the parameters of the virtual external camera.

Returns
It returns the camera parameters.

Definition at line 175 of file vpRobotWireFrameSimulator.h.

References I, vpMath::maximum(), vpMath::minimum(), vpWireFrameSimulator::px_ext, and vpWireFrameSimulator::py_ext.

◆ getExternalCameraParameters() [2/3]

vpCameraParameters vpWireFrameSimulator::getExternalCameraParameters ( const vpImage< unsigned char > & I) const
inlineinherited

Get the parameters of the virtual external camera.

Parameters
I: The image used to display the view of the camera.
Returns
It returns the camera parameters.

Definition at line 322 of file vpWireFrameSimulator.h.

References vpMath::maximum(), vpMath::minimum(), px_ext, and py_ext.

Referenced by projectCameraTrajectory(), projectCameraTrajectory(), projectCameraTrajectory(), and projectCameraTrajectory().

◆ getExternalCameraParameters() [3/3]

vpCameraParameters vpWireFrameSimulator::getExternalCameraParameters ( const vpImage< vpRGBa > & I) const
inlineinherited

Get the parameters of the virtual external camera.

Parameters
I: The image used to display the view of the camera.
Returns
It returns the camera parameters.

Definition at line 341 of file vpWireFrameSimulator.h.

References vpMath::maximum(), vpMath::minimum(), px_ext, and py_ext.

◆ getExternalCameraPosition()

vpHomogeneousMatrix vpRobotWireFrameSimulator::getExternalCameraPosition ( ) const
inlineinherited

Get the external camera's position relative to the the world reference frame.

Returns
the main external camera position relative to the the world reference frame.

Definition at line 194 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::getExternalCameraPosition().

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

◆ getExternalImage() [1/4]

◆ getExternalImage() [2/4]

void vpWireFrameSimulator::getExternalImage ( vpImage< unsigned char > & I)
inherited

◆ getExternalImage() [3/4]

void vpWireFrameSimulator::getExternalImage ( vpImage< unsigned char > & I,
const vpHomogeneousMatrix & cam_Mf )
inherited

Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.

Parameters
I: The image where the external view is displayed.
cam_Mf: The pose between the point of view and the fixed world frame.
Warning
: The objects are displayed thanks to overlays. The image I is not modified.

Definition at line 1211 of file vpWireFrameSimulator.cpp.

References camColor, camera, cMo, curColor, vpDisplay::display(), display_scene(), displayCamera, displayImageSimulator, displayObject, fMo, vpImageSimulator::getImage(), getInternalCameraParameters(), vpHomogeneousMatrix::inverse(), vpMath::maximum(), vpMath::minimum(), objectImage, px_ext, py_ext, rotz, scene, and vpImageSimulator::setCameraPosition().

◆ getExternalImage() [4/4]

void vpWireFrameSimulator::getExternalImage ( vpImage< vpRGBa > & I,
const vpHomogeneousMatrix & cam_Mf )
inherited

Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.

Parameters
I: The image where the external view is displayed.
cam_Mf: The pose between the point of view and the fixed world frame.
Warning
: The objects are displayed thanks to overlays. The image I is not modified.

Definition at line 943 of file vpWireFrameSimulator.cpp.

References camColor, camera, cMo, curColor, vpDisplay::display(), display_scene(), displayCamera, displayImageSimulator, displayObject, fMo, vpImageSimulator::getImage(), getInternalCameraParameters(), vpHomogeneousMatrix::inverse(), vpMath::maximum(), vpMath::minimum(), objectImage, px_ext, py_ext, rotz, scene, and vpImageSimulator::setCameraPosition().

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

References get_fMc().

◆ getInternalCameraParameters() [1/2]

vpCameraParameters vpWireFrameSimulator::getInternalCameraParameters ( const vpImage< unsigned char > & I) const
inlineinherited

Get the parameters of the virtual internal camera.

Parameters
I: The image used to display the view of the camera.
Returns
It returns the camera parameters.

Definition at line 374 of file vpWireFrameSimulator.h.

References vpMath::maximum(), vpMath::minimum(), px_int, and py_int.

Referenced by getExternalImage(), getExternalImage(), getExternalImage(), getExternalImage(), getInternalImage(), and getInternalImage().

◆ getInternalCameraParameters() [2/2]

vpCameraParameters vpWireFrameSimulator::getInternalCameraParameters ( const vpImage< vpRGBa > & I) const
inlineinherited

Get the parameters of the virtual internal camera.

Parameters
I: The image used to display the view of the camera.
Returns
It returns the camera parameters.

Definition at line 393 of file vpWireFrameSimulator.h.

References vpMath::maximum(), vpMath::minimum(), px_int, and py_int.

◆ getInternalImage() [1/2]

void vpWireFrameSimulator::getInternalImage ( vpImage< unsigned char > & I)
inherited

◆ getInternalImage() [2/2]

void vpWireFrameSimulator::getInternalImage ( vpImage< vpRGBa > & I)
inherited

Get the internal view ie the view of the camera.

Parameters
I: The image where the internal view is displayed.
Warning
: The objects are displayed thanks to overlays. The image I is not modified.

Definition at line 734 of file vpWireFrameSimulator.cpp.

References cdMo, cMo, curColor, D_TOOL, desColor, desiredObject, desiredScene, vpDisplay::display(), display_scene(), displayDesiredObject, displayImageSimulator, displayObject, vpImageSimulator::getImage(), getInternalCameraParameters(), vpMath::maximum(), vpMath::minimum(), vpException::notInitialized, objectImage, px_int, py_int, vpColor::red, rotz, scene, sceneInitialized, and vpImageSimulator::setCameraPosition().

◆ getInternalView() [1/2]

void vpRobotWireFrameSimulator::getInternalView ( vpImage< unsigned char > & I_)
inherited

◆ getInternalView() [2/2]

◆ 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>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
#ifdef VISP_HAVE_AFMA6
vpColVector q1(6), q2(6);
vpRobotAfma6 robot;
// Get the current articular position of the robot
// 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.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position) VP_OVERRIDE
vpHomogeneousMatrix fMc
See also
getForwardKinematics()

Definition at line 596 of file vpAfma6.cpp.

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

Referenced by vpSimulatorAfma6::initialiseCameraRelativeToObject(), vpRobotAfma6::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 1061 of file vpAfma6.cpp.

References _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 1045 of file vpAfma6.cpp.

References _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 1083 of file vpAfma6.cpp.

References _long_56.

◆ getMaxRotationVelocity()

◆ getMaxTranslationVelocity()

◆ getNDof()

int vpRobot::getNDof ( ) const
inlineinherited

Return robot degrees of freedom number.

Definition at line 142 of file vpRobot.h.

References nDof.

◆ getPosition() [1/5]

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

Return the current robot position in the specified frame.

Definition at line 215 of file vpRobot.cpp.

References getPosition().

◆ getPosition() [2/5]

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

Get the current position of the robot.

Parameters
frame: Control frame type in which to get the position, either :
  • in the camera cartesian frame,
  • joint (articular) coordinates of each axes
  • in a reference or fixed cartesian frame attached to the robot base
  • in a mixt cartesian frame (translation in reference frame, and rotation in camera frame)
q: Measured position of the robot:
  • in camera cartesian frame, a 6 dimension vector, set to 0.
  • in articular, a 6 dimension vector corresponding to the joint position of each dof in radians.
  • 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:
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
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 transformation in terms of a
// homogeneous matrix
}
@ REFERENCE_FRAME
Definition vpRobot.h:75
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Class that consider the case of a translation vector.
See also
getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp)
setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)

Implements vpRobot.

Definition at line 1624 of file vpSimulatorAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpHomogeneousMatrix::extract(), vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::get_fMc(), vpRobotException::lowLevelError, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, and vpColVector::resize().

Referenced by getPosition(), getPosition(), and getPosition().

◆ getPosition() [3/5]

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

Get the current time stamped position of the robot.

Parameters
frame: Control frame type in which to get the position, either :
  • in the camera cartesian frame,
  • joint (articular) coordinates of each axes
  • in a reference or fixed cartesian frame attached to the robot base
  • in a mixt cartesian frame (translation in reference frame, and rotation in camera frame)
q: Measured position of the robot:
  • in camera cartesian frame, a 6 dimension vector, set to 0.
  • in articular, a 6 dimension vector corresponding to the joint position of each dof in radians.
  • 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: Unix time in second since January 1st 1970.
See also
getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)

Definition at line 1694 of file vpSimulatorAfma6.cpp.

References getPosition(), and vpTime::measureTimeSecond().

◆ getPosition() [4/5]

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

Get the current position of the robot.

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

The difference is here that the position is returned using a ThetaU representation.

See also
getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)

Definition at line 1711 of file vpSimulatorAfma6.cpp.

References getPosition().

◆ getPosition() [5/5]

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

Get the current time stamped position of the robot.

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

The difference is here that the position is returned using a ThetaU representation.

Definition at line 1738 of file vpSimulatorAfma6.cpp.

References getPosition(), and vpTime::measureTimeSecond().

◆ getPositioningVelocity()

double vpSimulatorAfma6::getPositioningVelocity ( void )
inline

Definition at line 215 of file vpSimulatorAfma6.h.

References getPositioningVelocity().

Referenced by findHighestPositioningSpeed(), and getPositioningVelocity().

◆ getRobotFrame()

vpControlFrameType vpRobot::getRobotFrame ( void ) const
inlineprotectedinherited

◆ getRobotState()

virtual vpRobotStateType vpRobot::getRobotState ( void ) const
inlinevirtualinherited

Definition at line 152 of file vpRobot.h.

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

◆ getSamplingTime()

double vpRobotSimulator::getSamplingTime ( ) const
inlineinherited

Return the sampling time.

Returns
Sampling time in second used to compute the robot displacement from the velocity applied to the robot during this time.

Definition at line 73 of file vpRobotSimulator.h.

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

◆ getToolType()

vpAfma6ToolType vpAfma6::getToolType ( ) const
inlineinherited

Get the current tool type.

Definition at line 167 of file vpAfma6.h.

References tool_current.

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

◆ getVelocity() [1/4]

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

Get the robot velocities.

Parameters
frame: Frame in which velocities are measured.
Returns
Measured velocities. Translations are expressed in m/s and rotations in rad/s.
#include <visp3/core/vpColVector.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
// Set requested joint velocities
vpColVector q_dot(6);
q_dot[0] = 0.1; // Joint 1 velocity in rad/s
q_dot[1] = 0.2; // Joint 2 velocity in rad/s
q_dot[2] = 0.3; // Joint 3 velocity in rad/s
q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
// Initialisation of the velocity measurement
vpColVector q_dot_mes; // Measured velocities
for ( ; ; ) {
q_dot_mes = robot.getVelocity(vpRobot::ARTICULAR_FRAME);
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]
}
}
VISP_EXPORT int wait(double t0, double t)

Definition at line 1168 of file vpSimulatorAfma6.cpp.

References getVelocity().

◆ getVelocity() [2/4]

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

Get the time stamped robot velocities.

Parameters
frame: Frame in which velocities are measured.
timestamp: Unix time in second since January 1st 1970.
Returns
Measured velocities. Translations are expressed in m/s and rotations in rad/s.
See also
getVelocity(vpRobot::vpControlFrameType frame)

Definition at line 1188 of file vpSimulatorAfma6.cpp.

References getVelocity(), and vpTime::measureTimeSecond().

◆ getVelocity() [3/4]

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

Get the robot velocities.

Parameters
frame: Frame in which velocities are measured.
vel: Measured velocities. Translations are expressed in m/s and rotations in rad/s.
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]$.
#include <visp3/core/vpColVector.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
// Set requested joint velocities
vpColVector q_dot(6);
q_dot[0] = 0.1; // Joint 1 velocity in m/s
q_dot[1] = 0.2; // Joint 2 velocity in m/s
q_dot[2] = 0.3; // Joint 3 velocity in m/s
q_dot[3] = M_PI/8; // Joint 4 velocity in rad/s
q_dot[4] = M_PI/4; // Joint 5 velocity in rad/s
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q_dot);
// Initialisation of the velocity measurement
vpColVector q_dot_mes; // Measured velocities
for ( ; ; ) {
robot.getVelocity(vpRobot::ARTICULAR_FRAME, q_dot_mes);
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 1064 of file vpSimulatorAfma6.cpp.

References vpAfma6::_eMc, vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobotWireFrameSimulator::get_artCoord(), vpRobotWireFrameSimulator::get_artVel(), vpAfma6::get_eJe(), vpAfma6::get_fJe(), vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and vpERROR_TRACE.

Referenced by getVelocity(), getVelocity(), and getVelocity().

◆ getVelocity() [4/4]

void vpSimulatorAfma6::getVelocity ( const vpRobot::vpControlFrameType frame,
vpColVector & vel,
double & timestamp )

Get the robot time stamped velocities.

Parameters
frame: Frame in which velocities are measured.
vel: Measured velocities. Translations are expressed in m/s and rotations in rad/s.
timestamp: Unix time in second since January 1st 1970.
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]$.
See also
getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)

Definition at line 1117 of file vpSimulatorAfma6.cpp.

References getVelocity(), and vpTime::measureTimeSecond().

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

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

References parseConfigFile().

◆ init() [3/6]

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

Set the type of tool attached to the robot and transformation between the end-effector and the tool ( $^e{\bf M}c$). This last parameter is loaded from a file.

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

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(vpAfma6::vpToolType, vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpToolType, const vpHomogeneousMatrix&)

Definition at line 210 of file vpAfma6.cpp.

References parseConfigFile(), and setToolType().

◆ init() [4/6]

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

Set the type of tool attached to the robot and the transformation between the end-effector and the tool ( $^e{\bf M}c$).

Parameters
tool: Type of tool in use.
eMc: Homogeneous matrix representation of the transformation between the end-effector frame and the tool frame.
See also
init(vpAfma6::vpAfma6ToolType, vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpAfma6ToolType, const std::string&)

Definition at line 245 of file vpAfma6.cpp.

References set_eMc(), and setToolType().

◆ init() [5/6]

void vpSimulatorAfma6::init ( void )
protectedvirtual

Method which initialises the parameters linked to the robot characteristics.

Set the path to the arm files (*.bnd and *.sln) used by the simulator. If the path set in vpConfig.h in VISP_ROBOT_ARMS_DIR macro is not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment variable that the user has to set.

Implements vpRobot.

Definition at line 122 of file vpSimulatorAfma6.cpp.

References vpAfma6::_joint_max, vpAfma6::_joint_min, vpRobotWireFrameSimulator::artCoord, vpRobot::ARTICULAR_FRAME, vpRobotWireFrameSimulator::artVel, vpIoTools::checkDirectory(), defaultPositioningVelocity, vpRobotWireFrameSimulator::fMi, vpIoTools::getenv(), init(), vpAfma6::njoint, vpRobot::setRobotFrame(), setRobotState(), vpRobotWireFrameSimulator::size_fMi, vpIoTools::splitChain(), vpRobot::STATE_STOP, and vpAfma6::TOOL_CCMOP.

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

◆ init() [6/6]

Initialize the robot kinematics with the extrinsic calibration parameters associated to a specific camera.

The eMc parameters depend on the camera.

Warning
Only perspective projection without distortion is available!
Parameters
tool: Tool to use. Note that the generic camera is not handled.
proj_model: Projection model associated to the camera.
See also
vpCameraParameters, init()

Definition at line 223 of file vpSimulatorAfma6.cpp.

References vpAfma6::_eMc, vpAfma6::_erc, vpAfma6::_etc, vpException::dimensionError, vpRobotWireFrameSimulator::get_displayBusy(), vpRobotWireFrameSimulator::m_mutex_eMc, vpAfma6::projModel, vpMath::rad(), vpRobotWireFrameSimulator::robotArms, vpRobotWireFrameSimulator::set_displayBusy(), setCameraParameters(), vpAfma6::setToolType(), vpAfma6::TOOL_CCMOP, vpAfma6::TOOL_CUSTOM, vpAfma6::TOOL_GENERIC_CAMERA, vpAfma6::TOOL_GRIPPER, vpAfma6::TOOL_INTEL_D435_CAMERA, vpAfma6::TOOL_VACUUM, and vpTime::wait().

◆ initArms()

void vpSimulatorAfma6::initArms ( )
protectedvirtual

Initialise the display of the robot's arms.

Set the path to the scene files (*.bnd and *.sln) used by the simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is not valid, the path is set from the VISP_SCENES_DIR environment variable that the user has to set.

Implements vpRobotWireFrameSimulator.

Definition at line 2162 of file vpSimulatorAfma6.cpp.

References vpWireFrameSimulator::camera, vpWireFrameSimulator::cameraFactor, vpIoTools::checkDirectory(), vpException::dimensionError, vpWireFrameSimulator::displayCamera, vpIoTools::getenv(), vpAfma6::getToolType(), vpRobotWireFrameSimulator::robotArms, vpIoTools::splitChain(), vpAfma6::TOOL_CCMOP, vpAfma6::TOOL_CUSTOM, vpAfma6::TOOL_GENERIC_CAMERA, vpAfma6::TOOL_GRIPPER, vpAfma6::TOOL_INTEL_D435_CAMERA, and vpAfma6::TOOL_VACUUM.

Referenced by initDisplay().

◆ initDisplay()

◆ initialiseCameraRelativeToObject()

bool vpSimulatorAfma6::initialiseCameraRelativeToObject ( const vpHomogeneousMatrix & cMo_)

This method enables to initialise the joint coordinates of the robot in order to position the camera relative to the object.

Before using this method it is advised to set the position of the object thanks to the set_fMo() method.

In other terms, set the world to camera transformation ${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}$, and from the inverse kinematics set the joint positions ${\bf q}$ that corresponds to the ${^f}{\bf M}_c$ transformation.

Parameters
cMo_: the desired pose of the camera.
Returns
false if the robot kinematics is not able to reach the cMo position.

Definition at line 2362 of file vpSimulatorAfma6.cpp.

References compute_fMi(), vpWireFrameSimulator::fMo, vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::getInverseKinematics(), vpHomogeneousMatrix::inverse(), vpRobotWireFrameSimulator::m_mutex_scene, vpRobotWireFrameSimulator::set_artCoord(), vpRobotWireFrameSimulator::set_artVel(), vpRobotWireFrameSimulator::set_velocity(), vpColVector::t(), vpRobot::verbose_, and vpERROR_TRACE.

◆ initialiseObjectRelativeToCamera()

void vpSimulatorAfma6::initialiseObjectRelativeToCamera ( const vpHomogeneousMatrix & cMo_)

This method enables to initialise the pose between the object and the reference frame, in order to position the object relative to the camera.

Before using this method it is advised to set the articular coordinates of the robot.

In other terms, set the world to object transformation ${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o$ where $ {^f}{\bf M}_c
= f({\bf q})$ with ${\bf q}$ the robot joint position

Parameters
cMo_: the desired pose of the camera.

Definition at line 2405 of file vpSimulatorAfma6.cpp.

References vpWireFrameSimulator::fMo, get_fMi(), vpRobotWireFrameSimulator::m_mutex_scene, vpRobotWireFrameSimulator::set_artVel(), and vpRobotWireFrameSimulator::set_velocity().

◆ initScene() [1/8]

void vpRobotWireFrameSimulator::initScene ( const char * obj)
inherited

Initialize the display. It enables to choose the type of scene which will be used to display the object at the current position. The object at the desired position is not displayed.

Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file, or the vrml file.

Parameters
obj: Path to the scene file you want to use.

Definition at line 179 of file vpRobotWireFrameSimulator.cpp.

References vpWireFrameSimulator::camera, vpWireFrameSimulator::displayCamera, and vpWireFrameSimulator::initScene().

◆ initScene() [2/8]

void vpRobotWireFrameSimulator::initScene ( const char * obj,
const char * desired_object )
inherited

Initialize the display. It enables to choose the type of scene which will be used to display the object at the current position and at the desired position.

Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file. It is also possible to use a vrml (.wrl) file.

Parameters
obj: Path to the scene file you want to use.
desired_object: Path to the scene file you want to use.

Definition at line 133 of file vpRobotWireFrameSimulator.cpp.

References vpWireFrameSimulator::camera, vpWireFrameSimulator::displayCamera, and vpWireFrameSimulator::initScene().

◆ initScene() [3/8]

void vpRobotWireFrameSimulator::initScene ( const vpSceneObject & obj)
inherited

Initialize the display. It enables to choose the type of object which will be used to display the object at the current position. The object at the desired position is not displayed.

It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.

Parameters
obj: Type of scene used to display the object at the current position.

Definition at line 157 of file vpRobotWireFrameSimulator.cpp.

References vpWireFrameSimulator::camera, vpWireFrameSimulator::displayCamera, and vpWireFrameSimulator::initScene().

◆ initScene() [4/8]

void vpRobotWireFrameSimulator::initScene ( const vpSceneObject & obj,
const vpSceneDesiredObject & desired_object )
inherited

Initialize the display. It enables to choose the type of scene which will be used to display the object at the current position and at the desired position.

It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.

Parameters
obj: Type of scene used to display the object at the current position.
desired_object: Type of scene used to display the object at the desired pose (in the internal view).

Definition at line 108 of file vpRobotWireFrameSimulator.cpp.

References vpWireFrameSimulator::camera, vpWireFrameSimulator::displayCamera, vpWireFrameSimulator::initScene(), and m_mutex_scene.

◆ initScene() [5/8]

void vpWireFrameSimulator::initScene ( const char * obj,
const char * desired_object,
const std::list< vpImageSimulator > & imObj )
inherited

Initialize the simulator. It enables to choose the type of scene which will be used to display the object at the current position and at the desired position.

Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.

It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.

Parameters
obj: Path to the scene file you want to use.
desired_object: Path to the scene file you want to use.
imObj: A list of vpImageSimulator instances.

Definition at line 503 of file vpWireFrameSimulator.cpp.

References displayImageSimulator, initScene(), and objectImage.

◆ initScene() [6/8]

void vpWireFrameSimulator::initScene ( const char * obj,
const std::list< vpImageSimulator > & imObj )
inherited

Initialize the simulator. It enables to choose the type of scene which will be used to display the object at the current position. The object at the desired position is not displayed.

Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.

It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.

Parameters
obj: Path to the scene file you want to use.
imObj: A list of vpImageSimulator instances.

Definition at line 719 of file vpWireFrameSimulator.cpp.

References displayImageSimulator, initScene(), and objectImage.

◆ initScene() [7/8]

void vpWireFrameSimulator::initScene ( const vpSceneObject & obj,
const std::list< vpImageSimulator > & imObj )
inherited

Initialize the simulator. It enables to choose the type of object which will be used to display the object at the current position. The object at the desired position is not displayed.

It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.

It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.

Parameters
obj: Type of scene used to display the object at the current position.
imObj: A list of vpImageSimulator instances.

Definition at line 645 of file vpWireFrameSimulator.cpp.

References displayImageSimulator, initScene(), and objectImage.

◆ initScene() [8/8]

void vpWireFrameSimulator::initScene ( const vpSceneObject & obj,
const vpSceneDesiredObject & desired_object,
const std::list< vpImageSimulator > & imObj )
inherited

Initialize the simulator. It enables to choose the type of scene which will be used to display the object at the current position and at the desired position.

It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.

It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.

Parameters
obj: Type of scene used to display the object at the current position.
desired_object: Type of scene used to display the object at the desired pose (in the internal view).
imObj: A list of vpImageSimulator instances.

Definition at line 410 of file vpWireFrameSimulator.cpp.

References displayImageSimulator, initScene(), and objectImage.

◆ isInJointLimit()

int vpSimulatorAfma6::isInJointLimit ( )
protectedvirtual

Method used to check if the robot reached a joint limit.

Implements vpRobotWireFrameSimulator.

Definition at line 1805 of file vpSimulatorAfma6.cpp.

References vpAfma6::_joint_max, vpAfma6::_joint_min, and vpRobotWireFrameSimulator::get_artCoord().

Referenced by updateArticularPosition().

◆ launcher()

void vpRobotWireFrameSimulator::launcher ( vpRobotWireFrameSimulator & simulator)
inlinestaticprotectedinherited

◆ move()

void vpSimulatorAfma6::move ( const char * filename)

Moves the robot to the joint position specified in the filename.

Parameters
filename: File containing a joint position.
See also
readPosFile

Definition at line 2057 of file vpSimulatorAfma6.cpp.

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

◆ navigation() [1/2]

◆ navigation() [2/2]

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

References _coupl_56, _eMc, _erc, _etc, _joint_max, _joint_min, _long_56, and vpRobotException::readingParametersError.

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

◆ projectCameraTrajectory() [1/4]

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory ( const vpImage< unsigned char > & I,
const vpHomogeneousMatrix & cMo_,
const vpHomogeneousMatrix & fMo_ )
protectedinherited

◆ projectCameraTrajectory() [2/4]

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory ( const vpImage< unsigned char > & I,
const vpHomogeneousMatrix & cMo_,
const vpHomogeneousMatrix & fMo_,
const vpHomogeneousMatrix & cMf )
protectedinherited

◆ projectCameraTrajectory() [3/4]

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory ( const vpImage< vpRGBa > & I,
const vpHomogeneousMatrix & cMo_,
const vpHomogeneousMatrix & fMo_ )
protectedinherited

◆ projectCameraTrajectory() [4/4]

vpImagePoint vpWireFrameSimulator::projectCameraTrajectory ( const vpImage< vpRGBa > & I,
const vpHomogeneousMatrix & cMo_,
const vpHomogeneousMatrix & fMo_,
const vpHomogeneousMatrix & cMf )
protectedinherited

◆ readPosFile()

bool vpSimulatorAfma6::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.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 1944 of file vpSimulatorAfma6.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>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
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:129
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:162

Definition at line 162 of file vpRobot.cpp.

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

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

◆ savePosFile()

bool vpSimulatorAfma6::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 2026 of file vpSimulatorAfma6.cpp.

References vpMath::deg().

◆ set_artCoord()

◆ set_artVel()

◆ set_displayBusy()

void vpRobotWireFrameSimulator::set_displayBusy ( const bool & status)
inlineprotectedinherited

◆ set_eMc()

void vpAfma6::set_eMc ( const vpHomogeneousMatrix & eMc)
virtualinherited

Set the geometric transformation between the end-effector frame and the tool frame (commonly a camera).

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

Reimplemented in vpRobotAfma6.

Definition at line 1189 of file vpAfma6.cpp.

References _eMc, _erc, _etc, and vpHomogeneousMatrix::getTranslationVector().

Referenced by init(), and vpRobotAfma6::set_eMc().

◆ set_fMo()

void vpRobotWireFrameSimulator::set_fMo ( const vpHomogeneousMatrix & fMo_)
inlineinherited

Set the pose between the object and the fixed world frame.

Parameters
fMo_: The pose between the object and the fixed world frame.

Definition at line 319 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::fMo.

◆ set_velocity()

◆ setCameraColor()

void vpRobotWireFrameSimulator::setCameraColor ( const vpColor & col)
inlineinherited

Set the color used to display the camera in the external view.

Parameters
col: The desired color.

Definition at line 221 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::camColor.

◆ setCameraParameters()

void vpSimulatorAfma6::setCameraParameters ( const vpCameraParameters & cam)

Set the intrinsic camera parameters.

Parameters
cam: The desired camera parameters.

Definition at line 428 of file vpSimulatorAfma6.cpp.

References vpWireFrameSimulator::px_int, and vpWireFrameSimulator::py_int.

Referenced by init().

◆ setCameraPositionRelObj()

void vpWireFrameSimulator::setCameraPositionRelObj ( const vpHomogeneousMatrix & cMo_)
inlineinherited

Set the transformation between the camera frame and the object frame.

Parameters
cMo_: The pose of the object in the camera frame.
Examples
servoSimu4Points.cpp, servoSimuCylinder.cpp, servoSimuSphere.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, and wireframeSimulator.cpp.

Definition at line 467 of file vpWireFrameSimulator.h.

References fMc, fMo, vpHomogeneousMatrix::inverse(), and rotz.

◆ setCameraPositionRelWorld()

void vpWireFrameSimulator::setCameraPositionRelWorld ( const vpHomogeneousMatrix & fMc_)
inlineinherited

Set the position of the the world reference frame relative to the camera.

Parameters
fMc_: The pose of the camera.

Definition at line 479 of file vpWireFrameSimulator.h.

References cMo, fMo, vpHomogeneousMatrix::inverse(), and rotz.

◆ setCameraSizeFactor()

void vpWireFrameSimulator::setCameraSizeFactor ( float factor)
inlineinherited

Set the parameter which enables to choose the size of the main camera in the external camera views. By default this parameter is set to 1.

Parameters
factor: The ration for the camera size.

Definition at line 491 of file vpWireFrameSimulator.h.

References cameraFactor.

◆ setCameraTrajectoryColor()

void vpWireFrameSimulator::setCameraTrajectoryColor ( const vpColor & col)
inlineinherited

Set the color used to display the camera trajectory in the external view.

Parameters
col: The desired color.

Definition at line 499 of file vpWireFrameSimulator.h.

References camTrajColor.

◆ setCameraTrajectoryDisplayType()

void vpWireFrameSimulator::setCameraTrajectoryDisplayType ( const vpCameraTrajectoryDisplayType & camTraj_type)
inlineinherited

Set the way to display the history of the main camera trajectory in the main external view. The choice is given between displaying lines and points.

Parameters
camTraj_type: The chosen way to display the camera trajectory.

Definition at line 508 of file vpWireFrameSimulator.h.

◆ setConstantSamplingTimeMode()

void vpRobotWireFrameSimulator::setConstantSamplingTimeMode ( const bool _constantSamplingTimeMode)
inlineinherited

Set the flag used to force the sampling time in the thread computing the robot's displacement to a constant value; see setSamplingTime(). It may be useful if the main thread (computing the features) is very time consuming. False by default.

Parameters
_constantSamplingTimeMode: The new value of the constantSamplingTimeMode flag.

Definition at line 232 of file vpRobotWireFrameSimulator.h.

References constantSamplingTimeMode.

◆ setCurrentViewColor()

void vpRobotWireFrameSimulator::setCurrentViewColor ( const vpColor & col)
inlineinherited

Set the color used to display the object at the current position in the robot's camera view.

Parameters
col: The desired color.

Definition at line 243 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::curColor.

◆ setDesiredCameraPosition()

void vpRobotWireFrameSimulator::setDesiredCameraPosition ( const vpHomogeneousMatrix & cdMo_)
inlineinherited

Set the desired position of the robot's camera relative to the object.

Parameters
cdMo_: The desired pose of the camera.

Definition at line 258 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::setDesiredCameraPosition().

◆ setDesiredViewColor()

void vpRobotWireFrameSimulator::setDesiredViewColor ( const vpColor & col)
inlineinherited

Set the color used to display the object at the desired position in the robot's camera view.

Parameters
col: The desired color.

Definition at line 251 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::desColor.

◆ setDisplayCameraTrajectory()

void vpWireFrameSimulator::setDisplayCameraTrajectory ( const bool & do_display)
inlineinherited

Enable or disable the displaying of the camera trajectory in the main external camera view.

By default the trajectory is displayed.

Parameters
do_display: Set to true to display the camera trajectory.

Definition at line 539 of file vpWireFrameSimulator.h.

◆ setDisplayRobotType()

void vpRobotWireFrameSimulator::setDisplayRobotType ( const vpDisplayRobotType dispType)
inlineinherited

Set the way to draw the robot. Depending on what you choose you can display a 3D wireframe model or a set of lines linking the frames used to compute the geometrical model.

Parameters
dispType: Type of display. Can be MODEL_3D or MODEL_DH.

Definition at line 270 of file vpRobotWireFrameSimulator.h.

References displayType.

◆ setExternalCameraParameters()

void vpWireFrameSimulator::setExternalCameraParameters ( const vpCameraParameters & cam)
inlineinherited

Set the internal camera parameters.

Parameters
cam: The desired camera parameters.
Examples
servoSimu4Points.cpp, servoSimuCylinder.cpp, servoSimuSphere.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, and wireframeSimulator.cpp.

Definition at line 546 of file vpWireFrameSimulator.h.

References px_ext, and py_ext.

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

◆ setExternalCameraPosition()

void vpRobotWireFrameSimulator::setExternalCameraPosition ( const vpHomogeneousMatrix & camMf_)
inlineinherited

Set the external camera point of view.

Parameters
camMf_: The pose of the external camera relative to the world reference frame.

Definition at line 277 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::setExternalCameraPosition().

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

◆ setGraphicsThickness()

void vpRobotWireFrameSimulator::setGraphicsThickness ( unsigned int thickness)
inlineinherited

Specify the thickness of the graphics drawings.

Definition at line 284 of file vpRobotWireFrameSimulator.h.

References vpWireFrameSimulator::thickness_.

◆ setInternalCameraParameters()

void vpWireFrameSimulator::setInternalCameraParameters ( const vpCameraParameters & cam)
inlineinherited

Set the internal camera parameters.

Parameters
cam: The desired camera parameters.
Examples
servoSimu4Points.cpp, servoSimuCylinder.cpp, servoSimuSphere.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, and wireframeSimulator.cpp.

Definition at line 577 of file vpWireFrameSimulator.h.

References px_int, and py_int.

◆ setJointLimit()

void vpSimulatorAfma6::setJointLimit ( const vpColVector & limitMin,
const vpColVector & limitMax )

This method enables to set the minimum and maximum joint limits for all the six axis of the robot. The three first values have to be given in meter and the others in radian.

Parameters
limitMin: The minimum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
limitMax: The maximum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.

Definition at line 1754 of file vpSimulatorAfma6.cpp.

References vpAfma6::_joint_max, vpAfma6::_joint_min, vpArray2D< Type >::getRows(), and vpTRACE.

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

Definition at line 259 of file vpRobot.cpp.

References maxRotationVelocity.

Referenced by init(), 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.

Definition at line 238 of file vpRobot.cpp.

References maxTranslationVelocity.

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

◆ setNbPtTrajectory()

void vpWireFrameSimulator::setNbPtTrajectory ( unsigned int nbPt)
inlineinherited

Set the maximum number of main camera's positions which are stored. Those position can be displayed in the external camera field of view. By default this parameter is set to 1000.

Parameters
nbPt: The desired number of position which are saved.

Definition at line 590 of file vpWireFrameSimulator.h.

References nbrPtLimit.

◆ setPosition() [1/4]

void vpSimulatorAfma6::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 <visp3/core/vpColVector.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
robot.readPosFile("MyPositionFilename.pos", q);
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
}
Exceptions
vpRobotException::lowLevelError: vpRobot::MIXT_FRAME not implemented.
vpRobotException::positionOutOfRangeError: The requested position is out of range.
See also
setPositioningVelocity()

Definition at line 1547 of file vpSimulatorAfma6.cpp.

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

◆ setPosition() [2/4]

bool vpSimulatorAfma6::setPosition ( const vpHomogeneousMatrix & cdMo_,
vpImage< unsigned char > * Iint = nullptr,
const double & errMax = 0.001 )

This method enable to move the robot with respect to the initialized object. The robot trajectory is a straight line from the current position to the one corresponding to the desired pose (3D visual servoing).

Parameters
cdMo_: the desired pose of the camera wrt. the object
Iint: pointer to the image where the internal view is displayed
errMax: maximum error to consider the pose is reached
Returns
True is the pose is reached, False else

Definition at line 2429 of file vpSimulatorAfma6.cpp.

References vpThetaUVector::buildFrom(), vpRobot::CAMERA_FRAME, vpDisplay::display(), vpHomogeneousMatrix::extract(), vpDisplay::flush(), vpColVector::frobeniusNorm(), vpRobotWireFrameSimulator::get_cMo(), vpRobotWireFrameSimulator::getInternalView(), vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpHomogeneousMatrix::inverse(), vpTime::measureTimeMs(), vpRobotWireFrameSimulator::set_artVel(), vpRobotWireFrameSimulator::set_velocity(), vpRobot::setMaxRotationVelocity(), vpRobot::setMaxTranslationVelocity(), setVelocity(), vpRotationMatrix::t(), and vpTime::wait().

◆ setPosition() [3/4]

void vpSimulatorAfma6::setPosition ( const vpRobot::vpControlFrameType frame,
const vpColVector & q )
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
q: 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 <visp3/core/vpColVector.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
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; // rotation around z axis, in rad
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);
}
@ CAMERA_FRAME
Definition vpRobot.h:81

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;
}
Error that can be emitted by the vpRobot class and its derivatives.
@ positionOutOfRangeError
Position is out of range.

Implements vpRobot.

Definition at line 1288 of file vpSimulatorAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobotWireFrameSimulator::get_artCoord(), vpAfma6::get_fMc(), vpAfma6::getInverseKinematics(), vpRobot::getRobotState(), vpRobotException::lowLevelError, vpRobotWireFrameSimulator::m_mutex_setVelocityCalled, vpRobot::MIXT_FRAME, vpRobotException::positionOutOfRangeError, vpRobot::REFERENCE_FRAME, vpRobotWireFrameSimulator::set_artCoord(), vpRobotWireFrameSimulator::set_artVel(), vpRobotWireFrameSimulator::set_velocity(), vpRobotWireFrameSimulator::setVelocityCalled, vpRobot::STATE_POSITION_CONTROL, vpRobot::verbose_, and vpERROR_TRACE.

Referenced by move(), setPosition(), and setPosition().

◆ setPosition() [4/4]

void vpSimulatorAfma6::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 overloads setPosition(const vpRobot::vpControlFrameType, const vpColVector &).

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

All the positions are expressed in meters for the translations and radians for the rotations.

Parameters
pos1: First coordinate of the position to reach.
pos2: Second coordinate of the position to reach.
pos3: Third coordinate of the position to reach.
pos4: Fourth coordinate of the position to reach.
pos5: Fifth coordinate of the position to reach.
pos6: Sixth coordinate of the position to reach.
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 translation positions in meters, and A,B,C the rotations in radians of the end-effector.
  • In the camera and the reference frame, rotations [pos4, pos5, pos6] are represented by a vpRxyzVector with values in radians.
  • 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 <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
// 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
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);
}
See also
setPosition()

Definition at line 1489 of file vpSimulatorAfma6.cpp.

References setPosition(), and vpERROR_TRACE.

◆ setPositioningVelocity()

void vpSimulatorAfma6::setPositioningVelocity ( double vel)
inline

Definition at line 243 of file vpSimulatorAfma6.h.

◆ setRobotFrame()

◆ setRobotState()

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

Change the robot state.

Parameters
newState: New requested robot state.

Reimplemented from vpRobot.

Definition at line 718 of file vpSimulatorAfma6.cpp.

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

Referenced by init(), move(), and setPosition().

◆ setSamplingTime()

void vpRobotWireFrameSimulator::setSamplingTime ( const double & delta_t)
inlinevirtualinherited

Set the sampling time.

Parameters
delta_t: Sampling time in second used to compute the robot displacement from the velocity applied to the robot during this time.

Since the wireframe simulator is threaded, the sampling time is set to vpTime::getMinTimeForUsleepCall() / 1000 seconds.

Reimplemented from vpRobotSimulator.

Definition at line 295 of file vpRobotWireFrameSimulator.h.

References vpRobotSimulator::delta_t_, and vpTime::getMinTimeForUsleepCall().

Referenced by vpRobotWireFrameSimulator(), and vpRobotWireFrameSimulator().

◆ setSingularityManagement()

void vpRobotWireFrameSimulator::setSingularityManagement ( bool sm)
inlineinherited

Set the parameter which enable or disable the singularity management.

Definition at line 307 of file vpRobotWireFrameSimulator.h.

References singularityManagement.

◆ setToolType()

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

Set the current tool type.

Definition at line 192 of file vpAfma6.h.

References tool_current.

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

◆ setVelocity()

void vpSimulatorAfma6::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. 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 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().
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
#include <visp3/robot/vpSimulatorAfma6.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
int main()
{
vpColVector qvel(6);
// Set a joint velocity
qvel[0] = 0.1; // Joint 1 velocity in m/s
qvel[1] = 0.1; // Joint 2 velocity in m/s
qvel[2] = 0.1; // Joint 3 velocity in m/s
qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
qvel[4] = 0; // Joint 5 velocity in rad/s
qvel[5] = 0; // Joint 6 velocity in rad/s
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
for ( ; ; ) {
// 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);
}

Implements vpRobot.

Definition at line 829 of file vpSimulatorAfma6.cpp.

References vpRobot::ARTICULAR_FRAME, vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpRobot::getRobotState(), vpArray2D< Type >::getRows(), vpRobotWireFrameSimulator::jointLimit, vpRobotWireFrameSimulator::m_mutex_frame, vpRobotWireFrameSimulator::m_mutex_setVelocityCalled, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpRobotWireFrameSimulator::set_velocity(), vpRobot::setRobotFrame(), vpRobotWireFrameSimulator::setVelocityCalled, vpRobot::STATE_VELOCITY_CONTROL, vpERROR_TRACE, and vpRobotException::wrongStateError.

Referenced by setPosition().

◆ setVerbose()

void vpRobotWireFrameSimulator::setVerbose ( bool verbose)
inlineinherited

Activates extra printings when the robot reaches joint limits...

Definition at line 312 of file vpRobotWireFrameSimulator.h.

References vpRobot::verbose_.

◆ singularityTest()

bool vpSimulatorAfma6::singularityTest ( const vpColVector & q,
vpMatrix & J )
protected

Test to detect if the robot is near one of its singularities.

The goal is to avoid the problems du to such configurations.

Definition at line 1781 of file vpSimulatorAfma6.cpp.

Referenced by computeArticularVelocity().

◆ stopMotion()

◆ updateArticularPosition()

void vpSimulatorAfma6::updateArticularPosition ( )
protectedvirtual

Method lauched by the thread to compute the position of the robot in the articular frame.

Implements vpRobotWireFrameSimulator.

Definition at line 439 of file vpSimulatorAfma6.cpp.

References vpAfma6::_joint_max, vpAfma6::_joint_min, vpRobotWireFrameSimulator::cameraParam, compute_fMi(), computeArticularVelocity(), vpRobotWireFrameSimulator::constantSamplingTimeMode, vpMeterPixelConversion::convertPoint(), vpMath::deg(), vpDisplay::display(), vpRobotWireFrameSimulator::displayAllowed, vpDisplay::displayCamera(), vpDisplay::displayFrame(), vpDisplay::displayLine(), vpRobotWireFrameSimulator::displayType, vpDisplay::flush(), vpRobotWireFrameSimulator::fMi, vpRobotWireFrameSimulator::get_artCoord(), vpRobotWireFrameSimulator::get_artVel(), vpRobotWireFrameSimulator::get_displayBusy(), get_fMi(), vpPoint::get_x(), vpPoint::get_y(), vpRobotWireFrameSimulator::getExternalCameraPosition(), getExternalImage(), vpTime::getMinTimeForUsleepCall(), vpRobotSimulator::getSamplingTime(), vpColor::green, vpRobotWireFrameSimulator::I, isInJointLimit(), vpRobotWireFrameSimulator::jointLimit, vpRobotWireFrameSimulator::jointLimitArt, vpRobotWireFrameSimulator::m_mutex_robotStop, vpRobotWireFrameSimulator::m_mutex_setVelocityCalled, vpTime::measureTimeMs(), vpRobotWireFrameSimulator::MODEL_3D, vpRobotWireFrameSimulator::MODEL_DH, vpColor::none, vpRobotWireFrameSimulator::robotStop, vpRobotWireFrameSimulator::set_artCoord(), vpRobotWireFrameSimulator::set_artVel(), vpRobotWireFrameSimulator::set_displayBusy(), vpRobotWireFrameSimulator::setVelocityCalled, vpRobotWireFrameSimulator::tcur, vpWireFrameSimulator::thickness_, vpRobotWireFrameSimulator::tprev, vpForwardProjection::track(), vpRobot::verbose_, and vpTime::wait().

Member Data Documentation

◆ _coupl_56

double vpAfma6::_coupl_56
protectedinherited

◆ _eMc

◆ _erc

◆ _etc

◆ _joint_max

◆ _joint_min

◆ _long_56

◆ areJointLimitsAvailable

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 111 of file vpRobot.h.

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

◆ artCoord

vpColVector vpRobotWireFrameSimulator::artCoord
protectedinherited

◆ artVel

vpColVector vpRobotWireFrameSimulator::artVel
protectedinherited

◆ blocked

bool vpWireFrameSimulator::blocked
protectedinherited

Definition at line 265 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ blockedr

bool vpWireFrameSimulator::blockedr
protectedinherited

Definition at line 262 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ blockedt

bool vpWireFrameSimulator::blockedt
protectedinherited

Definition at line 264 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ blockedz

bool vpWireFrameSimulator::blockedz
protectedinherited

Definition at line 263 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ camColor

◆ camera

◆ cameraFactor

float vpWireFrameSimulator::cameraFactor
protectedinherited

◆ cameraParam

◆ cameraTrajectory

std::list<vpImagePoint> vpWireFrameSimulator::cameraTrajectory
protectedinherited

◆ camMf

◆ camMf2

◆ camTrajColor

vpColor vpWireFrameSimulator::camTrajColor
protectedinherited

◆ camTrajType

vpCameraTrajectoryDisplayType vpWireFrameSimulator::camTrajType
protectedinherited

◆ cdMo

◆ cMo

◆ 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 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 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 getCameraParameters(), and vpSimulatorAfma6::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 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 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 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 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 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 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 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 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 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 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 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 getCameraParameters(), and vpSimulatorAfma6::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 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 getCameraParameters().

◆ constantSamplingTimeMode

bool vpRobotWireFrameSimulator::constantSamplingTimeMode
protectedinherited

Flag used to force the sampling time in the thread computing the robot's displacement to a constant value (samplingTime). It may be useful if the main thread (computing the features) is very time consuming. False by default.

Definition at line 158 of file vpRobotWireFrameSimulator.h.

Referenced by setConstantSamplingTimeMode(), vpSimulatorAfma6::updateArticularPosition(), vpSimulatorViper850::updateArticularPosition(), vpRobotWireFrameSimulator(), and vpRobotWireFrameSimulator().

◆ curColor

◆ defaultPositioningVelocity

BEGIN_VISP_NAMESPACE const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0
static

Definition at line 188 of file vpSimulatorAfma6.h.

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

◆ defaultTool

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

Default tool attached to the robot end effector.

Definition at line 134 of file vpAfma6.h.

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

◆ delta_t_

◆ desColor

◆ desiredObject

◆ desiredScene

◆ display

vpDisplayX vpRobotWireFrameSimulator::display
protectedinherited

◆ displayAllowed

bool vpRobotWireFrameSimulator::displayAllowed
protectedinherited

◆ displayBusy

bool vpRobotWireFrameSimulator::displayBusy
protectedinherited

◆ displayCamera

◆ displayCameraTrajectory

bool vpWireFrameSimulator::displayCameraTrajectory
protectedinherited

◆ displayDesiredObject

◆ displayImageSimulator

◆ displayObject

◆ displayType

◆ eJe

◆ eJeAvailable

int vpRobot::eJeAvailable
protectedinherited

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

Definition at line 105 of file vpRobot.h.

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

◆ extCamChanged

bool vpWireFrameSimulator::extCamChanged
protectedinherited

◆ f2Mf

◆ fJe

◆ fJeAvailable

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 109 of file vpRobot.h.

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

◆ fMc

vpHomogeneousMatrix vpWireFrameSimulator::fMc
protectedinherited

◆ fMi

vpHomogeneousMatrix* vpRobotWireFrameSimulator::fMi
protectedinherited

Table containing all the homogeneous matrices between the reference frame of the robot and the frames you used to compute the Denavit-Hartenberg representation

If you use a camera at the end of the effector, the last homogeneous matrix has to be the one between the reference frame and the camera frame (fMc)

Definition at line 103 of file vpRobotWireFrameSimulator.h.

Referenced by vpSimulatorAfma6::compute_fMi(), vpSimulatorViper850::compute_fMi(), vpSimulatorAfma6::get_fMi(), vpSimulatorViper850::get_fMi(), vpSimulatorAfma6::init(), vpSimulatorViper850::init(), vpSimulatorAfma6::updateArticularPosition(), vpSimulatorViper850::updateArticularPosition(), vpRobotWireFrameSimulator(), vpRobotWireFrameSimulator(), vpSimulatorAfma6::~vpSimulatorAfma6(), and vpSimulatorViper850::~vpSimulatorViper850().

◆ fMo

◆ fMoList

std::list<vpHomogeneousMatrix> vpWireFrameSimulator::fMoList
protectedinherited

◆ I

◆ jointLimit

◆ jointLimitArt

unsigned int vpRobotWireFrameSimulator::jointLimitArt
protectedinherited

◆ m_mutex_artCoord

std::mutex vpRobotWireFrameSimulator::m_mutex_artCoord
protectedinherited

◆ m_mutex_artVel

std::mutex vpRobotWireFrameSimulator::m_mutex_artVel
protectedinherited

◆ m_mutex_display

std::mutex vpRobotWireFrameSimulator::m_mutex_display
protectedinherited

◆ m_mutex_eMc

◆ m_mutex_fMi

◆ m_mutex_frame

◆ m_mutex_robotStop

◆ m_mutex_scene

◆ m_mutex_setVelocityCalled

◆ m_mutex_velocity

std::mutex vpRobotWireFrameSimulator::m_mutex_velocity
protectedinherited

◆ m_thread

◆ maxRotationVelocity

◆ maxRotationVelocityDefault

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

◆ maxTranslationVelocity

double vpRobot::maxTranslationVelocity
protectedinherited

◆ maxTranslationVelocityDefault

BEGIN_VISP_NAMESPACE const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

◆ nbrPtLimit

unsigned int vpWireFrameSimulator::nbrPtLimit
protectedinherited

◆ nDof

◆ njoint

◆ object

vpSceneObject vpWireFrameSimulator::object
protectedinherited

Definition at line 243 of file vpWireFrameSimulator.h.

Referenced by vpWireFrameSimulator().

◆ objectImage

◆ old_iPr

vpImagePoint vpWireFrameSimulator::old_iPr
protectedinherited

Definition at line 259 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ old_iPt

vpImagePoint vpWireFrameSimulator::old_iPt
protectedinherited

Definition at line 261 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ old_iPz

vpImagePoint vpWireFrameSimulator::old_iPz
protectedinherited

Definition at line 260 of file vpWireFrameSimulator.h.

Referenced by navigation(), navigation(), and vpWireFrameSimulator().

◆ poseList

std::list<vpHomogeneousMatrix> vpWireFrameSimulator::poseList
protectedinherited

◆ projModel

◆ px_ext

◆ px_int

◆ py_ext

◆ py_int

◆ qmax

double* vpRobot::qmax
protectedinherited

Definition at line 113 of file vpRobot.h.

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

◆ qmin

double* vpRobot::qmin
protectedinherited

Definition at line 112 of file vpRobot.h.

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

◆ refMo

vpHomogeneousMatrix vpWireFrameSimulator::refMo
protectedinherited

Definition at line 239 of file vpWireFrameSimulator.h.

Referenced by vpWireFrameSimulator().

◆ robotArms

◆ robotStop

◆ rotz

◆ scene

◆ sceneInitialized

◆ setVelocityCalled

bool vpRobotWireFrameSimulator::setVelocityCalled
protectedinherited

◆ singularityManagement

bool vpRobotWireFrameSimulator::singularityManagement
protectedinherited

◆ size_fMi

unsigned int vpRobotWireFrameSimulator::size_fMi
protectedinherited

◆ tcur

◆ thickness_

◆ tool_current

vpAfma6ToolType vpAfma6::tool_current
protectedinherited

Current tool in use.

Definition at line 211 of file vpAfma6.h.

Referenced by getToolType(), setToolType(), and vpAfma6().

◆ tprev

double vpRobotWireFrameSimulator::tprev
protectedinherited

◆ velocity

vpColVector vpRobotWireFrameSimulator::velocity
protectedinherited

The velocity in the current frame (articular, camera or reference)

Definition at line 110 of file vpRobotWireFrameSimulator.h.

Referenced by get_velocity(), set_velocity(), vpRobotWireFrameSimulator(), and vpRobotWireFrameSimulator().

◆ verbose_