GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/log.hxx Lines: 86 88 97.7 %
Date: 2024-01-23 21:41:47 Branches: 120 225 53.3 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_spatial_log_hxx__
6
#define __pinocchio_spatial_log_hxx__
7
8
namespace pinocchio
9
{
10
  /// \brief Generic evaluation of log3 function
11
  template<typename _Scalar>
12
  struct log3_impl
13
  {
14
    template<typename Matrix3Like, typename Vector3Out>
15
21810
    static void run(const Eigen::MatrixBase<Matrix3Like> & R,
16
                    typename Matrix3Like::Scalar & theta,
17
                    const Eigen::MatrixBase<Vector3Out> & res)
18
    {
19

21810
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
20

21810
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, res, 3, 1);
21
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>
22
23
      typedef typename Matrix3Like::Scalar Scalar;
24
      typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
25
26

21810
      static const Scalar PI_value = PI<Scalar>();
27
28
21810
      Scalar tr = R.trace();
29
21810
      if(tr >= Scalar(3))
30
      {
31
12557
        tr = Scalar(3); // clip value
32
12557
        theta = Scalar(0); // acos((3-1)/2)
33
      }
34
9253
      else if(tr <= Scalar(-1))
35
      {
36
        tr = Scalar(-1); // clip value
37
        theta = PI_value; // acos((-1-1)/2)
38
      }
39
      else
40
9253
        theta = math::acos((tr - Scalar(1))/Scalar(2));
41
21810
      assert(theta == theta && "theta contains some NaN"); // theta != NaN
42
43
21810
      Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res);
44
45
      // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
46
21810
      if(theta >= PI_value - 1e-2)
47
      {
48
        // 1e-2: A low value is not required since the computation is
49
        // using explicit formula. However, the precision of this method
50
        // is the square root of the precision with the antisymmetric
51
        // method (Nominal case).
52
3
        const Scalar cphi = -(tr - Scalar(1))/Scalar(2);
53
3
        const Scalar beta = theta*theta / (Scalar(1) + cphi);
54


3
        const Vector3 tmp((R.diagonal().array() + cphi) * beta);
55



3
        res_(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
56



3
        res_(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
57



3
        res_(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
58
      }
59
      else
60
      {
61
21807
        const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
62
21807
                          ? theta / sin(theta)
63
                          : Scalar(1)) / Scalar(2);
64
21807
        res_(0) = t * (R (2, 1) - R (1, 2));
65
21807
        res_(1) = t * (R (0, 2) - R (2, 0));
66
21807
        res_(2) = t * (R (1, 0) - R (0, 1));
67
      }
68
21810
    }
69
  };
70
71
  /// \brief Generic evaluation of Jlog3 function
72
  template<typename _Scalar>
73
  struct Jlog3_impl
74
  {
75
    template<typename Scalar, typename Vector3Like, typename Matrix3Like>
76
992
    static void run(const Scalar & theta,
77
                    const Eigen::MatrixBase<Vector3Like> & log,
78
                    const Eigen::MatrixBase<Matrix3Like> & Jlog)
79
    {
80


992
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like,  log, 3, 1);
81


992
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3);
82
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>
83
84
992
      Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);
85
86
      Scalar alpha, diag_value;
87

992
      if(theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
88
      {
89
110
        alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
90
110
        diag_value = Scalar(0.5) * (2 - theta*theta / Scalar(6));
91
      }
92
      else
93
      {
94
        // Jlog = alpha I
95
882
        Scalar ct,st; SINCOS(theta,&st,&ct);
96
882
        const Scalar st_1mct = st/(Scalar(1)-ct);
97
98
882
        alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
99
882
        diag_value = Scalar(0.5) * (theta * st_1mct);
100
      }
101
102


992
      Jlog_.noalias() = alpha * log * log.transpose();
103

992
      Jlog_.diagonal().array() += diag_value;
104
105
      // Jlog += r_{\times}/2
106

992
      addSkew(Scalar(0.5) * log, Jlog_);
107
992
    }
108
  };
109
110
  /// \brief Generic evaluation of log6 function
111
  template<typename _Scalar>
112
  struct log6_impl
113
  {
114
    template<typename Scalar, int Options, typename MotionDerived>
115
19397
    static void run(const SE3Tpl<Scalar,Options> & M,
116
                    MotionDense<MotionDerived> & mout)
117
    {
118
      typedef SE3Tpl<Scalar,Options> SE3;
119
      typedef typename SE3::Vector3 Vector3;
120
121
19397
      typename SE3::ConstAngularRef R = M.rotation();
122
19397
      typename SE3::ConstLinearRef p = M.translation();
123
124
      Scalar t;
125
19397
      Vector3 w(log3(R,t)); // t in [0,π]
126
19397
      const Scalar t2 = t*t;
127
      Scalar alpha, beta;
128
129

19397
      if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
130
      {
131
15811
        alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
132
15811
        beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
133
      }
134
      else
135
      {
136
3586
        Scalar st,ct; SINCOS(t,&st,&ct);
137
3586
        alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
138
3586
        beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
139
      }
140
141





19397
      mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w;
142

19397
      mout.angular() = w;
143
19397
    }
144
  };
145
146
  template<typename _Scalar>
147
  struct Jlog6_impl
148
  {
149
    template<typename Scalar, int Options, typename Matrix6Like>
150
315
    static void run(const SE3Tpl<Scalar, Options> & M,
151
                    const Eigen::MatrixBase<Matrix6Like> & Jlog)
152
    {
153


315
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6);
154
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>
155
156
      typedef SE3Tpl<Scalar,Options> SE3;
157
      typedef typename SE3::Vector3 Vector3;
158
315
      Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);
159
160
315
      typename SE3::ConstAngularRef R = M.rotation();
161
315
      typename SE3::ConstLinearRef p = M.translation();
162
163
      Scalar t;
164
315
      Vector3 w(log3(R,t));
165
166
      // value is decomposed as following:
167
      // value = [ A, B;
168
      //           C, D ]
169
      typedef Eigen::Block<Matrix6Like,3,3> Block33;
170
315
      Block33 A = value.template topLeftCorner<3,3>();
171
315
      Block33 B = value.template topRightCorner<3,3>();
172
315
      Block33 C = value.template bottomLeftCorner<3,3>();
173
315
      Block33 D = value.template bottomRightCorner<3,3>();
174
175
315
      Jlog3(t, w, A);
176
315
      D = A;
177
178
315
      const Scalar t2 = t*t;
179
      Scalar beta, beta_dot_over_theta;
180

315
      if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
181
      {
182
72
        beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
183
72
        beta_dot_over_theta = Scalar(1)/Scalar(360);
184
      }
185
      else
186
      {
187
243
        const Scalar tinv = Scalar(1)/t,
188
243
                     t2inv = tinv*tinv;
189
243
        Scalar st,ct; SINCOS (t, &st, &ct);
190
243
        const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
191
192
243
        beta = t2inv - st*tinv*inv_2_2ct;
193
243
        beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
194
243
          (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
195
      }
196
197
315
      Scalar wTp = w.dot(p);
198
199


315
      Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
200
      // C can be treated as a temporary variable
201


315
      C.noalias() = v3_tmp * w.transpose();
202


315
      C.noalias() += beta * w * p.transpose();
203

315
      C.diagonal().array() += wTp * beta;
204

315
      addSkew(Scalar(.5)*p,C);
205
206

315
      B.noalias() = C * A;
207
315
      C.setZero();
208
315
    }
209
  };
210
211
} // namespace pinocchio
212
213
#endif // ifndef __pinocchio_spatial_log_hxx__