ViSP  2.8.0

#include <vpServo.h>

Public Types

enum  vpServoType {
  NONE, EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, EYETOHAND_L_cVe_eJe,
  EYETOHAND_L_cVf_fVe_eJe, EYETOHAND_L_cVf_fJe
}
 
enum  vpServoIteractionMatrixType { CURRENT, DESIRED, MEAN, USER_DEFINED }
 
enum  vpServoInversionType { TRANSPOSE, PSEUDO_INVERSE }
 
enum  vpServoPrintType {
  ALL, CONTROLLER, ERROR_VECTOR, FEATURE_CURRENT,
  FEATURE_DESIRED, GAIN, INTERACTION_MATRIX, MINIMUM
}
 

Public Member Functions

 vpServo ()
 
 vpServo (vpServoType _servoType)
 
virtual ~vpServo ()
 
vpVelocityTwistMatrix get_cVe () const
 
vpVelocityTwistMatrix get_cVf () const
 
vpVelocityTwistMatrix set_fVe () const
 
vpMatrix get_eJe () const
 
vpMatrix get_fJe () const
 
void kill ()
 
void setServo (vpServoType _servo_type)
 
void set_cVe (vpVelocityTwistMatrix &_cVe)
 
void set_cVf (vpVelocityTwistMatrix &_cVf)
 
void set_fVe (vpVelocityTwistMatrix &_fVe)
 
void set_cVe (vpHomogeneousMatrix &cMe)
 
void set_cVf (vpHomogeneousMatrix &cMf)
 
void set_fVe (vpHomogeneousMatrix &fMe)
 
void set_eJe (vpMatrix &_eJe)
 
void set_fJe (vpMatrix &_fJe)
 
void setInteractionMatrixType (const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
 
void setForceInteractionMatrixComputation (bool forceInteractionMatrixComputation)
 
void setLambda (double _lambda)
 
void setLambda (const double at_zero, const double at_infinity, const double deriv_at_zero)
 
void setLambda (const vpAdaptiveGain &_l)
 
void addFeature (vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
 
void addFeature (vpBasicFeature &s, const unsigned int select=vpBasicFeature::FEATURE_ALL)
 
vpMatrix computeInteractionMatrix ()
 
vpColVector computeError ()
 
vpColVector computeControlLaw ()
 
bool testInitialization ()
 
bool testUpdated ()
 
vpColVector secondaryTask (vpColVector &de2dt)
 
vpColVector secondaryTask (vpColVector &e2, vpColVector &de2dt)
 
unsigned int getDimension ()
 
vpColVector getError () const
 
vpMatrix getInteractionMatrix ()
 
vpMatrix getI_WpW () const
 
vpServoType getServoType () const
 
vpMatrix getTaskJacobian () const
 
vpMatrix getTaskJacobianPseudoInverse () const
 
double getTaskRank () const
 
vpColVector getTaskSingularValues ()
 
vpMatrix getWpW () const
 
void print (const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
 

Public Attributes

vpMatrix L
 
vpColVector error
 
vpMatrix J1
 
vpMatrix J1p
 
vpColVector s
 
vpColVector sStar
 
vpColVector e1
 
vpColVector e
 
vpColVector q_dot
 
vpColVector v
 
vpServoType servoType
 
unsigned int rankJ1
 
vpList< vpBasicFeature * > featureList
 
vpList< vpBasicFeature * > desiredFeatureList
 
vpList< unsigned int > featureSelectionList
 
vpAdaptiveGain lambda
 
int signInteractionMatrix
 
vpServoIteractionMatrixType interactionMatrixType
 
vpServoInversionType inversionType
 

Protected Member Functions

void init ()
 

Protected Attributes

vpVelocityTwistMatrix cVe
 
bool init_cVe
 
vpVelocityTwistMatrix cVf
 
bool init_cVf
 
vpVelocityTwistMatrix fVe
 
bool init_fVe
 
vpMatrix eJe
 
bool init_eJe
 
vpMatrix fJe
 
bool init_fJe
 
bool errorComputed
 
bool interactionMatrixComputed
 
unsigned int dim_task
 
bool taskWasKilled
 
bool forceInteractionMatrixComputation
 
vpMatrix WpW
 
vpMatrix I_WpW
 
vpColVector sv
 

Detailed Description

Class required to compute the visual servoing control law descbribed in [1] and [2].

Warning
To avoid potential memory leaks, it is mendatory to call explicitly the kill() function to destroy the task. Otherwise, the destructor ~vpServo() launch an exception vpServoException::notKilledProperly.

To learn how to use this class, we suggest first to follow the Tutorial: Image-based visual servo.

The example below shows how to build a position-based visual servo from 3D visual features $s=({^{c^*}}t_c,\theta u)$. In that case, we have $s^* = 0$. Let us denote $\theta u$ the angle/axis parametrization of the rotation ${^{c^*}}R_c$. Moreover, $ {^{c^*}}t_c$ and ${^{c^*}}R_c$ represent respectively the translation and the rotation between the desired camera frame and the current one obtained by pose estimation (see vpPose class).

#include <visp/vpColVector.h>
#include <visp/vpFeatureThetaU.h>
#include <visp/vpFeatureTranslation.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpMatrix.h>
#include <visp/vpServo.h>
int main()
{
// Creation of an homogeneous matrix that represent the displacement
// the camera has to achieve to move from the desired camera frame
// and the current one
// ... cdMc is here the result of a pose estimation
// Creation of the current visual feature s = (c*_t_c, ThetaU)
// Set the initial values of the current visual feature s = (c*_t_c, ThetaU)
s_t.buildFrom(cdMc);
s_tu.buildFrom(cdMc);
// Build the desired visual feature s* = (0,0)
vpFeatureTranslation s_star_t(vpFeatureTranslation::cdMc); // Default initialization to zero
vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc);// Default initialization to zero
vpColVector v; // Camera velocity
double error; // Task error
// Creation of the visual servo task.
vpServo task;
// Visual servo task initialization
// - Camera is monted on the robot end-effector and velocities are
// computed in the camera frame
// - Interaction matrix is computed with the current visual features s
// - Set the contant gain to 1
task.setLambda(1);
// - Add current and desired translation feature
task.addFeature(s_t, s_star_t);
// - Add current and desired ThetaU feature for the rotation
task.addFeature(s_tu, s_star_tu);
// Visual servoing loop. The objective is here to update the visual
// features s = (c*_t_c, ThetaU), compute the control law and apply
// it to the robot
do {
// ... cdMc is here the result of a pose estimation
// Update the current visual feature s
s_t.buildFrom(cdMc); // Update translation visual feature
s_tu.buildFrom(cdMc); // Update ThetaU visual feature
v = task.computeControlLaw(); // Compute camera velocity skew
error = ( task.getError() ).sumSquare(); // error = s^2 - s_star^2
} while (error > 0.0001); // Stop the task when current and desired visual features are close
// A call to kill() is requested here to destroy properly the current
// and desired feature lists.
task.kill();
}
Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu3D_cdMc_CamVelocity.cpp, servoSimu3D_cMcd_CamVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, servoViper850Point2DCamVelocityKalman.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeature.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 153 of file vpServo.h.

Member Enumeration Documentation

Enumerator
TRANSPOSE 
PSEUDO_INVERSE 

Definition at line 177 of file vpServo.h.

Enumerator
CURRENT 
DESIRED 
MEAN 
USER_DEFINED 

Definition at line 169 of file vpServo.h.

Enumerator
ALL 

Print all the task information.

CONTROLLER 

Print the type of controller law.

ERROR_VECTOR 

Print the error vector $(s-s^*)$.

FEATURE_CURRENT 

Print the current features $s$.

FEATURE_DESIRED 

Print the desired features $s^*$.

GAIN 

Print the gain $\lambda$.

INTERACTION_MATRIX 

Print the interaction matrix.

MINIMUM 

Same as vpServoPrintType::ERROR_VECTOR.

Definition at line 183 of file vpServo.h.

Enumerator
NONE 
EYEINHAND_CAMERA 
EYEINHAND_L_cVe_eJe 
EYETOHAND_L_cVe_eJe 
EYETOHAND_L_cVf_fVe_eJe 
EYETOHAND_L_cVf_fJe 

Definition at line 159 of file vpServo.h.

Constructor & Destructor Documentation

vpServo::vpServo ( )

Default constructor.

By default:

  • the interaction matrix $ L $ is computed with the desired features $ s^*$ ( $ L=L_{s^*}$). With setInteractionMatrixType() you can also use the interaction matrix with the current visual features $L_s$, or the mean $(L_s / 2 + L_{s^*} / 2)$.
  • the control law is build from the pseudo inverse of the interaction matrix $ v = - \lambda \; L^+ \; e$. With setInteractionMatrixType() can also set the usage of the transpose $ v = \lambda \; L^t \; e$ .

Definition at line 77 of file vpServo.cpp.

References init().

vpServo::vpServo ( vpServoType  _servoType)

constructor with Choice of the visual servoing control law

Definition at line 82 of file vpServo.cpp.

References setServo().

vpServo::~vpServo ( )
virtual

destructor

Destructor.

In fact, it does nothing. You have to call kill() to destroy the current and desired feature lists.

See also
kill()

Definition at line 98 of file vpServo.cpp.

References taskWasKilled, and vpTRACE.

Member Function Documentation

void vpServo::addFeature ( vpBasicFeature s,
vpBasicFeature s_star,
const unsigned int  select = vpBasicFeature::FEATURE_ALL 
)

create a new ste of two visual features

create a new set of 2 features in the task

Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeature.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 444 of file vpServo.cpp.

References desiredFeatureList, featureList, featureSelectionList, and s.

void vpServo::addFeature ( vpBasicFeature s,
const unsigned int  select = vpBasicFeature::FEATURE_ALL 
)

create a new ste of two visual features

create a new visual features in the task (the desired feature is then null)

Definition at line 456 of file vpServo.cpp.

References desiredFeatureList, vpBasicFeature::duplicate(), featureList, featureSelectionList, vpBasicFeature::init(), s, vpBasicFeature::setDeallocate(), and vpBasicFeature::vpServo.

vpColVector vpServo::computeControlLaw ( )

compute the desired control law

Compute the control law according to the equation:

\[ -\lambda {\bf W^+W\widehat J_s^+(s-s^*)} \]

or

\[ -\lambda {\bf\widehat J_s^+(s-s^*)} \]

if the dimension of the task is equal to number of available degrees of freedom (in that case ${\bf W^+W = I}$)

in this equation Js is function of the interaction matrix and of the robot Jacobian. It is also built according to the chosen configuration eye-in-hand or eye-to-hand (see vpServo::setServo method)

Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 883 of file vpServo.cpp.

References computeError(), computeInteractionMatrix(), cVe, cVf, e, e1, eJe, error, EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, EYETOHAND_L_cVe_eJe, EYETOHAND_L_cVf_fJe, EYETOHAND_L_cVf_fVe_eJe, fJe, fVe, vpMatrix::getCols(), I_WpW, init_cVe, init_eJe, init_fJe, init_fVe, inversionType, J1, J1p, lambda, NONE, PSEUDO_INVERSE, vpMatrix::pseudoInverse(), rankJ1, vpMatrix::resize(), vpServoException::servoError, servoType, vpMatrix::setIdentity(), signInteractionMatrix, sv, vpMatrix::t(), testInitialization(), testUpdated(), vpERROR_TRACE, and WpW.

vpColVector vpServo::computeError ( )

Compute the error $(s - s^*)$ between the current set of visual features $s$ and the desired set of visual features $s^*$.

Returns
The error $(s - s^*)$. To access to the computed error you can also use the public vpServo::error class member.

Definition at line 682 of file vpServo.cpp.

References desiredFeatureList, dim_task, vpList< type >::empty(), vpBasicFeature::error(), error, errorComputed, vpList< type >::front(), vpBasicFeature::get_s(), vpMatrix::getRows(), vpList< type >::next(), vpServoException::noFeatureError, vpList< type >::outside(), vpColVector::resize(), s, sStar, vpList< type >::value(), vpDEBUG_TRACE, and vpERROR_TRACE.

Referenced by computeControlLaw().

vpMatrix vpServo::computeInteractionMatrix ( )

compute the interaction matrix related to the set of visual features

Compute the interaction matrix related to the set of visual features.

Returns
Ls

Definition at line 590 of file vpServo.cpp.

References CURRENT, DESIRED, desiredFeatureList, dim_task, forceInteractionMatrixComputation, vpMatrix::getCols(), vpMatrix::getRows(), interactionMatrixComputed, interactionMatrixType, L, MEAN, USER_DEFINED, and vpERROR_TRACE.

Referenced by computeControlLaw().

vpVelocityTwistMatrix vpServo::get_cVe ( ) const
inline

Return the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.

Definition at line 206 of file vpServo.h.

vpVelocityTwistMatrix vpServo::get_cVf ( ) const
inline

Return the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.

Definition at line 210 of file vpServo.h.

vpMatrix vpServo::get_eJe ( ) const
inline

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

Definition at line 218 of file vpServo.h.

vpMatrix vpServo::get_fJe ( ) const
inline

Return the robot jacobian expressed in the robot fixed frame (also called world or base frame).

Definition at line 222 of file vpServo.h.

unsigned int vpServo::getDimension ( )
vpColVector vpServo::getError ( ) const
inline

Return the error $(s - s^*)$ between the current set of visual features $s$ and the desired set of visual features $s^*$. The error vector is updated after a call of computeError() or computeControlLaw().

vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
vpColVector e = task.getError(); // Get the error vector
Examples:
manServoMomentsSimple.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 301 of file vpServo.h.

Referenced by vpServoData::save().

vpMatrix vpServo::getI_WpW ( ) const
inline

Return the projection operator ${\bf I}-{\bf W}^+{\bf W}$. This operator is updated after a call of computeControlLaw().

vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator
See also
getWpW()

Definition at line 334 of file vpServo.h.

vpMatrix vpServo::getInteractionMatrix ( )
inline

Definition at line 317 of file vpServo.h.

vpServoType vpServo::getServoType ( ) const
inline

Return the visual servo type.

Definition at line 341 of file vpServo.h.

vpMatrix vpServo::getTaskJacobian ( ) const
inline

Return the task jacobian $J_1$. The task jacobian is updated after a call of computeControlLaw().

In the general case, the task jacobian is given by $J_1 = L {^c}V_a {^a}J_e$.

vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
vpMatrix J1 = task.getTaskJacobian(); // Get the task jacobian used to compute v
See also
getTaskJacobianPseudoInverse(), getInteractionMatrix()
Examples:
servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp.

Definition at line 357 of file vpServo.h.

vpMatrix vpServo::getTaskJacobianPseudoInverse ( ) const
inline

Return the pseudo inverse of the task jacobian $J_1$. The task jacobian and its pseudo inverse are updated after a call of computeControlLaw().

Returns
Pseudo inverse ${J_1}^{+}$ of the task jacobian.
vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
vpMatrix J1p = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v
See also
getTaskJacobian()

Definition at line 375 of file vpServo.h.

double vpServo::getTaskRank ( ) const
inline

Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw().

vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
double rank = task.getTaskRank(); // Get the rank of the task jacobian
Examples:
servoAfma6Ellipse2DCamVelocity.cpp, servoPioneerPanSegment3D.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuSphere2DCamVelocity.cpp, and simulateCircle2DCamVelocity.cpp.

Definition at line 389 of file vpServo.h.

vpColVector vpServo::getTaskSingularValues ( )
inline

Get task singular values.

Returns
Singular values that relies on the task jacobian pseudo inverse.

Definition at line 399 of file vpServo.h.

vpMatrix vpServo::getWpW ( ) const
inline

Return the projection operator ${\bf W}^+{\bf W}$. This operator is updated after a call of computeControlLaw().

vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
vpMatrix WpW = task.getWpW(); // Get the projection operator
See also
getI_WpW()

Definition at line 416 of file vpServo.h.

void vpServo::init ( void  )
protected

basic initialization

Initialize the servo.

By default:

  • the interaction matrix $ L $ is computed with the desired features $ s^*$ ( $ L=L_{s^*}$). With setInteractionMatrixType() you can also use the interaction matrix with the current visual features $L_s$, or the mean $(L_s / 2 + L_{s^*} / 2)$.
  • the control law is build from the pseudo inverse of the interaction matrix $ v = - \lambda \; L^+ \; e$. With setInteractionMatrixType() can also set the usage of the transpose $ v = \lambda \; L^t \; e$ .

Definition at line 128 of file vpServo.cpp.

References DESIRED, desiredFeatureList, dim_task, errorComputed, featureList, featureSelectionList, forceInteractionMatrixComputation, init_cVe, init_cVf, init_eJe, init_fJe, init_fVe, interactionMatrixComputed, interactionMatrixType, inversionType, vpList< type >::kill(), NONE, PSEUDO_INVERSE, servoType, signInteractionMatrix, and taskWasKilled.

Referenced by vpServo().

void vpServo::kill ( )

destruction (memory deallocation if required)

Task destruction. Kill the current and desired visual feature lists.

It is mendatory to call explicitly this function to avoid potential memory leaks.

vpServo task ;
...
task.addFeature(s); // Add current ThetaU feature
task.kill(); // A call to kill() is requested here
Examples:
manServo4PointsDisplay.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeature.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 177 of file vpServo.cpp.

References desiredFeatureList, featureList, vpList< type >::front(), vpBasicFeature::getDeallocate(), vpList< type >::kill(), vpList< type >::next(), vpList< type >::outside(), taskWasKilled, vpList< type >::value(), and vpBasicFeature::vpServo.

void vpServo::print ( const vpServo::vpServoPrintType  displayLevel = ALL,
std::ostream &  os = std::cout 
)

Prints on os stream information about the task:

Parameters
displayLevel: If vpServo::ALL prints
  • Type of control law (eye-in-hand, eye-to-hand)
  • Control frame (camera frame, joint space)
  • List of current visual features : s
  • List of desired visual features : s*
  • Interaction Matrix Ls
  • Error vector (s-s*)
  • Gain of the controller
displayLevel: If vpServo::MINIMUM prints only the error vector (s-s*).
os: Output stream.
Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 258 of file vpServo.cpp.

References ALL, CONTROLLER, desiredFeatureList, error, ERROR_VECTOR, errorComputed, EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, EYETOHAND_L_cVe_eJe, EYETOHAND_L_cVf_fJe, EYETOHAND_L_cVf_fVe_eJe, FEATURE_CURRENT, FEATURE_DESIRED, featureList, featureSelectionList, vpList< type >::front(), GAIN, INTERACTION_MATRIX, interactionMatrixComputed, L, lambda, MINIMUM, vpList< type >::next(), NONE, vpList< type >::outside(), vpBasicFeature::print(), s, servoType, vpColVector::t(), and vpList< type >::value().

vpColVector vpServo::secondaryTask ( vpColVector de2dt)

Add a secondary task.

Compute the secondary task according to the projection operator. See equation (7) of the IEEE RA magazine, dec 2005 paper.

Warning
computeControlLaw() must be call prior to this function.

Compute the vector:

\[ + ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t} \]

to be added to the primary task

\[ -\lambda {\bf W^+W\widehat J_s^+(s-s^*)} \]

which is computed using the computeControlLaw() method.

Warning
The projection operator $ \bf W^+W $ is computed in computeControlLaw() which must be called prior to this function.
See also
computeControlLaw()
Examples:
servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, and servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp.

Definition at line 1053 of file vpServo.cpp.

References vpMatrix::getCols(), I_WpW, J1, vpServoException::noDofFree, rankJ1, vpMatrix::resize(), vpMatrix::setIdentity(), vpERROR_TRACE, and WpW.

vpColVector vpServo::secondaryTask ( vpColVector e2,
vpColVector de2dt 
)

Add a secondary task.

Compute the secondary task according to the projection operator. See equation (7) of the IEEE RA magazine, dec 2005 paper.

Warning
computeControlLaw() must be call prior to this function.

Compute the vector:

\[ -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t} \]

to be added to the primary task

\[ -\lambda {\bf W^+W\widehat J_s^+(s-s^*)} \]

which is computed using computeControlLaw() method

Warning
The projection operator $ \bf W^+W $ is computed in computeControlLaw() which must be called prior to this function.
See also
computeControlLaw()

Definition at line 1101 of file vpServo.cpp.

References e1, vpMatrix::getCols(), I_WpW, J1, lambda, vpServoException::noDofFree, rankJ1, vpMatrix::resize(), vpMatrix::setIdentity(), vpERROR_TRACE, and WpW.

void vpServo::set_cVe ( vpHomogeneousMatrix cMe)
inline

Definition at line 234 of file vpServo.h.

void vpServo::set_cVf ( vpVelocityTwistMatrix _cVf)
inline

Definition at line 231 of file vpServo.h.

void vpServo::set_cVf ( vpHomogeneousMatrix cMf)
inline

Definition at line 235 of file vpServo.h.

void vpServo::set_fJe ( vpMatrix _fJe)
inline

Definition at line 239 of file vpServo.h.

vpVelocityTwistMatrix vpServo::set_fVe ( ) const
inline

Return the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).

Definition at line 214 of file vpServo.h.

void vpServo::set_fVe ( vpVelocityTwistMatrix _fVe)
inline

Definition at line 232 of file vpServo.h.

void vpServo::set_fVe ( vpHomogeneousMatrix fMe)
inline

Definition at line 236 of file vpServo.h.

void vpServo::setForceInteractionMatrixComputation ( bool  forceInteractionMatrixComputation)
inline

Set a variable which enable to compute the interaction matrix for each iteration.

Parameters
forceInteractionMatrixComputationIf true it forces the interaction matrix computation even if it is already done.

Definition at line 250 of file vpServo.h.

void vpServo::setInteractionMatrixType ( const vpServoIteractionMatrixType interactionMatrixType,
const vpServoInversionType interactionMatrixInversion = PSEUDO_INVERSE 
)

Set the type of the interaction matrix (current, mean, desired, user).

Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp, servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoBiclopsPoint2DArtVelocity.cpp, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuSphere.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 509 of file vpServo.cpp.

References interactionMatrixType, and inversionType.

void vpServo::setLambda ( double  _lambda)
inline

set the gain lambda

Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 253 of file vpServo.h.

void vpServo::setLambda ( const double  at_zero,
const double  at_infinity,
const double  deriv_at_zero 
)
inline

Definition at line 254 of file vpServo.h.

void vpServo::setLambda ( const vpAdaptiveGain _l)
inline

Definition at line 258 of file vpServo.h.

void vpServo::setServo ( vpServoType  _servo_type)

Choice of the visual servoing control law.

Examples:
manServo4PointsDisplay.cpp, manServoMomentsSimple.cpp, manSimu4Dots.cpp, manSimu4Points.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.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, servoMomentImage.cpp, servoMomentPoints.cpp, servoMomentPolygon.cpp, servoPioneerPanSegment3D.cpp, servoPioneerPoint2DDepth.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimu4Points.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuCircle2DCamVelocity.cpp, servoSimuCircle2DCamVelocityDisplay.cpp, servoSimuCylinder.cpp, servoSimuCylinder2DCamVelocityDisplay.cpp, servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp, servoSimuFourPoints2DCamVelocity.cpp, servoSimuFourPoints2DCamVelocityDisplay.cpp, servoSimuFourPoints2DPolarCamVelocityDisplay.cpp, servoSimuLine2DCamVelocityDisplay.cpp, servoSimuPoint2DCamVelocity1.cpp, servoSimuPoint2DCamVelocity2.cpp, servoSimuPoint2DCamVelocity3.cpp, servoSimuPoint2DhalfCamVelocity1.cpp, servoSimuPoint2DhalfCamVelocity2.cpp, servoSimuPoint2DhalfCamVelocity3.cpp, servoSimuPoint3DCamVelocity.cpp, servoSimuSphere.cpp, servoSimuSphere2DCamVelocity.cpp, servoSimuSphere2DCamVelocitySecondaryTask.cpp, servoSimuSquareLine2DCamVelocityDisplay.cpp, servoSimuThetaUCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp, servoViper850FourPoints2DArtVelocityInteractionDesired.cpp, servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, testFeatureSegment.cpp, tutorial-ibvs-4pts-display.cpp, tutorial-ibvs-4pts-image-tracking.cpp, tutorial-ibvs-4pts-ogre-tracking.cpp, tutorial-ibvs-4pts-ogre.cpp, tutorial-ibvs-4pts-plotter.cpp, tutorial-ibvs-4pts-wireframe-camera.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, tutorial-ibvs-4pts-wireframe-robot-viper.cpp, tutorial-ibvs-4pts.cpp, tutorial-simu-pioneer-pan.cpp, and tutorial-simu-pioneer.cpp.

Definition at line 214 of file vpServo.cpp.

References vpMatrix::eye(), EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, servoType, set_cVe(), set_eJe(), and signInteractionMatrix.

Referenced by vpServo().

bool vpServo::testInitialization ( )

test if all the initialization are correct if true control law can be computed

Definition at line 796 of file vpServo.cpp.

References EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, EYETOHAND_L_cVe_eJe, EYETOHAND_L_cVf_fJe, EYETOHAND_L_cVf_fVe_eJe, init_cVe, init_cVf, init_eJe, init_fJe, init_fVe, NONE, vpServoException::servoError, servoType, and vpERROR_TRACE.

Referenced by computeControlLaw().

bool vpServo::testUpdated ( )

test if all the update are correct if true control law can be computed

Definition at line 831 of file vpServo.cpp.

References EYEINHAND_CAMERA, EYEINHAND_L_cVe_eJe, EYETOHAND_L_cVe_eJe, EYETOHAND_L_cVf_fJe, EYETOHAND_L_cVf_fVe_eJe, init_cVe, init_eJe, init_fJe, init_fVe, NONE, vpServoException::servoError, servoType, and vpERROR_TRACE.

Referenced by computeControlLaw().

Member Data Documentation

vpVelocityTwistMatrix vpServo::cVe
protected

Twist transformation matrix between Re and Rc.

Definition at line 490 of file vpServo.h.

Referenced by computeControlLaw().

vpVelocityTwistMatrix vpServo::cVf
protected

Twist transformation matrix between Rf and Rc.

Definition at line 493 of file vpServo.h.

Referenced by computeControlLaw().

vpList<vpBasicFeature *> vpServo::desiredFeatureList

List of desired visual features (produce $s^*$)

Definition at line 467 of file vpServo.h.

Referenced by addFeature(), computeError(), computeInteractionMatrix(), vpServoDisplay::display(), init(), kill(), and print().

unsigned int vpServo::dim_task
protected

dimension of the task

Definition at line 519 of file vpServo.h.

Referenced by computeError(), computeInteractionMatrix(), getDimension(), and init().

vpColVector vpServo::e

Task $e = e_1 + (I-{J_1}^{+} J_1) e_2$.

Definition at line 450 of file vpServo.h.

Referenced by computeControlLaw().

vpColVector vpServo::e1

Primary task $e_1 = {J_1}^{+}(s-s*)$.

Definition at line 448 of file vpServo.h.

Referenced by computeControlLaw(), and secondaryTask().

vpMatrix vpServo::eJe
protected

Jacobian expressed in the end-effector frame.

Definition at line 504 of file vpServo.h.

Referenced by computeControlLaw().

vpColVector vpServo::error

Error $(s - s^*)$ between the current set of visual features $s$ and the desired set of visual features $s^*$. This vector is updated after a call of computeError() or computeControlLaw().

Examples:
servoMomentImage.cpp.

Definition at line 434 of file vpServo.h.

Referenced by computeControlLaw(), computeError(), and print().

bool vpServo::errorComputed
protected

true if the error has been computed

Definition at line 515 of file vpServo.h.

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

vpList<vpBasicFeature *> vpServo::featureList

List of visual features (produce $s$)

Definition at line 465 of file vpServo.h.

Referenced by addFeature(), vpServoDisplay::display(), getDimension(), init(), kill(), and print().

vpList<unsigned int> vpServo::featureSelectionList

List of selection among visual features used for selection of a subset of each visual feature if required

Definition at line 470 of file vpServo.h.

Referenced by addFeature(), getDimension(), init(), and print().

vpMatrix vpServo::fJe
protected

Jacobian expressed in the robot reference frame.

Definition at line 507 of file vpServo.h.

Referenced by computeControlLaw().

bool vpServo::forceInteractionMatrixComputation
protected

Force the interaction matrix computation even if it is already done.

Definition at line 522 of file vpServo.h.

Referenced by computeInteractionMatrix(), and init().

vpVelocityTwistMatrix vpServo::fVe
protected

Twist transformation matrix between Re and Rf.

Definition at line 496 of file vpServo.h.

Referenced by computeControlLaw().

vpMatrix vpServo::I_WpW
protected

projection operators I-WpW

Definition at line 527 of file vpServo.h.

Referenced by computeControlLaw(), and secondaryTask().

bool vpServo::init_cVe
protected

Definition at line 491 of file vpServo.h.

Referenced by computeControlLaw(), init(), testInitialization(), and testUpdated().

bool vpServo::init_cVf
protected

Definition at line 494 of file vpServo.h.

Referenced by init(), and testInitialization().

bool vpServo::init_eJe
protected

Definition at line 505 of file vpServo.h.

Referenced by computeControlLaw(), init(), testInitialization(), and testUpdated().

bool vpServo::init_fJe
protected

Definition at line 508 of file vpServo.h.

Referenced by computeControlLaw(), init(), testInitialization(), and testUpdated().

bool vpServo::init_fVe
protected

Definition at line 497 of file vpServo.h.

Referenced by computeControlLaw(), init(), testInitialization(), and testUpdated().

bool vpServo::interactionMatrixComputed
protected

true if the interaction matrix has been computed

Definition at line 517 of file vpServo.h.

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

vpServoIteractionMatrixType vpServo::interactionMatrixType

Type of the interaction matrox (current, mean, desired, user)

Definition at line 479 of file vpServo.h.

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

vpServoInversionType vpServo::inversionType

Indicates if the transpose or the pseudo inverse of the interaction matrix should be used to compute the task

Definition at line 482 of file vpServo.h.

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

vpMatrix vpServo::J1

Task Jacobian $J_1 = L {^c}V_a {^a}J_e$.

Definition at line 436 of file vpServo.h.

Referenced by computeControlLaw(), and secondaryTask().

vpMatrix vpServo::J1p

Pseudo inverse ${J_1}^{+}$ of the task Jacobian.

Definition at line 438 of file vpServo.h.

Referenced by computeControlLaw().

vpMatrix vpServo::L

Interaction matrix.

Examples:
servoMomentImage.cpp.

Definition at line 430 of file vpServo.h.

Referenced by computeInteractionMatrix(), and print().

vpAdaptiveGain vpServo::lambda

Gain.

Definition at line 473 of file vpServo.h.

Referenced by computeControlLaw(), print(), and secondaryTask().

vpColVector vpServo::q_dot

Articular velocity.

Definition at line 454 of file vpServo.h.

Referenced by vpServoData::save().

unsigned int vpServo::rankJ1

Rank of the task Jacobian.

Definition at line 462 of file vpServo.h.

Referenced by computeControlLaw(), and secondaryTask().

vpColVector vpServo::s

Current state of visual features $s$. This vector is updated after a call of computeError() or computeControlLaw().

Examples:
servoBiclopsPoint2DArtVelocity.cpp.

Definition at line 442 of file vpServo.h.

Referenced by addFeature(), computeError(), print(), and vpServoData::save().

vpServoType vpServo::servoType

Chosen visual servoing control law.

Definition at line 459 of file vpServo.h.

Referenced by computeControlLaw(), init(), print(), setServo(), testInitialization(), and testUpdated().

int vpServo::signInteractionMatrix

Sign of the interaction +/- 1 (1 for eye-in-hand, -1 for eye-to-hand configuration)

Definition at line 477 of file vpServo.h.

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

vpColVector vpServo::sStar

Desired state of visual features $s^*$. This vector is updated after a call of computeError() or computeControlLaw().

Examples:
servoBiclopsPoint2DArtVelocity.cpp.

Definition at line 445 of file vpServo.h.

Referenced by computeError(), and vpServoData::save().

vpColVector vpServo::sv
protected

Definition at line 529 of file vpServo.h.

Referenced by computeControlLaw().

bool vpServo::taskWasKilled
protected

Definition at line 520 of file vpServo.h.

Referenced by init(), kill(), and ~vpServo().

vpColVector vpServo::v

Camera velocity.

Definition at line 456 of file vpServo.h.

vpMatrix vpServo::WpW
protected

projection operators WpW

Definition at line 525 of file vpServo.h.

Referenced by computeControlLaw(), and secondaryTask().