Visual Servoing Platform
version 3.6.1 under development (2024-12-17)
|
#include <visp3/robot/vpSimulatorPioneer.h>
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 | |
vpSimulatorPioneer () | |
Inherited functionalities from vpRobotSimulator | |
double | getSamplingTime () const |
virtual void | setSamplingTime (const double &delta_t) |
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 | |
Protected Member Functions Inherited from vpRobot | |
vpControlFrameType | setRobotFrame (vpRobot::vpControlFrameType newFrame) |
vpControlFrameType | getRobotFrame (void) const |
Protected Attributes | |
vpHomogeneousMatrix | wMc_ |
vpHomogeneousMatrix | wMe_ |
double | xm_ |
double | ym_ |
double | theta_ |
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 |
Inherited functionalities from vpUnicycle | |
vpHomogeneousMatrix | cMe_ |
vpMatrix | eJe_ |
void | set_cMe (const vpHomogeneousMatrix &cMe) |
void | set_eJe (const vpMatrix &eJe) |
vpHomogeneousMatrix | get_cMe () const |
vpVelocityTwistMatrix | get_cVe () const |
void | get_cVe (vpVelocityTwistMatrix &cVe) const |
vpMatrix | get_eJe () const |
Inherited functionalities from vpSimulatorPioneer | |
void | get_eJe (vpMatrix &eJe) VP_OVERRIDE |
void | getPosition (vpHomogeneousMatrix &wMc) const |
void | getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q) VP_OVERRIDE |
void | setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE |
Class that defines the Pioneer mobile robot simulator equipped with a static camera.
It intends to simulate the mobile robot described in vpPioneer class. This robot has 2 dof: , the translational and rotational velocities that are applied at point E.
The robot position evolves with respect to a world frame; wMc. When a new joint velocity is applied to the robot using setVelocity(), the position of the camera wrt the world frame is updated.
The following code shows how to control this robot in position and velocity.
The usage of this class is also highlighted in Tutorial: Visual servo simulation on a pioneer-like unicycle robot.
Definition at line 103 of file vpSimulatorPioneer.h.
|
inherited |
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. |
|
inherited |
Robot control states.
BEGIN_VISP_NAMESPACE vpSimulatorPioneer::vpSimulatorPioneer | ( | ) |
Constructor.
Definition at line 54 of file vpSimulatorPioneer.cpp.
|
inlineinherited |
Return the transformation between the camera frame and the mobile robot end effector frame.
Definition at line 65 of file vpUnicycle.h.
|
inlineinherited |
Return the twist transformation from camera frame to the mobile robot end effector frame. This transformation allows to compute a velocity expressed in the end effector frame into the camera frame.
Definition at line 72 of file vpUnicycle.h.
References vpVelocityTwistMatrix::buildFrom().
|
inlineinherited |
Return the twist transformation from camera frame to the mobile robot end effector frame. This transformation allows to compute a velocity expressed in the end effector frame into the camera frame.
Definition at line 86 of file vpUnicycle.h.
References vpUnicycle::get_cVe().
Referenced by vpUnicycle::get_cVe().
|
inlineinherited |
Return the robot jacobian expressed in the end effector frame.
Definition at line 96 of file vpUnicycle.h.
Referenced by vpRobotPioneer::get_eJe(), get_eJe(), and vpSimulatorPioneerPan::get_eJe().
|
virtual |
Get the robot jacobian expressed in the end-effector frame. The jacobian expression is given in vpPioneer class.
_eJe | : A 6 by 2 matrix representing the robot jacobian expressed in the end-effector frame. |
Implements vpRobot.
Definition at line 86 of file vpSimulatorPioneer.cpp.
References vpUnicycle::get_eJe().
|
inherited |
Get the maximal rotation velocity that can be sent to the robot during a velocity control.
Definition at line 274 of file vpRobot.cpp.
References vpRobot::maxRotationVelocity.
Referenced by vpRobotPololuPtu::setPosition(), vpRobotPololuPtu::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inherited |
Get the maximal translation velocity that can be sent to the robot during a velocity control.
Definition at line 252 of file vpRobot.cpp.
References vpRobot::maxTranslationVelocity.
Referenced by vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inlineinherited |
Return robot degrees of freedom number.
|
inherited |
Return the current robot position in the specified frame.
Definition at line 217 of file vpRobot.cpp.
References vpRobot::getPosition().
|
virtual |
Get the robot position (frame has to be specified).
Implements vpRobot.
Definition at line 183 of file vpSimulatorPioneer.cpp.
References vpRobot::ARTICULAR_FRAME, vpRxyzVector::buildFrom(), vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpHomogeneousMatrix::extract(), vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpColVector::resize(), and wMc_.
void vpSimulatorPioneer::getPosition | ( | vpHomogeneousMatrix & | wMc | ) | const |
Get the robot position in the world frame.
Definition at line 162 of file vpSimulatorPioneer.cpp.
References wMc_.
|
inlineprotectedinherited |
|
inlinevirtualinherited |
Definition at line 155 of file vpRobot.h.
Referenced by vpRobotBiclops::getPosition(), vpRobotBiclops::getVelocity(), vpSimulatorCamera::setPosition(), vpRobotAfma4::setPosition(), vpRobotAfma6::setPosition(), vpRobotFranka::setPosition(), vpRobotUniversalRobots::setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotBiclops::setPosition(), vpRobotPololuPtu::setPosition(), vpRobotPtu46::setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpRobotAfma4::setRobotState(), vpRobotAfma6::setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), vpRobotPtu46::setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotBiclops::setVelocity(), vpRobotPololuPtu::setVelocity(), vpRobotPtu46::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpRobotFlirPtu::stopMotion(), vpRobotFranka::stopMotion(), vpRobotViper650::stopMotion(), and vpRobotViper850::stopMotion().
|
inlineinherited |
Return the sampling time.
Definition at line 73 of file vpRobotSimulator.h.
|
staticinherited |
Saturate velocities.
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. |
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.
Definition at line 164 of file vpRobot.cpp.
References vpException::dimensionError, and vpArray2D< Type >::size().
Referenced by vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), vpRobotKinova::setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().
|
inlineinherited |
Set the transformation between the camera frame and the end effector frame.
Definition at line 102 of file vpUnicycle.h.
Referenced by vpPioneer::vpPioneer(), and vpPioneerPan::vpPioneerPan().
|
inlineinherited |
Set the robot jacobian expressed in the end effector frame.
eJe | : The robot jacobian to set such as with the robot control velocities and the six dimension velocity skew. |
Definition at line 112 of file vpUnicycle.h.
Referenced by vpPioneer::vpPioneer(), and vpPioneerPan::vpPioneerPan().
|
inherited |
Set the maximal rotation velocity that can be sent to the robot during a velocity control.
w_max | : Maximum rotational velocity expressed in rad/s. |
Definition at line 261 of file vpRobot.cpp.
References vpRobot::maxRotationVelocity.
Referenced by vpRobotViper650::setMaxRotationVelocity(), and vpRobotViper850::setMaxRotationVelocity().
|
inherited |
Set the maximal translation velocity that can be sent to the robot during a velocity control.
v_max | : Maximum translation velocity expressed in m/s. |
Definition at line 240 of file vpRobot.cpp.
References vpRobot::maxTranslationVelocity.
|
protectedinherited |
Definition at line 208 of file vpRobot.cpp.
Referenced by vpSimulatorCamera::setVelocity(), setVelocity(), and vpSimulatorPioneerPan::setVelocity().
|
virtualinherited |
Reimplemented in vpRobotViper850, vpRobotViper650, vpRobotUniversalRobots, vpRobotPtu46, vpRobotFranka, vpRobotFlirPtu, vpRobotAfma6, vpRobotAfma4, vpRobotPololuPtu, and vpRobotBiclops.
Definition at line 202 of file vpRobot.cpp.
Referenced by vpSimulatorCamera::setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpRobotAfma4::setRobotState(), vpRobotAfma6::setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), vpRobotPtu46::setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpSimulatorCamera::setVelocity(), setVelocity(), and vpSimulatorPioneerPan::setVelocity().
|
inlinevirtualinherited |
Set the sampling time.
delta_t | : Sampling time in second used to compute the robot displacement from the velocity applied to the robot during this time. |
Definition at line 81 of file vpRobotSimulator.h.
|
virtual |
Send to the controller a velocity.
frame | : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented. |
v | : Velocity vector to apply to the robot, where is the linear translational velocity in m/s and is the rotational velocity in rad/s around the vertical axis. |
Depending on the velocity specified as input, the robot position is updated using the sampling time that can be modified using setSamplingTime().
Implements vpRobot.
Definition at line 105 of file vpSimulatorPioneer.cpp.
References vpRobot::ARTICULAR_FRAME, vpHomogeneousMatrix::buildFrom(), vpRobot::CAMERA_FRAME, vpUnicycle::cMe_, vpRobotSimulator::delta_t_, vpException::dimensionError, vpRobot::END_EFFECTOR_FRAME, vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpRobot::getRobotState(), vpHomogeneousMatrix::inverse(), vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, vpRobot::saturateVelocities(), vpRobot::setRobotFrame(), vpRobot::setRobotState(), vpArray2D< Type >::size(), vpRobot::STATE_VELOCITY_CONTROL, theta_, wMc_, wMe_, vpRobotException::wrongStateError, xm_, and ym_.
|
inlineinherited |
Definition at line 170 of file vpRobot.h.
Referenced by vpRobotAfma4::vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().
|
protectedinherited |
Definition at line 114 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
protectedinherited |
Definition at line 116 of file vpUnicycle.h.
Referenced by setVelocity(), and vpSimulatorPioneerPan::setVelocity().
|
protectedinherited |
Definition at line 60 of file vpRobotSimulator.h.
Referenced by vpSimulatorCamera::setVelocity(), setVelocity(), and vpSimulatorPioneerPan::setVelocity().
|
protectedinherited |
robot Jacobian expressed in the end-effector frame
Definition at line 106 of file vpRobot.h.
Referenced by vpRobotFlirPtu::get_eJe(), vpRobotAfma6::get_eJe(), vpRobotPtu46::get_eJe(), vpRobotAfma4::get_eJe(), vpRobotBiclops::get_eJe(), vpRobotKinova::get_eJe(), vpRobotPololuPtu::get_eJe(), vpRobotViper650::get_eJe(), vpRobotViper850::get_eJe(), vpSimulatorCamera::get_eJe(), vpRobot::operator=(), and vpRobotAfma4::setVelocity().
|
protectedinherited |
Definition at line 117 of file vpUnicycle.h.
|
protectedinherited |
is the robot Jacobian expressed in the end-effector frame available
Definition at line 108 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
protectedinherited |
robot Jacobian expressed in the robot reference frame available
Definition at line 110 of file vpRobot.h.
Referenced by vpRobotFlirPtu::get_fJe(), vpRobotAfma6::get_fJe(), vpRobotPtu46::get_fJe(), vpRobotAfma4::get_fJe(), vpRobotBiclops::get_fJe(), vpRobotKinova::get_fJe(), vpRobotPololuPtu::get_fJe(), vpRobotViper650::get_fJe(), vpRobotViper850::get_fJe(), and vpRobot::operator=().
|
protectedinherited |
is the robot Jacobian expressed in the robot reference frame available
Definition at line 112 of file vpRobot.h.
Referenced by vpRobot::operator=().
|
protectedinherited |
Definition at line 100 of file vpRobot.h.
Referenced by vpRobot::getMaxRotationVelocity(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotTemplate::init(), vpRobot::operator=(), vpRobot::setMaxRotationVelocity(), vpRobotPtu46::setVelocity(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().
|
staticprotectedinherited |
Definition at line 101 of file vpRobot.h.
Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().
|
protectedinherited |
Definition at line 98 of file vpRobot.h.
Referenced by vpRobot::getMaxTranslationVelocity(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotTemplate::init(), vpRobot::operator=(), and vpRobot::setMaxTranslationVelocity().
|
staticprotectedinherited |
Definition at line 99 of file vpRobot.h.
Referenced by vpRobotFlirPtu::init(), vpRobotKinova::init(), and vpRobotTemplate::init().
|
protectedinherited |
number of degrees of freedom
Definition at line 104 of file vpRobot.h.
Referenced by vpRobotPololuPtu::get_eJe(), vpRobotPololuPtu::get_fJe(), vpRobotKinova::getJointPosition(), vpRobotPololuPtu::getPosition(), vpRobotFlirPtu::init(), vpRobotKinova::init(), vpRobotUniversalRobots::init(), vpRobotTemplate::init(), vpRobot::operator=(), vpRobotUniversalRobots::readPosFile(), vpRobotKinova::setDoF(), vpRobotKinova::setJointVelocity(), vpRobotKinova::setPosition(), vpRobotPololuPtu::setPosition(), vpRobotPololuPtu::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotKinova::setVelocity(), vpRobotTemplate::setVelocity(), and vpRobotPololuPtu::vpRobotPololuPtu().
|
protectedinherited |
Definition at line 116 of file vpRobot.h.
Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().
|
protectedinherited |
Definition at line 115 of file vpRobot.h.
Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().
|
protected |
Definition at line 116 of file vpSimulatorPioneer.h.
Referenced by setVelocity().
|
protectedinherited |
Definition at line 118 of file vpRobot.h.
Referenced by vpRobotAfma4::init(), vpRobotAfma6::init(), vpRobotViper650::init(), vpRobotViper850::init(), vpRobot::operator=(), vpRobotAfma4::vpRobotAfma4(), vpRobotAfma6::vpRobotAfma6(), vpRobotViper650::vpRobotViper650(), and vpRobotViper850::vpRobotViper850().
|
protected |
Definition at line 108 of file vpSimulatorPioneer.h.
Referenced by getPosition(), and setVelocity().
|
protected |
Definition at line 111 of file vpSimulatorPioneer.h.
Referenced by setVelocity().
|
protected |
Definition at line 114 of file vpSimulatorPioneer.h.
Referenced by setVelocity().
|
protected |
Definition at line 115 of file vpSimulatorPioneer.h.
Referenced by setVelocity().