![]() |
Visual Servoing Platform
version 3.3.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 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 |
std::list< vpBasicFeature * > | featureList |
std::list< vpBasicFeature * > | desiredFeatureList |
std::list< unsigned int > | featureSelectionList |
vpAdaptiveGain | lambda |
int | signInteractionMatrix |
vpServoIteractionMatrixType | interactionMatrixType |
vpServoInversionType | inversionType |
Protected Member Functions | |
void | init () |
void | computeProjectionOperators () |
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 |
vpMatrix | P |
vpColVector | sv |
double | mu |
vpColVector | e1_initial |
bool | iscJcIdentity |
vpMatrix | cJc |
Class required to compute the visual servoing control law descbribed in [Chaumette06a] and [Chaumette07a].
To learn how to use this class, we suggest first to follow the Tutorial: Image-based visual servo. The Tutorial: Visual servo simulation on a pioneer-like unicycle robot and Tutorial: How to boost your visual servo control law are also useful for advanced usage of this class.
The example below shows how to build a position-based visual servo from 3D visual features . In that case, we have
. Let us denote
the angle/axis parametrization of the rotation
. Moreover,
and
represent respectively the translation and the rotation between the desired camera frame and the current one obtained by pose estimation (see vpPose class).
Enumerator | |
---|---|
TRANSPOSE | In the control law (see vpServo::vpServoType), uses the transpose instead of the pseudo inverse. |
PSEUDO_INVERSE | In the control law (see vpServo::vpServoType), uses the pseudo inverse. |
Enumerator | |
---|---|
CURRENT | In the control law (see vpServo::vpServoType), uses the interaction matrix |
DESIRED | In the control law (see vpServo::vpServoType), uses the interaction matrix |
MEAN | In the control law (see vpServo::vpServoType), uses the interaction matrix |
USER_DEFINED | In the control law (see vpServo::vpServoType), uses an interaction matrix set by the user. |
enum vpServo::vpServoType |
vpServo::vpServo | ( | ) |
Default constructor that initializes the following settings:
Definition at line 68 of file vpServo.cpp.
|
explicit |
Constructor that allows to choose the visual servoing control law.
servo_type | : Visual servoing control law. |
The other settings are the following:
Definition at line 93 of file vpServo.cpp.
|
virtual |
Destructor.
In fact, it does nothing. You have to call kill() to destroy the current and desired feature lists.
Definition at line 115 of file vpServo.cpp.
References taskWasKilled, and vpTRACE.
void vpServo::addFeature | ( | vpBasicFeature & | s_cur, |
unsigned int | select = vpBasicFeature::FEATURE_ALL |
||
) |
Add a new features in the task. The desired visual feature denoted
is equal to zero.
s_cur | : Current visual feature denoted ![]() |
select | : Feature selector. By default all the features in s are used, but is is possible to specify which one is used in case of multiple features. |
The following sample code explain how to use this method to add a feature:
For example to use only the feature, the previous code becomes:
Definition at line 530 of file vpServo.cpp.
void vpServo::addFeature | ( | vpBasicFeature & | s_cur, |
vpBasicFeature & | s_star, | ||
unsigned int | select = vpBasicFeature::FEATURE_ALL |
||
) |
Add a new set of 2 features and
in the task.
s_cur | : Current visual feature denoted ![]() |
s_star | : Desired visual feature denoted ![]() |
select | : Feature selector. By default all the features in s and s_star are used, but is is possible to specify which one is used in case of multiple features. |
The following sample code explain how to use this method to add a visual feature point :
For example to use only the visual feature, the previous code becomes:
Definition at line 496 of file vpServo.cpp.
vpColVector vpServo::computeControlLaw | ( | ) |
Compute the control law specified using setServo(). See vpServo::vpServoType for more details concerning the control laws that are available. The Tutorial: Image-based visual servo and Tutorial: How to boost your visual servo control law are also useful to illustrate the usage of this function.
The general form of the control law is the following:
where :
To ensure continuous sequencing the computeControlLaw(double) function can be used. It will ensure that the velocities that are computed are continuous.
Definition at line 934 of file vpServo.cpp.
vpColVector vpServo::computeControlLaw | ( | double | t | ) |
Compute the control law specified using setServo(). See vpServo::vpServoType for more details concerning the control laws that are available. The Tutorial: How to boost your visual servo control law is also useful to illustrate the usage of this function.
To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from the task sequencing approach described in [Mansard07e] equation (17). This additional term allows to compute continuous velocities by avoiding abrupt changes in the command.
The form of the control law considered here is the following:
where :
t | : Time in second. When set to zero, ![]() |
Definition at line 1086 of file vpServo.cpp.
vpColVector vpServo::computeControlLaw | ( | double | t, |
const vpColVector & | e_dot_init | ||
) |
Compute the control law specified using setServo(). See vpServo::vpServoType for more details concerning the control laws that are available.
To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from the task sequencing approach described in [Mansard07e] equation (17). This additional term allows to compute continuous velocities by avoiding abrupt changes in the command.
The form of the control law considered here is the following:
where :
t | : Time in second. When set to zero, ![]() |
e_dot_init | : Initial value of ![]() |
Definition at line 1247 of file vpServo.cpp.
vpColVector vpServo::computeError | ( | ) |
Compute the error between the current set of visual features
and the desired set of visual features
.
Definition at line 715 of file vpServo.cpp.
vpMatrix vpServo::computeInteractionMatrix | ( | ) |
Compute and return the interaction matrix related to the set of visual features.
Definition at line 653 of file vpServo.cpp.
|
protected |
Compute the classic projetion operator and the large projection operator.
Definition at line 1372 of file vpServo.cpp.
References error, vpColVector::frobeniusNorm(), vpArray2D< Type >::getCols(), I_WpW, J1, P, vpArray2D< Type >::resize(), vpColVector::t(), vpMatrix::transpose(), and WpW.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
unsigned int vpServo::getDimension | ( | ) | const |
Return the task dimension.
Definition at line 559 of file vpServo.cpp.
|
inline |
Return the error between the current set of visual features
and the desired set of visual features
. The error vector is updated after a call of computeError() or computeControlLaw().
vpMatrix vpServo::getI_WpW | ( | ) | const |
Return the projection operator . This operator is updated after a call of computeControlLaw().
Definition at line 1759 of file vpServo.cpp.
vpMatrix vpServo::getLargeP | ( | ) | const |
Return the large projection operator. This operator is updated after a call of computeControlLaw().
Definition at line 1773 of file vpServo.cpp.
|
inline |
vpMatrix vpServo::getTaskJacobian | ( | ) | const |
Return the task jacobian . The task jacobian is updated after a call of computeControlLaw().
In the general case, the task jacobian is given by .
Definition at line 1789 of file vpServo.cpp.
vpMatrix vpServo::getTaskJacobianPseudoInverse | ( | ) | const |
Return the pseudo inverse of the task jacobian .
In the general case, the task jacobian is given by .
The task jacobian and its pseudo inverse are updated after a call of computeControlLaw().
Definition at line 1808 of file vpServo.cpp.
unsigned int vpServo::getTaskRank | ( | ) | const |
Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw().
Definition at line 1820 of file vpServo.cpp.
|
inline |
vpMatrix vpServo::getWpW | ( | ) | const |
Return the projection operator . This operator is updated after a call of computeControlLaw().
When the dimension of the task is equal to the number of degrees of freedom available .
Definition at line 1837 of file vpServo.cpp.
|
protected |
Basic initialization.
Initialize the servo with the following settings:
Definition at line 141 of file vpServo.cpp.
void vpServo::kill | ( | ) |
Task destruction. Kill the current and desired visual feature lists.
It is mendatory to call explicitly this function to avoid potential memory leaks.
Definition at line 191 of file vpServo.cpp.
void vpServo::print | ( | const vpServo::vpServoPrintType | displayLevel = ALL , |
std::ostream & | os = std::cout |
||
) |
Prints on os stream information about the task:
displayLevel | : Indicates which are the task information to print. See vpServo::vpServoPrintType for more details. |
os | : Output stream. |
Definition at line 312 of file vpServo.cpp.
vpColVector vpServo::secondaryTask | ( | const vpColVector & | de2dt, |
const bool & | useLargeProjectionOperator = false |
||
) |
Compute and return the secondary task vector according to the classic projection operator (see equation(7) in the paper [Marchand05b]) or the new large projection operator (see equation(24) in the paper [Marey:2010]).
de2dt | : Value of ![]() ![]() |
useLargeProjectionOperator | : if true will be use the large projection operator, if false the classic one (default). |
If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
where
with
The following sample code shows how to use this method to compute a secondary task using the classic projection operator:
The following sample code shows how to use this method to compute a secondary task using the large projection operator:
Definition at line 1484 of file vpServo.cpp.
vpColVector vpServo::secondaryTask | ( | const vpColVector & | e2, |
const vpColVector & | de2dt, | ||
const bool & | useLargeProjectionOperator = false |
||
) |
Compute and return the secondary task vector according to the classic projection operator (see equation(7) in the paper [Marchand05b]) or the new large projection operator (see equation(24) in the paper [Marey:2010]).
e2 | : Value of the secondary task ![]() |
de2dt | : Value of ![]() ![]() |
useLargeProjectionOperator | if true will be use the large projection operator, if false the classic one (default). |
If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
where
with
The following sample code shows how to use this method to compute a secondary task using the classical projection operator:
The following sample code shows how to use this method to compute a secondary task using the large projection operator:
Definition at line 1588 of file vpServo.cpp.
vpColVector vpServo::secondaryTaskJointLimitAvoidance | ( | const vpColVector & | q, |
const vpColVector & | dq, | ||
const vpColVector & | qmin, | ||
const vpColVector & | qmax, | ||
const double & | rho = 0.1 , |
||
const double & | rho1 = 0.3 , |
||
const double & | lambda_tune = 0.7 |
||
) | const |
Compute and return the secondary task vector for joint limit avoidance [Marey:2010b] using the new large projection operator (see equation(24) in the paper [Marey:2010]). The robot avoids the joint limits very smoothly even when the main task constrains all the robot degrees of freedom.
q | : Actual joint positions vector |
dq | : Actual joint velocities vector |
qmin | : Vector containing the low limit value of each joint in the chain. |
qmax | : Vector containing the high limit value of each joint in the chain. |
rho | : tuning paramenter ![]() ![]() ![]() |
rho1 | : tuning paramenter ![]() ![]() ![]() |
lambda_tune | : value ![]() |
Definition at line 1666 of file vpServo.cpp.
|
inline |
|
inline |
Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
|
inline |
|
inline |
|
inline |
Set the robot jacobian expressed in the end-effector frame.
|
inline |
|
inline |
|
inline |
void vpServo::setCameraDoF | ( | const vpColVector & | dof | ) |
Set a 6-dim column vector representing the degrees of freedom that are controlled in the camera frame. When set to 1, all the 6 dof are controlled.
dof | : Degrees of freedom to control in the camera frame. Below we give the correspondance between the index of the vector and the considered dof:
|
The following example shows how to use this function to control only wx, wy like a pan/tilt:
Definition at line 288 of file vpServo.cpp.
|
inline |
Set a variable which enables to compute the interaction matrix at each iteration.
When the interaction matrix is computed from the desired features which are in general constant, the interaction matrix
is computed just at the first iteration of the servo loop. Sometimes, when the desired features are time dependent
or varying, the interaction matrix need to be computed at each iteration of the servo loop. This method allows to force the computation of
in this particular case.
force_computation | : If true it forces the interaction matrix computation even if it is already done. |
void vpServo::setInteractionMatrixType | ( | const vpServoIteractionMatrixType & | interactionMatrixType, |
const vpServoInversionType & | interactionMatrixInversion = PSEUDO_INVERSE |
||
) |
Set the interaction matrix type (current, desired, mean or user defined) and how its inverse is computed.
interactionMatrixType | : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for more details. |
interactionMatrixInversion | : How is the inverse computed. See vpServo::vpServoInversionType for more details. |
Definition at line 573 of file vpServo.cpp.
References interactionMatrixType, and inversionType.
|
inline |
Set the gain used in the control law (see vpServo::vpServoType) as adaptive. Value of
that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
|
inline |
Set the gain used in the control law (see vpServo::vpServoType) as constant.
The usage of an adaptive gain allows to reduce the convergence time, see setLambda(const vpAdaptiveGain&).
c | : Constant gain. Values are in general between 0.1 and 1. Higher is the gain, higher are the velocities that may be applied to the robot. |
|
inline |
Set the gain used in the control law (see vpServo::vpServoType) as adaptive. Value of
that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
gain_at_zero | : the expected gain when ![]() ![]() |
gain_at_infinity | : the expected gain when ![]() ![]() |
slope_at_zero | : the expected slope of ![]() ![]() ![]() |
For more details on these parameters see vpAdaptiveGain class.
|
inline |
Set the value of the parameter used to ensure the continuity of the velocities computed using computeControlLaw(double).
A recommended value is 4.
void vpServo::setServo | ( | const vpServoType & | servo_type | ) |
Set the visual servoing control law.
servo_type | : Control law that will be considered. See vpServo::vpServoType to see the possible values. |
Definition at line 222 of file vpServo.cpp.
bool vpServo::testInitialization | ( | ) |
Test if all the initialization are correct. If true, the 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_cVf, init_eJe, init_fJe, init_fVe, NONE, vpServoException::servoError, servoType, and vpERROR_TRACE.
bool vpServo::testUpdated | ( | ) |
Test if all the update are correct. If true control law can be computed.
Definition at line 870 of file vpServo.cpp.
|
protected |
|
protected |
|
protected |
std::list<vpBasicFeature *> vpServo::desiredFeatureList |
|
protected |
Dimension of the task updated during computeControlLaw().
vpColVector vpServo::e |
vpColVector vpServo::e1 |
|
protected |
|
protected |
vpColVector vpServo::error |
Error between the current set of visual features
and the desired set of visual features
. This vector is updated after a call of computeError() or computeControlLaw().
Definition at line 548 of file vpServo.h.
Referenced by computeProjectionOperators().
|
protected |
std::list<vpBasicFeature *> vpServo::featureList |
std::list<unsigned int> vpServo::featureSelectionList |
|
protected |
|
protected |
|
protected |
|
protected |
Projection operators .
Definition at line 643 of file vpServo.h.
Referenced by computeProjectionOperators().
|
protected |
Definition at line 606 of file vpServo.h.
Referenced by testInitialization().
|
protected |
Definition at line 609 of file vpServo.h.
Referenced by testInitialization().
|
protected |
Definition at line 620 of file vpServo.h.
Referenced by testInitialization().
|
protected |
Definition at line 623 of file vpServo.h.
Referenced by testInitialization().
|
protected |
Definition at line 612 of file vpServo.h.
Referenced by testInitialization().
|
protected |
vpServoIteractionMatrixType vpServo::interactionMatrixType |
Type of the interaction matrox (current, mean, desired, user)
Definition at line 594 of file vpServo.h.
Referenced by 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 597 of file vpServo.h.
Referenced by setInteractionMatrixType().
|
protected |
vpMatrix vpServo::J1 |
Task Jacobian .
Definition at line 550 of file vpServo.h.
Referenced by computeProjectionOperators().
vpMatrix vpServo::J1p |
vpMatrix vpServo::L |
vpAdaptiveGain vpServo::lambda |
|
protected |
New Large projection operator (see equation(24) in the paper [Marey:2010]). This projection operator allows performing secondary task even when the main task is full rank.
with
Definition at line 661 of file vpServo.h.
Referenced by computeProjectionOperators().
vpColVector vpServo::q_dot |
vpColVector vpServo::s |
Current state of visual features . This vector is updated after a call of computeError() or computeControlLaw().
vpServoType vpServo::servoType |
Chosen visual servoing control law.
Definition at line 574 of file vpServo.h.
Referenced by testInitialization().
int vpServo::signInteractionMatrix |
vpColVector vpServo::sStar |
Desired state of visual features . This vector is updated after a call of computeError() or computeControlLaw().
|
protected |
|
protected |
Flag to indicate if the task was killed.
Definition at line 636 of file vpServo.h.
Referenced by ~vpServo().
vpColVector vpServo::v |
|
protected |
Projection operators .
Definition at line 641 of file vpServo.h.
Referenced by computeProjectionOperators().