Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpServo.h
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
36#ifndef vpServo_H
37#define vpServo_H
38
44#include <list>
45
46#include <visp3/core/vpMatrix.h>
47#include <visp3/core/vpVelocityTwistMatrix.h>
48#include <visp3/visual_features/vpBasicFeature.h>
49#include <visp3/vs/vpAdaptiveGain.h>
50#include <visp3/vs/vpServoException.h>
51
141class VISP_EXPORT vpServo
142{
143 /*
144 Choice of the visual servoing control law
145 */
146public:
147 typedef enum
148 {
149 NONE,
151 EYEINHAND_CAMERA,
155 EYEINHAND_L_cVe_eJe,
160 EYETOHAND_L_cVe_eJe,
165 EYETOHAND_L_cVf_fVe_eJe,
170 EYETOHAND_L_cVf_fJe
175 } vpServoType;
176
177 typedef enum
178 {
179 CURRENT,
183 DESIRED,
187 MEAN,
191 USER_DEFINED
194 } vpServoIteractionMatrixType;
195 typedef enum
196 {
197 TRANSPOSE,
199 PSEUDO_INVERSE
201 } vpServoInversionType;
202
214
215 // private:
216 //#ifndef DOXYGEN_SHOULD_SKIP_THIS
217 // vpServo(const vpServo &)
218 // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
219 // servoType(vpServo::NONE),
220 // rankJ1(0), featureList(), desiredFeatureList(),
221 // featureSelectionList(), lambda(), signInteractionMatrix(1),
222 // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
223 // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
224 // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
225 // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
226 // taskWasKilled(false), forceInteractionMatrixComputation(false),
227 // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
228 // {
229 // throw vpException(vpException::functionNotImplementedError, "Not
230 // implemented!");
231 // }
232 // vpServo &operator=(const vpServo &){
233 // throw vpException(vpException::functionNotImplementedError, "Not
234 // implemented!"); return *this;
235 // }
236 //#endif
237
238public:
239 // default constructor
240 vpServo();
241 // constructor with Choice of the visual servoing control law
242 explicit vpServo(vpServoType servoType);
243 // destructor
244 virtual ~vpServo();
245
246 // create a new ste of two visual features
247 void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select = vpBasicFeature::FEATURE_ALL);
248 // create a new ste of two visual features
249 void addFeature(vpBasicFeature &s, unsigned int select = vpBasicFeature::FEATURE_ALL);
250
251 // compute the desired control law
252 vpColVector computeControlLaw();
253 // compute the desired control law
254 vpColVector computeControlLaw(double t);
255 vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
256
257 // compute the error between the current set of visual features and
258 // the desired set of visual features
259 vpColVector computeError();
260 // compute the interaction matrix related to the set of visual features
261 vpMatrix computeInteractionMatrix();
262
263 // Return the task dimension.
264 unsigned int getDimension() const;
276 inline vpColVector getError() const { return error; }
277
291 inline vpMatrix getInteractionMatrix() const { return L; }
292
293 vpMatrix getI_WpW() const;
297 inline vpServoType getServoType() const { return servoType; }
298
299 vpMatrix getLargeP() const;
300
301 vpMatrix getTaskJacobian() const;
302 vpMatrix getTaskJacobianPseudoInverse() const;
303 unsigned int getTaskRank() const;
304
310 inline vpColVector getTaskSingularValues() const { return sv; }
311
312 vpMatrix getWpW() const;
313
318 vpVelocityTwistMatrix get_cVe() const { return cVe; }
324 vpVelocityTwistMatrix get_cVf() const { return cVf; }
330 vpVelocityTwistMatrix get_fVe() const { return fVe; }
334 vpMatrix get_eJe() const { return eJe; }
339 vpMatrix get_fJe() const { return fJe; }
340
341 double getPseudoInverseThreshold() const;
342
343 // destruction (memory deallocation if required)
344 void kill();
345
346 void print(const vpServo::vpServoPrintType display_level = ALL, std::ostream &os = std::cout);
347
348 // Add a secondary task.
349 vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator = false);
350 // Add a secondary task.
351 vpColVector secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
352 const bool &useLargeProjectionOperator = false);
353 // Add a secondary task to avoid the joint limit.
354 vpColVector secondaryTaskJointLimitAvoidance(const vpColVector &q, const vpColVector &dq, const vpColVector &jointMin,
355 const vpColVector &jointMax, const double &rho = 0.1,
356 const double &rho1 = 0.3, const double &lambda_tune = 0.7);
357
358 void setCameraDoF(const vpColVector &dof);
359
376 void setForceInteractionMatrixComputation(bool force_computation)
377 {
378 this->forceInteractionMatrixComputation = force_computation;
379 }
380
390 void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
391 const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
392
403 void setLambda(double c) { lambda.initFromConstant(c); }
404
419 void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
420 {
421 lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
422 }
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_,
541 const vpColVector &error_, vpMatrix &P_) const;
542
543public:
555
564
569
574
577
579 unsigned int rankJ1;
580
582 std::list<vpBasicFeature *> featureList;
584 std::list<vpBasicFeature *> desiredFeatureList;
587 std::list<unsigned int> featureSelectionList;
588
591
600
601protected:
602 /*
603 Twist transformation matrix
604 */
605
615
616 /*
617 Jacobians
618 */
619
626
627 /*
628 Task building
629 */
630
636 unsigned int dim_task;
641
666
669
670 double mu;
671
673
676
680
682
684};
685
686#endif
Adaptive gain computation.
class that defines what is a visual feature
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpColVector q_dot
Articular velocity.
Definition vpServo.h:571
unsigned int rankJ1
Rank of the task Jacobian.
Definition vpServo.h:579
vpMatrix eJe
Jacobian expressed in the end-effector frame.
Definition vpServo.h:621
int signInteractionMatrix
Definition vpServo.h:594
vpMatrix WpW
Projection operators .
Definition vpServo.h:645
vpVelocityTwistMatrix cVf
Twist transformation matrix between Rf and Rc.
Definition vpServo.h:610
vpMatrix J1
Task Jacobian .
Definition vpServo.h:552
vpServoType getServoType() const
Definition vpServo.h:297
vpMatrix getInteractionMatrix() const
Definition vpServo.h:291
void setMu(double mu_)
Definition vpServo.h:440
bool init_cVe
Definition vpServo.h:608
bool errorComputed
true if the error has been computed.
Definition vpServo.h:632
vpMatrix fJe
Jacobian expressed in the robot reference frame.
Definition vpServo.h:624
vpServoType
Definition vpServo.h:148
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:611
double mu
Definition vpServo.h:670
vpVelocityTwistMatrix get_cVe() const
Definition vpServo.h:318
vpMatrix get_fJe() const
Definition vpServo.h:339
vpVelocityTwistMatrix cVe
Twist transformation matrix between Re and Rc.
Definition vpServo.h:607
bool init_fJe
Definition vpServo.h:625
vpMatrix P
Definition vpServo.h:665
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition vpServo.h:448
vpColVector e1
Primary task .
Definition vpServo.h:566
vpColVector e1_initial
Definition vpServo.h:672
vpVelocityTwistMatrix get_fVe() const
Definition vpServo.h:330
bool forceInteractionMatrixComputation
Force the interaction matrix computation even if it is already done.
Definition vpServo.h:640
void set_fVe(const vpHomogeneousMatrix &fMe)
Definition vpServo.h:497
void setLambda(double c)
Definition vpServo.h:403
void setForceInteractionMatrixComputation(bool force_computation)
Definition vpServo.h:376
vpMatrix cJc
Definition vpServo.h:679
vpVelocityTwistMatrix fVe
Twist transformation matrix between Re and Rf.
Definition vpServo.h:613
void set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
bool taskWasKilled
Flag to indicate if the task was killed.
Definition vpServo.h:638
std::list< vpBasicFeature * > featureList
List of current visual features .
Definition vpServo.h:582
vpColVector error
Definition vpServo.h:550
bool iscJcIdentity
Boolean to know if cJc is identity (for fast computation)
Definition vpServo.h:675
vpMatrix I_WpW
Projection operators .
Definition vpServo.h:647
void set_fVe(const vpVelocityTwistMatrix &fVe_)
Definition vpServo.h:487
vpColVector v
Camera velocity.
Definition vpServo.h:573
vpColVector sStar
Definition vpServo.h:563
vpMatrix J1p
Pseudo inverse of the task Jacobian.
Definition vpServo.h:554
vpMatrix get_eJe() const
Definition vpServo.h:334
vpColVector s
Definition vpServo.h:559
void setLambda(const vpAdaptiveGain &l)
Definition vpServo.h:433
vpMatrix I
Identity matrix.
Definition vpServo.h:643
void set_fJe(const vpMatrix &fJe_)
Definition vpServo.h:515
std::list< vpBasicFeature * > desiredFeatureList
List of desired visual features .
Definition vpServo.h:584
vpColVector getTaskSingularValues() const
Definition vpServo.h:310
bool m_first_iteration
True until first call of computeControlLaw() is achieved.
Definition vpServo.h:681
vpMatrix L
Interaction matrix.
Definition vpServo.h:545
vpServoType servoType
Chosen visual servoing control law.
Definition vpServo.h:576
vpServoIteractionMatrixType interactionMatrixType
Type of the interaction matrix (current, mean, desired, user)
Definition vpServo.h:596
double m_pseudo_inverse_threshold
Threshold used in the pseudo inverse.
Definition vpServo.h:683
vpColVector getError() const
Definition vpServo.h:276
void setLambda(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
Definition vpServo.h:419
vpVelocityTwistMatrix get_cVf() const
Definition vpServo.h:324
vpServoInversionType
Definition vpServo.h:196
std::list< unsigned int > featureSelectionList
Definition vpServo.h:587
vpColVector e
Task .
Definition vpServo.h:568
bool init_eJe
Definition vpServo.h:622
vpServoPrintType
Definition vpServo.h:204
@ CONTROLLER
Definition vpServo.h:206
@ ERROR_VECTOR
Definition vpServo.h:207
@ FEATURE_CURRENT
Definition vpServo.h:208
@ FEATURE_DESIRED
Definition vpServo.h:209
@ INTERACTION_MATRIX
Definition vpServo.h:211
vpColVector sv
Singular values from the pseudo inverse.
Definition vpServo.h:668
vpServoIteractionMatrixType
Definition vpServo.h:178
bool interactionMatrixComputed
true if the interaction matrix has been computed.
Definition vpServo.h:634
void set_cVe(const vpHomogeneousMatrix &cMe)
Definition vpServo.h:457
bool init_fVe
Definition vpServo.h:614
unsigned int dim_task
Dimension of the task updated during computeControlLaw().
Definition vpServo.h:636
vpServoInversionType inversionType
Definition vpServo.h:599
vpAdaptiveGain lambda
Gain used in the control law.
Definition vpServo.h:590