Visual Servoing Platform  version 3.2.0

#include <vpRobotSimulator.h>

+ Inheritance diagram for vpRobotSimulator:

Public Types

enum  vpRobotStateType { STATE_STOP, STATE_VELOCITY_CONTROL, STATE_POSITION_CONTROL, STATE_ACCELERATION_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

 vpRobotSimulator ()
 
virtual ~vpRobotSimulator ()
 
Inherited functionalities from vpRobotSimulator
double getSamplingTime () const
 
virtual void setSamplingTime (const double &delta_t)
 
Inherited functionalities from vpRobot
virtual void get_eJe (vpMatrix &_eJe)=0
 
virtual void get_fJe (vpMatrix &_fJe)=0
 
virtual void getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &q)=0
 
double getMaxTranslationVelocity (void) const
 
double getMaxRotationVelocity (void) const
 
virtual void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q)=0
 
vpColVector getPosition (const vpRobot::vpControlFrameType frame)
 
virtual vpRobotStateType getRobotState (void) const
 
virtual void init ()=0
 
void setMaxRotationVelocity (const double maxVr)
 
void setMaxTranslationVelocity (const double maxVt)
 
virtual void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q)=0
 
virtual vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState)
 
virtual void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel)=0
 
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

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

Protected Attributes

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
 

Detailed Description

This class aims to be a basis used to create all the robot simulators.

group_robot_simu_unicycle

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

◆ vpRobotStateType

enum vpRobot::vpRobotStateType
inherited

Robot control states.

Enumerator
STATE_STOP 

Stops robot motion especially in velocity and acceleration control.

STATE_VELOCITY_CONTROL 

Initialize the velocity controller.

STATE_POSITION_CONTROL 

Initialize the position controller.

STATE_ACCELERATION_CONTROL 

Initialize the acceleration controller.

Definition at line 63 of file vpRobot.h.

Constructor & Destructor Documentation

◆ vpRobotSimulator()

vpRobotSimulator::vpRobotSimulator ( )

Basic constructor that sets the sampling time by default to 40ms.

Definition at line 45 of file vpRobotSimulator.cpp.

◆ ~vpRobotSimulator()

virtual vpRobotSimulator::~vpRobotSimulator ( )
inlinevirtual

Basic destructor

Definition at line 71 of file vpRobotSimulator.h.

Member Function Documentation

◆ get_eJe()

virtual void vpRobot::get_eJe ( vpMatrix _eJe)
pure virtualinherited

◆ get_fJe()

virtual void vpRobot::get_fJe ( vpMatrix _fJe)
pure virtualinherited

Get the robot Jacobian expressed in the robot reference (or world) frame.

Implemented in vpRobotViper850, vpRobotViper650, vpRobotFranka, vpSimulatorViper850, vpRobotAfma4, vpSimulatorAfma6, vpRobotAfma6, vpRobotPtu46, and vpRobotTemplate.

◆ getDisplacement()

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

Get a displacement (frame as to ve specified) between two successive position control.

Implemented in vpRobotPtu46, vpRobotViper850, vpRobotViper650, vpRobotAfma6, vpRobotAfma4, vpRobotTemplate, vpSimulatorViper850, and vpSimulatorAfma6.

◆ getMaxRotationVelocity()

double vpRobot::getMaxRotationVelocity ( void  ) const
inherited

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

Returns
Maximum rotation velocity expressed in rad/s.

Definition at line 272 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::computeArticularVelocity(), vpSimulatorAfma6::findHighestPositioningSpeed(), vpSimulatorViper850::findHighestPositioningSpeed(), vpSimulatorAfma6::setPosition(), and vpSimulatorAfma6::setVelocity().

◆ getMaxTranslationVelocity()

double vpRobot::getMaxTranslationVelocity ( void  ) const
inherited

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

Returns
Maximum translational velocity expressed in m/s.

Definition at line 250 of file vpRobot.cpp.

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

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

◆ getPosition() [2/2]

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

◆ getRobotFrame()

vpControlFrameType vpRobot::getRobotFrame ( void  ) const
inlineprotectedinherited

Definition at line 171 of file vpRobot.h.

Referenced by vpSimulatorAfma6::computeArticularVelocity().

◆ getRobotState()

virtual vpRobotStateType vpRobot::getRobotState ( void  ) const
inlinevirtualinherited

◆ getSamplingTime()

◆ init()

◆ 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 162 of file vpRobot.cpp.

◆ setMaxRotationVelocity()

void vpRobot::setMaxRotationVelocity ( const double  w_max)
inherited

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

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

Definition at line 259 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::setPosition().

◆ setMaxTranslationVelocity()

void vpRobot::setMaxTranslationVelocity ( const double  v_max)
inherited

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

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

Definition at line 238 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::setPosition().

◆ setPosition()

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

Set a displacement (frame has to be specified) in position control.

Implemented in vpSimulatorViper850, vpSimulatorAfma6, vpRobotPtu46, vpRobotTemplate, vpRobotViper850, vpRobotViper650, vpRobotFranka, vpRobotAfma6, and vpRobotAfma4.

Examples
servoPioneerPanSegment3D.cpp.

◆ setRobotFrame()

vpRobot::vpControlFrameType vpRobot::setRobotFrame ( vpRobot::vpControlFrameType  newFrame)
protectedinherited

Definition at line 206 of file vpRobot.cpp.

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

◆ setRobotState()

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

Reimplemented in vpRobotViper850, vpRobotViper650, vpRobotFranka, vpRobotAfma6, vpRobotAfma4, vpRobotPtu46, vpSimulatorViper850, and vpSimulatorAfma6.

Examples
frankaMoveToPosition.cpp, frankaSavePosition.cpp, moveAfma4.cpp, moveBiclops.cpp, movePtu46.cpp, photometricVisualServoing.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.cpp, servoAfma4Point2DCamVelocityKalman.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoAfma6TwoLines2DCamVelocity.cpp, servoBiclopsPoint2DArtVelocity.cpp, servoFrankaPBVS.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity-jointAvoidance-large.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, servoViper850Point2DCamVelocityKalman.cpp, testFrankaCartVelocity-2.cpp, testFrankaCartVelocity-3.cpp, testFrankaCartVelocity.cpp, testFrankaJointPosition.cpp, testFrankaJointVelocity-2.cpp, testFrankaJointVelocity-3.cpp, testFrankaJointVelocity.cpp, testFrankaJointVelocityLimits.cpp, testRobotViper650-frames.cpp, testRobotViper850-frames.cpp, testVirtuoseAfma6.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, and tutorial-ibvs-4pts-wireframe-robot-viper.cpp.

Definition at line 200 of file vpRobot.cpp.

Referenced by vpSimulatorAfma6::setRobotState(), and vpSimulatorAfma6::stopMotion().

◆ setSamplingTime()

◆ setVelocity()

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

◆ setVerbose()

void vpRobot::setVerbose ( bool  verbose)
inlineinherited

Member Data Documentation

◆ areJointLimitsAvailable

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 111 of file vpRobot.h.

◆ delta_t_

double vpRobotSimulator::delta_t_
protected

Definition at line 64 of file vpRobotSimulator.h.

◆ eJe

vpMatrix vpRobot::eJe
protectedinherited

robot Jacobian expressed in the end-effector frame

Definition at line 103 of file vpRobot.h.

◆ eJeAvailable

int vpRobot::eJeAvailable
protectedinherited

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

Definition at line 105 of file vpRobot.h.

◆ fJe

vpMatrix vpRobot::fJe
protectedinherited

robot Jacobian expressed in the robot reference frame available

Definition at line 107 of file vpRobot.h.

◆ fJeAvailable

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 109 of file vpRobot.h.

◆ maxRotationVelocity

double vpRobot::maxRotationVelocity
protectedinherited

Definition at line 97 of file vpRobot.h.

◆ maxRotationVelocityDefault

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

Definition at line 98 of file vpRobot.h.

◆ maxTranslationVelocity

double vpRobot::maxTranslationVelocity
protectedinherited

Definition at line 95 of file vpRobot.h.

◆ maxTranslationVelocityDefault

const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

Definition at line 96 of file vpRobot.h.

◆ nDof

int vpRobot::nDof
protectedinherited

number of degrees of freedom

Definition at line 101 of file vpRobot.h.

◆ qmax

double* vpRobot::qmax
protectedinherited

Definition at line 113 of file vpRobot.h.

◆ qmin

double* vpRobot::qmin
protectedinherited

Definition at line 112 of file vpRobot.h.

◆ verbose_

bool vpRobot::verbose_
protectedinherited

Definition at line 115 of file vpRobot.h.

vpColVector::t
vpRowVector t() const
Definition: vpColVector.cpp:695
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:101
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:71
vpRobot::saturateVelocities
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:162