CtrlUtil.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http{//www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2019
22  * GNU General Public License
23  */
24 //pragma once
25 #include <cmath>
26 #include <iostream>
27
29
31 {
32  double
33  s(double t, double s0, double v0, double a0, double j)
34  {
35  return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
36  }
37
38  double
39  v(double t, double v0, double a0, double j)
40  {
41  return v0 + a0 * t + 0.5 * j * t * t;
42  }
43
44  double
45  a(double t, double a0, double j)
46  {
47  return a0 + j * t;
48  }
49
50  double
51  t_to_v(double v, double a, double j)
52  {
53  // time to accelerate to v with jerk j starting at acceleration a0
54  //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
55  // 0 = (-v + a0*t) /(0.5*j) + t*t
56  // 0 = -2v/j + 2a0t/j + t*t
57  // --> p = 2a0/j; q = -2v/j
58  // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
59  double tmp = a / j;
60  return -a / j + std::sqrt(tmp * tmp + 2 * v / j);
61  }
62
63  double
64  t_to_v_with_wedge_acc(double v, double a0, double j)
65  {
66  // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
67  return 2 * t_to_v(v / 2.0, a0, j);
68  }
69
70  struct BrakingData
71  {
72  double s_total, v1, v2, v3, dt1, dt2, dt3;
73  };
74
75  double
76  brakingDistance(double v0, double a0, double j)
77  {
78  auto signV = math::MathUtils::Sign(v0);
79  auto t = t_to_v(v0, -signV * a0, signV * j);
80  return s(t, 0, v0, a0, -signV * j);
81  }
82
83  BrakingData
84  brakingDistance(double goal,
85  double s0,
86  double v0,
87  double a0,
88  double v_max,
89  double a_max,
90  double j,
91  double dec_max)
92  {
93  double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
94  dec_max *= -d;
95  // std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
96  double dt1 = std::abs((dec_max - a0) / (-j * d));
97  // double dt1 = t_to_v(v_max-v0, a0, j);
98  // double a1 = a(dt1, a0, -d * j);
99  double v1 = v(dt1, v0, a0, -d * j);
100  double acc_duration = std::abs(dec_max) / j;
101  double v2 =
102  v(acc_duration, 0, 0, d * j); // # inverse of time to accelerate from 0 to max v
103  v2 = math::MathUtils::LimitTo(v2, v_max);
104  // v2 = v1 + dv2
105  double dt2 = d * ((v1 - v2) / dec_max);
106  // double a2 = a(dt2, a1, 0);
107
108  double dt3 = t_to_v(std::abs(v2), 0, j);
109  double s1 = s(dt1, 0, v0, a0, d * j);
110  double s2 = s(dt2, 0, v1, dec_max, 0);
111  double s3 = s(dt3, 0, v2, dec_max, d * j);
112  double v3 = v(dt3, v2, dec_max, d * j);
113  double s_total = s1 + s2 + s3;
114
115  if (dt2 < 0) // # we dont have a phase with a=a_max and need to reduce the a_max
116  {
117  double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
118  double delta_a = a(excess_time / 2, 0, d * j);
119  a_max -= d * delta_a;
120  dec_max = std::abs(dec_max) - std::abs(delta_a);
121  dec_max *= -d;
122  return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
123  }
124
125  return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
126  // return (s_total, v1, v2, v3, dt1, dt2, dt3);
127  }
128
130  {
131  double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
132  };
133
134  bool
135  isPossibleToBrake(double v0, double a0, double j)
136  {
137  return j * v0 - a0 * a0 / 2 > 0.0f;
138  }
139
140  double
141  jerk(double t, double s0, double v0, double a0)
142  {
143  return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
144  }
145
146  std::tuple<double, double, double>
147  calcAccAndJerk(double s0, double v0)
148  {
149  s0 *= -1;
150  double t = -(3 * s0) / v0;
151  double a0 = -(2 * v0) / t;
152  double j = 2 * v0 / (t * t);
153  return std::make_tuple(t, a0, j);
154  }
155
156  WedgeBrakingData
157  braking_distance_with_wedge(double v0, double a0, double j)
158  {
159  // # v0 = v1 + v2
160  // # v1 = t1 * a0 + 0.5*j*t1**2
161  // # v2 = 0.5*j*t2**2
162  // # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
163  // # t2 = (a0 - t1 * j) / j
164  // # t2 = a0/j - t1
165  // # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
166  // # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
167  // # t1 = t1_2
168  double d = math::MathUtils::Sign(v0);
169  v0 *= d;
170  a0 *= d;
171  // j *= d;
172  double part = j * v0 - a0 * a0 / 2.0;
173  if (part < 0)
174  {
176  r.s_total = -1;
177  r.dt2 = -1;
178  return r;
179  throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
180  " a0: " + std::to_string(a0) + " v0: " + std::to_string(v0));
181  }
182  double t1 = std::sqrt(part) / j;
183  double t2 = (a0 / j) + t1;
184  double v1 = v(t1, v0, a0, -j);
185  // double dv1 = v(t1, 0, a0, -j);
186  // double diff_v1 = v0 - v1;
187  double a1 = a(t1, -a0, -j);
188  // double a1_2 = a(t2, 0, j);
189  double v2 = v(t2, v1, a1, j);
190  // double dv2 = v(t2, 0, 0, j);
191  // double v_sum = abs(dv1)+dv2;
192  double a2 = a(t2, a1, j);
193  // assert(abs(v_sum - v0) < 0.001);
194  // assert(abs(v2) < 0.0001);
195  double s1 = s(t1, 0, v0, a0, -j);
196  double s2 = s(t2, 0, v1, a1, j);
197  double s_total = s1 + s2;
198  return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
199  }
200 } // namespace armarx::ctrlutil
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::ctrlutil::WedgeBrakingData::dt2
double dt2
Definition: CtrlUtil.h:131
armarx::ctrlutil::WedgeBrakingData
Definition: CtrlUtil.h:129
armarx::ctrlutil::BrakingData
Definition: CtrlUtil.h:70
MathUtils.h
armarx::ctrlutil::WedgeBrakingData::a1
double a1
Definition: CtrlUtil.h:131
armarx::ctrlutil::WedgeBrakingData::v2
double v2
Definition: CtrlUtil.h:131
armarx::ctrlutil::calcAccAndJerk
std::tuple< double, double, double > calcAccAndJerk(double s0, double v0)
Definition: CtrlUtil.h:147
part
Use of this software is granted under one of the following two to be chosen freely by the user Boost Software License Version Marcin Kalicinski Permission is hereby free of to any person or organization obtaining a copy of the software and accompanying documentation covered by this and transmit the and to prepare derivative works of the and to permit third parties to whom the Software is furnished to do all subject to the including the above license this restriction and the following must be included in all copies of the in whole or in part
armarx::ctrlutil::brakingDistance
double brakingDistance(double v0, double a0, double j)
Definition: CtrlUtil.h:76
armarx::ctrlutil::BrakingData::v3
double v3
Definition: CtrlUtil.h:72
armarx::ctrlutil::WedgeBrakingData::s1
double s1
Definition: CtrlUtil.h:131
armarx::ctrlutil::WedgeBrakingData::t
double t
Definition: CtrlUtil.h:131
armarx::ctrlutil::isPossibleToBrake
bool isPossibleToBrake(double v0, double a0, double j)
Definition: CtrlUtil.h:135
armarx::ctrlutil::WedgeBrakingData::dt1
double dt1
Definition: CtrlUtil.h:131
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::ctrlutil::BrakingData::dt1
double dt1
Definition: CtrlUtil.h:72
armarx::ctrlutil::BrakingData::dt2
double dt2
Definition: CtrlUtil.h:72
armarx::ctrlutil::braking_distance_with_wedge
WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
Definition: CtrlUtil.h:157
armarx::ctrlutil::BrakingData::v2
double v2
Definition: CtrlUtil.h:72
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:40
armarx::ctrlutil::WedgeBrakingData::a2
double a2
Definition: CtrlUtil.h:131
armarx::ctrlutil::BrakingData::dt3
double dt3
Definition: CtrlUtil.h:72
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::math::MathUtils::LimitTo
static double LimitTo(double value, double absThreshold)
Definition: MathUtils.h:56
armarx::math::MathUtils::Sign
static int Sign(double value)
Definition: MathUtils.h:34
armarx::ctrlutil::WedgeBrakingData::s_total
double s_total
Definition: CtrlUtil.h:131
armarx::ctrlutil::WedgeBrakingData::v1
double v1
Definition: CtrlUtil.h:131
armarx::ctrlutil
Definition: CtrlUtil.h:30
armarx::ctrlutil::BrakingData::s_total
double s_total
Definition: CtrlUtil.h:72
armarx::ctrlutil::jerk
double jerk(double t, double s0, double v0, double a0)
Definition: CtrlUtil.h:141
armarx::ctrlutil::BrakingData::v1
double v1
Definition: CtrlUtil.h:72
armarx::ctrlutil::t_to_v_with_wedge_acc
double t_to_v_with_wedge_acc(double v, double a0, double j)
Definition: CtrlUtil.h:64
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx::ctrlutil::WedgeBrakingData::s2
double s2
Definition: CtrlUtil.h:131
armarx::ctrlutil::t_to_v
double t_to_v(double v, double a, double j)
Definition: CtrlUtil.h:51