Visual Servoing Platform version 3.5.0
vpServo.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Visual servoing control law.
33 *
34 * Authors:
35 * Eric Marchand
36 * Nicolas Mansard
37 * Fabien Spindler
38 *
39 *****************************************************************************/
40
41#ifndef vpServo_H
42#define vpServo_H
43
49#include <list>
50
51#include <visp3/core/vpMatrix.h>
52#include <visp3/core/vpVelocityTwistMatrix.h>
53#include <visp3/visual_features/vpBasicFeature.h>
54#include <visp3/vs/vpAdaptiveGain.h>
55#include <visp3/vs/vpServoException.h>
56
146class VISP_EXPORT vpServo
147{
148 /*
149 Choice of the visual servoing control law
150 */
151public:
152 typedef enum {
153 NONE,
155 EYEINHAND_CAMERA,
159 EYEINHAND_L_cVe_eJe,
164 EYETOHAND_L_cVe_eJe,
169 EYETOHAND_L_cVf_fVe_eJe,
174 EYETOHAND_L_cVf_fJe
179 } vpServoType;
180
181 typedef enum {
182 CURRENT,
186 DESIRED,
190 MEAN,
194 USER_DEFINED
197 } vpServoIteractionMatrixType;
198
199 typedef enum {
200 TRANSPOSE,
202 PSEUDO_INVERSE
204 } vpServoInversionType;
205
206 typedef enum {
214 MINIMUM
215 } vpServoPrintType;
216
217 // private:
218 //#ifndef DOXYGEN_SHOULD_SKIP_THIS
219 // vpServo(const vpServo &)
220 // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
221 // servoType(vpServo::NONE),
222 // rankJ1(0), featureList(), desiredFeatureList(),
223 // featureSelectionList(), lambda(), signInteractionMatrix(1),
224 // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
225 // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
226 // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
227 // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
228 // taskWasKilled(false), forceInteractionMatrixComputation(false),
229 // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
230 // {
231 // throw vpException(vpException::functionNotImplementedError, "Not
232 // implemented!");
233 // }
234 // vpServo &operator=(const vpServo &){
235 // throw vpException(vpException::functionNotImplementedError, "Not
236 // implemented!"); return *this;
237 // }
238 //#endif
239
240public:
241 // default constructor
242 vpServo();
243 // constructor with Choice of the visual servoing control law
244 explicit vpServo(vpServoType servoType);
245 // destructor
246 virtual ~vpServo();
247
248 // create a new ste of two visual features
249 void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
250 // create a new ste of two visual features
251 void addFeature(vpBasicFeature &s, unsigned int select = vpBasicFeature::FEATURE_ALL);
252
253 // compute the desired control law
254 vpColVector computeControlLaw();
255 // compute the desired control law
256 vpColVector computeControlLaw(double t);
257 vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
258
259 // compute the error between the current set of visual features and
260 // the desired set of visual features
261 vpColVector computeError();
262 // compute the interaction matrix related to the set of visual features
263 vpMatrix computeInteractionMatrix();
264
265 // Return the task dimension.
266 unsigned int getDimension() const;
278 inline vpColVector getError() const { return error; }
279
280 /*
281 Return the interaction matrix \f$L\f$ used to compute the task jacobian
282 \f$J_1\f$. The interaction matrix is updated after a call to
283 computeInteractionMatrix() or computeControlLaw().
284
285 \code
286 vpServo task;
287 ...
288 vpColVector v = task.computeControlLaw(); // Compute the velocity
289 corresponding to the visual servoing vpMatrix L =
290 task.getInteractionMatrix(); // Get the interaction matrix used to compute v
291 \endcode
292 \sa getTaskJacobian()
293 */
294 inline vpMatrix getInteractionMatrix() const { return L; }
295
296 vpMatrix getI_WpW() const;
300 inline vpServoType getServoType() const { return servoType; }
301
302 vpMatrix getLargeP() const;
303
304 vpMatrix getTaskJacobian() const;
305 vpMatrix getTaskJacobianPseudoInverse() const;
306 unsigned int getTaskRank() const;
307
313 inline vpColVector getTaskSingularValues() const { return sv; }
314
315 vpMatrix getWpW() const;
316
321 vpVelocityTwistMatrix get_cVe() const { return cVe; }
327 vpVelocityTwistMatrix get_cVf() const { return cVf; }
333 vpVelocityTwistMatrix get_fVe() const { return fVe; }
337 vpMatrix get_eJe() const { return eJe; }
342 vpMatrix get_fJe() const { return fJe; }
343
344 double getPseudoInverseThreshold() const;
345
346 // destruction (memory deallocation if required)
347 void kill();
348
349 void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
350
351 // Add a secondary task.
352 vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
353 // Add a secondary task.
354 vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
355 const bool &useLargeProjectionOperator = false);
356 // Add a secondary task to avoid the joint limit.
357 vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin,
358 const vpColVector &jointMax, const double &rho = 0.1,
359 const double &rho1 = 0.3, const double &lambda_tune = 0.7);
360
361 void setCameraDoF(const vpColVector &dof);
362
379 void setForceInteractionMatrixComputation(bool force_computation)
380 {
381 this->forceInteractionMatrixComputation = force_computation;
382 }
383
391 void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
392 const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
393
404 void setLambda(double c) { lambda.initFromConstant(c); }
405
421 void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
422 {
423 lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
424 }
433 void setLambda(const vpAdaptiveGain &l) { lambda = l; }
440 void setMu(double mu_) { this->mu = mu_; }
441 // Choice of the visual servoing control law
442 void setServo(const vpServoType &servo_type);
443
449 {
450 this->cVe = cVe_;
451 init_cVe = true;
452 }
458 {
459 cVe.buildFrom(cMe);
460 init_cVe = true;
461 }
468 {
469 this->cVf = cVf_;
470 init_cVf = true;
471 }
478 {
479 cVf.buildFrom(cMf);
480 init_cVf = true;
481 }
488 {
489 this->fVe = fVe_;
490 init_fVe = true;
491 }
498 {
499 fVe.buildFrom(fMe);
500 init_fVe = true;
501 }
502
506 void set_eJe(const vpMatrix &eJe_)
507 {
508 this->eJe = eJe_;
509 init_eJe = true;
510 }
515 void set_fJe(const vpMatrix &fJe_)
516 {
517 this->fJe = fJe_;
518 init_fJe = true;
519 }
520
521 void setPseudoInverseThreshold(double pseudo_inverse_threshold);
522
527 bool testInitialization();
531 bool testUpdated();
532
533protected:
535 void init();
536
540 void computeProjectionOperators(const vpMatrix &J1_, const vpMatrix &I_, const vpMatrix &I_WpW_, const vpColVector &error_, vpMatrix &P_) const;
541
542public:
554
563
568
573
576
578 unsigned int rankJ1;
579
581 std::list<vpBasicFeature *> featureList;
583 std::list<vpBasicFeature *> desiredFeatureList;
586 std::list<unsigned int> featureSelectionList;
587
590
599
600protected:
601 /*
602 Twist transformation matrix
603 */
604
614
615 /*
616 Jacobians
617 */
618
625
626 /*
627 Task building
628 */
629
635 unsigned int dim_task;
640
665
668
669 double mu;
670
672
675
679
681
683};
684
685#endif
Adaptive gain computation.
class that defines what is a visual feature
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector q_dot
Articular velocity.
Definition: vpServo.h:570
unsigned int rankJ1
Rank of the task Jacobian.
Definition: vpServo.h:578
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition: vpServo.h:620
int signInteractionMatrix
Definition: vpServo.h:593
vpMatrix WpW
Projection operators .
Definition: vpServo.h:644
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition: vpServo.h:609
vpMatrix J1
Task Jacobian .
Definition: vpServo.h:551
vpServoType getServoType() const
Definition: vpServo.h:300
vpMatrix getInteractionMatrix() const
Definition: vpServo.h:294
void setMu(double mu_)
Definition: vpServo.h:440
bool init_cVe
Definition: vpServo.h:607
bool errorComputed
true if the error has been computed.
Definition: vpServo.h:631
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition: vpServo.h:623
vpServoType
Definition: vpServo.h:152
void set_cVf(const vpVelocityTwistMatrix &cVf_)
Definition: vpServo.h:467
void set_cVf(const vpHomogeneousMatrix &cMf)
Definition: vpServo.h:477
bool init_cVf
Definition: vpServo.h:610
double mu
Definition: vpServo.h:669
vpVelocityTwistMatrix get_cVe() const
Definition: vpServo.h:321
vpMatrix get_fJe() const
Definition: vpServo.h:342
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition: vpServo.h:606
bool init_fJe
Definition: vpServo.h:624
vpMatrix P
Definition: vpServo.h:664
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
vpColVector e1
Primary task .
Definition: vpServo.h:565
vpColVector e1_initial
Definition: vpServo.h:671
vpVelocityTwistMatrix get_fVe() const
Definition: vpServo.h:333
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition: vpServo.h:639
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition: vpServo.h:497
void setLambda(double c)
Definition: vpServo.h:404
void setForceInteractionMatrixComputation(bool force_computation)
Definition: vpServo.h:379
vpMatrix cJc
Definition: vpServo.h:678
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition: vpServo.h:612
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
bool taskWasKilled
Flag to indicate if the task was killed.
Definition: vpServo.h:637
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition: vpServo.h:581
vpColVector error
Definition: vpServo.h:549
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition: vpServo.h:674
vpMatrix I_WpW
Projection operators .
Definition: vpServo.h:646
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition: vpServo.h:487
vpColVector v
Camera velocity.
Definition: vpServo.h:572
vpColVector sStar
Definition: vpServo.h:562
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition: vpServo.h:553
vpMatrix get_eJe() const
Definition: vpServo.h:337
vpColVector s
Definition: vpServo.h:558
void setLambda(const vpAdaptiveGain &l)
Definition: vpServo.h:433
vpMatrix I
Identity matrix.
Definition: vpServo.h:642
void set_fJe(const vpMatrix &fJe_)
Definition: vpServo.h:515
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition: vpServo.h:583
vpColVector getTaskSingularValues() const
Definition: vpServo.h:313
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition: vpServo.h:680
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
vpServoType servoType
Chosen visual servoing control law.
Definition: vpServo.h:575
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrox (current, mean, desired, user)
Definition: vpServo.h:595
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition: vpServo.h:682
vpColVector getError() const
Definition: vpServo.h:278
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition: vpServo.h:421
vpVelocityTwistMatrix get_cVf() const
Definition: vpServo.h:327
vpServoInversionType
Definition: vpServo.h:199
std::list< unsigned int > featureSelectionList
Definition: vpServo.h:586
vpColVector e
Task .
Definition: vpServo.h:567
bool init_eJe
Definition: vpServo.h:621
vpServoPrintType
Definition: vpServo.h:206
@ ALL
Definition: vpServo.h:207
@ CONTROLLER
Definition: vpServo.h:208
@ ERROR_VECTOR
Definition: vpServo.h:209
@ GAIN
Definition: vpServo.h:212
@ FEATURE_CURRENT
Definition: vpServo.h:210
@ FEATURE_DESIRED
Definition: vpServo.h:211
@ INTERACTION_MATRIX
Definition: vpServo.h:213
vpColVector sv
Singular values from the pseudo inverse.
Definition: vpServo.h:667
vpServoIteractionMatrixType
Definition: vpServo.h:181
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition: vpServo.h:633
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition: vpServo.h:457
bool init_fVe
Definition: vpServo.h:613
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition: vpServo.h:635
vpServoInversionType inversionType
Definition: vpServo.h:598
vpAdaptiveGain lambda
Gain used in the control law.
Definition: vpServo.h:589