Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches

#include <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 () VP_OVERRIDE
void get_eJe (vpMatrix &eJe_) VP_OVERRIDE
void get_fJe (vpMatrix &fJe_) VP_OVERRIDE
vpHomogeneousMatrix get_eMc () const
void getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE
void set_eMc (vpHomogeneousMatrix &eMc)
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q) VP_OVERRIDE
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
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)
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 () VP_OVERRIDE
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 53 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 73 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 61 of file vpRobot.h.

Constructor & Destructor Documentation

◆ vpRobotTemplate()

vpRobotTemplate::vpRobotTemplate ( )

Default constructor.

Definition at line 65 of file vpRobotTemplate.cpp.

References init(), and m_eMc.

◆ ~vpRobotTemplate()

vpRobotTemplate::~vpRobotTemplate ( )
virtual

Destructor.

Definition at line 70 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 86 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 66 of file vpRobotTemplate.h.

References m_eMc.

◆ 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 97 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 304 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 263 of file vpRobotTemplate.cpp.

Referenced by getPosition().

◆ 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/2]

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/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 275 of file vpRobotTemplate.cpp.

References getJointPosition(), and vpRobot::JOINT_STATE.

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

◆ init()

BEGIN_VISP_NAMESPACE 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>
#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;
}
Implementation of column vector and the associated operations.
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().

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

References m_eMc.

◆ 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 120 of file vpRobotTemplate.cpp.

References vpRobot::END_EFFECTOR_FRAME, vpException::fatalError, vpRobot::JOINT_STATE, m_eMc, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, 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 182 of file vpRobotTemplate.cpp.

Referenced by setVelocity().

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

◆ 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 291 of file vpRobotTemplate.cpp.

◆ setRobotFrame()

◆ setRobotState()

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

void vpRobot::setVerbose ( bool verbose)
inlineinherited

Member Data Documentation

◆ areJointLimitsAvailable

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 111 of file vpRobot.h.

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

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

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

◆ m_eMc

vpHomogeneousMatrix vpRobotTemplate::m_eMc
protected

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

Definition at line 86 of file vpRobotTemplate.h.

Referenced by get_eMc(), set_eMc(), setCartVelocity(), and vpRobotTemplate().

◆ 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

◆ nDof

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

◆ verbose_