Visual Servoing Platform  version 3.5.0 under development (2022-02-15)

#include <visp3/robot/vpRobotTemplate.h>

+ Inheritance diagram for vpRobotTemplate:

Public Types

enum  vpRobotStateType {
  STATE_STOP, STATE_VELOCITY_CONTROL, STATE_POSITION_CONTROL, STATE_ACCELERATION_CONTROL,
  STATE_FORCE_TORQUE_CONTROL
}
 
enum  vpControlFrameType {
  REFERENCE_FRAME, ARTICULAR_FRAME, JOINT_STATE = ARTICULAR_FRAME, END_EFFECTOR_FRAME,
  CAMERA_FRAME, TOOL_FRAME = CAMERA_FRAME, MIXT_FRAME
}
 

Public Member Functions

 vpRobotTemplate ()
 
virtual ~vpRobotTemplate ()
 
void get_eJe (vpMatrix &eJe_)
 
void get_fJe (vpMatrix &fJe_)
 
vpHomogeneousMatrix get_eMc () const
 
void getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &q)
 
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q)
 
void set_eMc (vpHomogeneousMatrix &eMc)
 
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q)
 
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel)
 
Inherited functionalities from vpRobot
double getMaxTranslationVelocity (void) const
 
double getMaxRotationVelocity (void) const
 
vpColVector getPosition (const vpRobot::vpControlFrameType frame)
 
virtual vpRobotStateType getRobotState (void) const
 
void setMaxRotationVelocity (double maxVr)
 
void setMaxTranslationVelocity (double maxVt)
 
virtual vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState)
 
void setVerbose (bool verbose)
 

Static Public Member Functions

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

Protected Member Functions

void init ()
 
void getJointPosition (vpColVector &q)
 
void setCartVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &v)
 
void setJointVelocity (const vpColVector &qdot)
 
Protected Member Functions Inherited from vpRobot
vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame)
 
vpControlFrameType getRobotFrame (void) const
 

Protected Attributes

vpHomogeneousMatrix m_eMc
 
double maxTranslationVelocity
 
double maxRotationVelocity
 
int nDof
 
vpMatrix eJe
 
int eJeAvailable
 
vpMatrix fJe
 
int fJeAvailable
 
int areJointLimitsAvailable
 
double * qmin
 
double * qmax
 
bool verbose_
 

Static Protected Attributes

static const double maxTranslationVelocityDefault = 0.2
 
static const double maxRotationVelocityDefault = 0.7
 

Detailed Description

Class that defines a robot just to show which function you must implement.

Definition at line 61 of file vpRobotTemplate.h.

Member Enumeration Documentation

◆ 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 75 of file vpRobot.h.

◆ vpRobotStateType

enum vpRobot::vpRobotStateType
inherited

Robot control states.

Enumerator
STATE_STOP 

Stops robot motion especially in velocity and acceleration control.

STATE_VELOCITY_CONTROL 

Initialize the velocity controller.

STATE_POSITION_CONTROL 

Initialize the position controller.

STATE_ACCELERATION_CONTROL 

Initialize the acceleration controller.

STATE_FORCE_TORQUE_CONTROL 

Initialize the force/torque controller.

Definition at line 64 of file vpRobot.h.

Constructor & Destructor Documentation

◆ vpRobotTemplate()

vpRobotTemplate::vpRobotTemplate ( )

Default constructor.

Definition at line 69 of file vpRobotTemplate.cpp.

References init().

◆ ~vpRobotTemplate()

vpRobotTemplate::~vpRobotTemplate ( )
virtual

Destructor.

Definition at line 78 of file vpRobotTemplate.cpp.

Member Function Documentation

◆ get_eJe()

void vpRobotTemplate::get_eJe ( vpMatrix eJe_)
virtual

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

Parameters
[out]eJe_: End-effector frame Jacobian.

Implements vpRobot.

Definition at line 97 of file vpRobotTemplate.cpp.

◆ get_eMc()

vpHomogeneousMatrix vpRobotTemplate::get_eMc ( ) const
inline

Return constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.

Definition at line 74 of file vpRobotTemplate.h.

References vpRobot::getDisplacement(), and vpRobot::getPosition().

◆ get_fJe()

void vpRobotTemplate::get_fJe ( vpMatrix fJe_)
virtual

Get the robot Jacobian expressed in the robot reference frame.

Parameters
[out]fJe_: Base (or reference) frame Jacobian.

Implements vpRobot.

Definition at line 108 of file vpRobotTemplate.cpp.

◆ getDisplacement()

void vpRobotTemplate::getDisplacement ( const vpRobot::vpControlFrameType  frame,
vpColVector q 
)
virtual

Get a displacement.

Parameters
[in]frame: Considered cartesian frame or joint state.
[out]q: Displacement in meter and rad.

Implements vpRobot.

Definition at line 312 of file vpRobotTemplate.cpp.

◆ getJointPosition()

void vpRobotTemplate::getJointPosition ( vpColVector q)
protected

Get robot joint positions.

Parameters
[in]q: Joint velocities in rad/s.

Definition at line 271 of file vpRobotTemplate.cpp.

Referenced by getPosition().

◆ getMaxRotationVelocity()

◆ getMaxTranslationVelocity()

◆ getPosition() [1/2]

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

Get robot position.

Parameters
[in]frame: Considered cartesian frame or joint state.
[out]q: Position of the arm.

Implements vpRobot.

Definition at line 283 of file vpRobotTemplate.cpp.

References getJointPosition(), and vpRobot::JOINT_STATE.

◆ getPosition() [2/2]

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

Return the current robot position in the specified frame.

Definition at line 216 of file vpRobot.cpp.

References vpRobot::getPosition().

◆ getRobotFrame()

vpControlFrameType vpRobot::getRobotFrame ( void  ) const
inlineprotectedinherited

◆ getRobotState()

virtual vpRobotStateType vpRobot::getRobotState ( void  ) const
inlinevirtualinherited

◆ init()

void vpRobotTemplate::init ( void  )
protectedvirtual

◆ saturateVelocities()

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

Saturate velocities.

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

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

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

Definition at line 163 of file vpRobot.cpp.

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

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

◆ set_eMc()

void vpRobotTemplate::set_eMc ( vpHomogeneousMatrix eMc)
inline

Set constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.

Definition at line 83 of file vpRobotTemplate.h.

References vpRobot::init(), vpRobot::setPosition(), and vpRobot::setVelocity().

◆ setCartVelocity()

void vpRobotTemplate::setCartVelocity ( const vpRobot::vpControlFrameType  frame,
const vpColVector v 
)
protected

Send to the controller a 6-dim velocity twist vector expressed in a Cartesian frame.

Parameters
[in]frame: Cartesian control frame (either tool frame or end-effector) in which the velocity v is expressed. Units are m/s for translation and rad/s for rotation velocities.
[in]v: 6-dim vector that contains the 6 components of the velocity twist to send to the robot. Units are m/s and rad/s.

Definition at line 131 of file vpRobotTemplate.cpp.

References vpRobot::END_EFFECTOR_FRAME, vpException::fatalError, vpRobot::JOINT_STATE, m_eMc, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpArray2D< Type >::size(), and vpRobot::TOOL_FRAME.

Referenced by setVelocity().

◆ setJointVelocity()

void vpRobotTemplate::setJointVelocity ( const vpColVector qdot)
protected

Send a joint velocity to the controller.

Parameters
[in]qdot: Joint velocities vector. Units are rad/s for a robot arm.

Definition at line 192 of file vpRobotTemplate.cpp.

Referenced by setVelocity().

◆ setMaxRotationVelocity()

void vpRobot::setMaxRotationVelocity ( double  w_max)
inherited

◆ setMaxTranslationVelocity()

void vpRobot::setMaxTranslationVelocity ( double  v_max)
inherited

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

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

Definition at line 239 of file vpRobot.cpp.

References vpRobot::maxTranslationVelocity.

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

◆ setPosition()

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

Set a position to reach.

Parameters
[in]frame: Considered cartesian frame or joint state.
[in]q: Position to reach.

Implements vpRobot.

Definition at line 299 of file vpRobotTemplate.cpp.

◆ setRobotFrame()

◆ setRobotState()

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

◆ setVelocity()

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

Send to the controller a velocity in a given frame.

Parameters
[in]frame: Control frame in which the velocity vel is expressed. Velocities could be joint velocities, or cartesian velocities. Units are m/s for translation and rad/s for rotation velocities.
[in]vel: Vector that contains the velocity to apply to the robot.

Implements vpRobot.

Definition at line 210 of file vpRobotTemplate.cpp.

References vpException::dimensionError, vpRobot::END_EFFECTOR_FRAME, vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpRobot::getRobotState(), vpRobot::JOINT_STATE, vpRobot::MIXT_FRAME, vpRobot::nDof, vpRobot::REFERENCE_FRAME, vpRobot::saturateVelocities(), setCartVelocity(), setJointVelocity(), vpArray2D< Type >::size(), vpRobot::STATE_VELOCITY_CONTROL, vpRobot::TOOL_FRAME, and vpRobotException::wrongStateError.

◆ setVerbose()

Member Data Documentation

◆ areJointLimitsAvailable

◆ eJe

vpMatrix vpRobot::eJe
protectedinherited

◆ eJeAvailable

int vpRobot::eJeAvailable
protectedinherited

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

Definition at line 106 of file vpRobot.h.

Referenced by vpRobot::operator=(), vpRobotCamera::vpRobotCamera(), vpSimulatorCamera::vpSimulatorCamera(), vpSimulatorPioneer::vpSimulatorPioneer(), and vpSimulatorPioneerPan::vpSimulatorPioneerPan().

◆ fJe

vpMatrix vpRobot::fJe
protectedinherited

robot Jacobian expressed in the robot reference frame available

Definition at line 108 of file vpRobot.h.

Referenced by vpRobotFlirPtu::get_fJe(), and vpRobot::operator=().

◆ fJeAvailable

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 110 of file vpRobot.h.

Referenced by vpRobot::operator=(), vpRobotCamera::vpRobotCamera(), vpSimulatorCamera::vpSimulatorCamera(), vpSimulatorPioneer::vpSimulatorPioneer(), and vpSimulatorPioneerPan::vpSimulatorPioneerPan().

◆ m_eMc

vpHomogeneousMatrix vpRobotTemplate::m_eMc
protected

Constant transformation between end-effector and tool (or camera) frame.

Definition at line 94 of file vpRobotTemplate.h.

Referenced by setCartVelocity().

◆ maxRotationVelocity

◆ maxRotationVelocityDefault

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

Definition at line 99 of file vpRobot.h.

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

◆ maxTranslationVelocity

double vpRobot::maxTranslationVelocity
protectedinherited

◆ maxTranslationVelocityDefault

const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

Definition at line 97 of file vpRobot.h.

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

◆ nDof

◆ qmax

◆ qmin

◆ verbose_