math.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Fabian Reister ( fabian dot reister at kit dot edu )
17  * @date 2021
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <array>
25 
26 #include <ceres/ceres.h>
27 
29 {
30 
31  auto
32  periodicDiff(const auto a, const auto b)
33  {
34  // angle to corresponding vector
35  const auto a1 = ceres::cos(a);
36  const auto a2 = ceres::sin(a);
37 
38  // angle to corresponding vector
39  const auto b1 = ceres::cos(b);
40  const auto b2 = ceres::sin(b);
41 
42  // cosine similarity
43  const auto cosSim = a1 * b1 + a2 * b2;
44 
45  const auto angleDiff = ceres::acos(cosSim);
46 
47  return angleDiff;
48  }
49 
50  std::array<double, 2> inline periodicDiffJacobian(const auto a, const auto b)
51  {
52  const auto derivative = [&a, &b]() -> double
53  {
54  if (a == 0. and b == 0.)
55  {
56  return 0.;
57  }
58 
59 
60  const double denomSquared =
61  1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b), 2);
62 
63  // prevent division by 0
64  if (denomSquared < 0.001)
65  {
66  return 0.;
67  }
68 
69  return (ceres::sin(a) * ceres::cos(b) - ceres::cos(a) * ceres::sin(b)) /
70  (ceres::sqrt(
71  1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b),
72  2)));
73  }();
74 
75 
76  std::array<double, 2> J;
77 
78  // the partial derivative for a and b is similar
79  J.at(0) = derivative;
80  J.at(1) = -derivative;
81 
82  return J;
83  }
84 } // namespace armarx::navigation::global_planning::optimization
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::navigation::global_planning::optimization
This file is part of ArmarX.
Definition: aron_conversions.h:12
armarx::navigation::global_planning::optimization::periodicDiffJacobian
std::array< double, 2 > periodicDiffJacobian(const auto a, const auto b)
Definition: math.h:50
armarx::navigation::global_planning::optimization::periodicDiff
auto periodicDiff(const auto a, const auto b)
Definition: math.h:32
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45