GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/feature-pose.hh Lines: 0 2 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 0 - %

Line Branch Exec Source
1
/*
2
 * Copyright 2019,
3
 * Joseph Mirabel
4
 *
5
 * LAAS-CNRS
6
 *
7
 */
8
9
#ifndef __SOT_FEATURE_POSE_HH__
10
#define __SOT_FEATURE_POSE_HH__
11
12
/* --------------------------------------------------------------------- */
13
/* --- INCLUDE --------------------------------------------------------- */
14
/* --------------------------------------------------------------------- */
15
16
/* SOT */
17
#include <sot/core/config.hh>
18
#include <sot/core/feature-abstract.hh>
19
#include <sot/core/matrix-geometry.hh>
20
21
/* --------------------------------------------------------------------- */
22
/* --- CLASS ----------------------------------------------------------- */
23
/* --------------------------------------------------------------------- */
24
25
namespace dynamicgraph {
26
namespace sot {
27
28
/// Enum used to specify what difference operation is used in FeaturePose.
29
enum Representation_t { SE3Representation, R3xSO3Representation };
30
31
/*!
32
  \brief Feature that controls the relative (or absolute) pose between
33
  two frames A (or world) and B.
34
35
  @tparam representation specify the difference operation to use. This changes
36
          - the descent direction,
37
          - the meaning of the mask.
38
          With R3xSO3Representation, the mask is relative to <em>Frame A</em>.
39
          If this feature is alone in a SOT, the relative motion of <em>Frame
40
  B</em> wrt <em>Frame A</em> will be a line. This is what most people want.
41
42
  Notations:
43
  - The frames are refered to with \c fa and \c fb.
44
  - Each frame is attached to a joint, which are refered to with \c ja and \c
45
  jb.
46
  - the difference operator is defined differently depending on the
47
  representation:
48
    - R3xSO3Representation:
49
        \f[ \begin{array}{ccccc}
50
        \ominus & : & (R^3\times SO(3))^2                 & \to & R^3 \times
51
  \mathfrak{so}(3) \\
52
                &   & (a = (t_a,R_a),b = (t_b,R_b))       & \mapsto & b \ominus
53
  a = (t_b - t_a, \log(R_a^{-1} R_b) \\ \end{array} \f]
54
    - SE3Representation:
55
        \f[ \begin{array}{ccccc}
56
        \ominus & : & SE(3)^2 & \to & \mathfrak{se}(3) \\
57
                &   & a, b    & \mapsto & b \ominus a = \log(a^{-1} b) \\
58
        \end{array} \f]
59
60
*/
61
template <Representation_t representation = R3xSO3Representation>
62
class SOT_CORE_DLLAPI FeaturePose : public FeatureAbstract {
63
 public:
64
  static const std::string CLASS_NAME;
65
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
66
67
 public:
68
  /*! \name Input signals
69
    @{
70
  */
71
  /// Input pose of <em>Joint A</em> wrt to world frame.
72
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> oMja;
73
  /// Input pose of <em>Frame A</em> wrt to <em>Joint A</em>.
74
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> jaMfa;
75
  /// Input pose of <em>Joint B</em> wrt to world frame.
76
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> oMjb;
77
  /// Input pose of <em>Frame B</em> wrt to <em>Joint B</em>.
78
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> jbMfb;
79
  /// Jacobian of the input <em>Joint A</em>, expressed in <em>Joint A</em>
80
  dynamicgraph::SignalPtr<Matrix, int> jaJja;
81
  /// Jacobian of the input <em>Joint B</em>, expressed in <em>Joint B</em>
82
  dynamicgraph::SignalPtr<Matrix, int> jbJjb;
83
84
  /// The desired pose of <em>Frame B</em> wrt to <em>Frame A</em>.
85
  dynamicgraph::SignalPtr<MatrixHomogeneous, int> faMfbDes;
86
  /// The desired velocity of <em>Frame B</em> wrt to <em>Frame A</em>. The
87
  /// value is expressed in <em>Frame A</em>.
88
  dynamicgraph::SignalPtr<Vector, int> faNufafbDes;
89
  /*! @} */
90
91
  /*! \name Output signals
92
    @{
93
  */
94
  /// Pose of <em>Frame B</em> wrt to <em>Frame A</em>.
95
  SignalTimeDependent<MatrixHomogeneous, int> faMfb;
96
97
  /// Pose of <em>Frame B</em> wrt to <em>Frame A</em>.
98
  /// It is expressed as a translation followed by a quaternion.
99
  SignalTimeDependent<Vector7, int> q_faMfb;
100
101
  /// Desired pose of <em>Frame B</em> wrt to <em>Frame A</em>.
102
  /// It is expressed as a translation followed by a quaternion.
103
  SignalTimeDependent<Vector7, int> q_faMfbDes;
104
  /*! @} */
105
106
  using FeatureAbstract::errorSOUT;
107
  using FeatureAbstract::jacobianSOUT;
108
  using FeatureAbstract::selectionSIN;
109
110
  /*! \name Dealing with the reference value to be reach with this feature.
111
    @{  */
112
  DECLARE_NO_REFERENCE();
113
  /*! @} */
114
115
 public:
116
  FeaturePose(const std::string &name);
117
  virtual ~FeaturePose(void);
118
119
  virtual unsigned int &getDimension(unsigned int &dim, int time);
120
121
  /// Computes \f$ {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \f$
122
  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
123
                                             int time);
124
  /// Computes \f$ \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \f$.
125
  /// There are two different cases, depending on the representation:
126
  /// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [
127
  /// {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$
128
  /// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see
129
  /// pinocchio::SE3Base<Scalar,Options>::toActionMatrix)
130
  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
131
                                                int time);
132
  /// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}}
133
  /// {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$. There are two
134
  /// different cases, depending on the representation:
135
  /// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} &
136
  /// 0_3 \\ 0_3 & I_3 \end{array} \right) \f$
137
  /// - SE3Representation: \f$ Y = I_6 \f$
138
  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
139
                                                int time);
140
141
  virtual void display(std::ostream &os) const;
142
143
 public:
144
  void servoCurrentPosition(const int &time);
145
146
 private:
147
  MatrixHomogeneous &computefaMfb(MatrixHomogeneous &res, int time);
148
  Vector7 &computeQfaMfb(Vector7 &res, int time);
149
  Vector7 &computeQfaMfbDes(Vector7 &res, int time);
150
151
  /// \todo Intermediate variables for internal computations
152
};
153
154
template <typename T>
155
Vector6d convertVelocity(const MatrixHomogeneous &M,
156
                         const MatrixHomogeneous &Mdes,
157
                         const Vector &faNufafbDes);
158
159
template <>
160
const std::string FeaturePose<SE3Representation>::CLASS_NAME;
161
template <>
162
const std::string FeaturePose<R3xSO3Representation>::CLASS_NAME;
163
164
#if __cplusplus >= 201103L
165
extern template class FeaturePose<SE3Representation>;
166
extern template class FeaturePose<R3xSO3Representation>;
167
#endif
168
169
typedef FeaturePose<R3xSO3Representation> FeaturePose_t;
170
typedef FeaturePose<SE3Representation> FeaturePoseSE3_t;
171
172
} /* namespace sot */
173
} /* namespace dynamicgraph */
174
175
#endif  // #ifndef __SOT_FEATURE_POSE_HH__
176
177
/*
178
 * Local variables:
179
 * c-basic-offset: 2
180
 * End:
181
 */