CtrlUtil.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
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
8  * published by the Free Software Foundation.
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
21  * @copyright http{//www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 //pragma once
25 #include <cmath>
27 #include <iostream>
28 
30 {
31  double s(double t, double s0, double v0, double a0, double j)
32  {
33  return s0 + v0 * t + 0.5 * a0 * t * t + 1.0 / 6.0 * j * t * t * t;
34  }
35  double v(double t, double v0, double a0, double j)
36  {
37  return v0 + a0 * t + 0.5 * j * t * t;
38  }
39  double a(double t, double a0, double j)
40  {
41  return a0 + j * t;
42  }
43 
44 
45  double t_to_v(double v, double a, double j)
46  {
47  // time to accelerate to v with jerk j starting at acceleration a0
48  //return 2*math.sqrt(a0**2+2*j*(v/2))-a0/j
49  // 0 = (-v + a0*t) /(0.5*j) + t*t
50  // 0 = -2v/j + 2a0t/j + t*t
51  // --> p = 2a0/j; q = -2v/j
52  // t = - a0/j +- sqrt((a0/j)**2 +2v/j)
53  double tmp = a / j;
54  return - a / j + std::sqrt(tmp * tmp + 2 * v / j);
55  }
56 
57 
58 
59  double t_to_v_with_wedge_acc(double v, double a0, double j)
60  {
61  // ramp up and down of acceleration (i.e. a0=at); returns t to achieve delta v
62  return 2 * t_to_v(v / 2.0, a0, j);
63  }
64 
65  struct BrakingData
66  {
67  double s_total, v1, v2, v3, dt1, dt2, dt3;
68  };
69 
70  double brakingDistance(double v0, double a0, double j)
71  {
72  auto signV = math::MathUtils::Sign(v0);
73  auto t = t_to_v(v0, -signV * a0, signV * j);
74  return s(t, 0, v0, a0, -signV * j);
75  }
76 
77  BrakingData brakingDistance(double goal, double s0, double v0, double a0, double v_max, double a_max, double j, double dec_max)
78  {
79  double d = math::MathUtils::Sign(goal - s0); // if v0 == 0.0 else abs(v0)/a_max
80  dec_max *= -d;
81  // std::cout << "dec max: " << (dec_max) << " d: " << d << std::endl;
82  double dt1 = std::abs((dec_max - a0) / (-j * d));
83  // double dt1 = t_to_v(v_max-v0, a0, j);
84  // double a1 = a(dt1, a0, -d * j);
85  double v1 = v(dt1, v0, a0, -d * j);
86  double acc_duration = std::abs(dec_max) / j;
87  double v2 = v(acc_duration, 0, 0, d * j);// # inverse of time to accelerate from 0 to max v
88  v2 = math::MathUtils::LimitTo(v2, v_max);
89  // v2 = v1 + dv2
90  double dt2 = d * ((v1 - v2) / dec_max);
91  // double a2 = a(dt2, a1, 0);
92 
93  double dt3 = t_to_v(std::abs(v2), 0, j);
94  double s1 = s(dt1, 0, v0, a0, d * j);
95  double s2 = s(dt2, 0, v1, dec_max, 0);
96  double s3 = s(dt3, 0, v2, dec_max, d * j);
97  double v3 = v(dt3, v2, dec_max, d * j);
98  double s_total = s1 + s2 + s3;
99 
100  if (dt2 < 0)// # we dont have a phase with a=a_max and need to reduce the a_max
101  {
102  double excess_time = t_to_v_with_wedge_acc(std::abs(v1), std::abs(dec_max), j);
103  double delta_a = a(excess_time / 2, 0, d * j);
104  a_max -= d * delta_a;
105  dec_max = std::abs(dec_max) - std::abs(delta_a);
106  dec_max *= -d;
107  return brakingDistance(goal, s0, v0, a0, v_max, a_max, j, dec_max);
108  }
109 
110  return {s_total * d, v1, v2, v3, dt1, dt2, dt3};
111  // return (s_total, v1, v2, v3, dt1, dt2, dt3);
112  }
113 
115  {
116  double s_total, s1, s2, v1, v2, a1, a2, t, dt1, dt2;
117  };
118  bool isPossibleToBrake(double v0, double a0, double j)
119  {
120  return j * v0 - a0 * a0 / 2 > 0.0f;
121  }
122 
123  double jerk(double t, double s0, double v0, double a0)
124  {
125  return (6 * s0 - 3 * t * (a0 * t + 2 * v0)) / (t * t * t);
126  }
127 
128  std::tuple<double, double, double> calcAccAndJerk(double s0, double v0)
129  {
130  s0 *= -1;
131  double t = - (3 * s0) / v0;
132  double a0 = - (2 * v0) / t;
133  double j = 2 * v0 / (t * t);
134  return std::make_tuple(t, a0, j);
135  }
136 
137 
138 
139  WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
140  {
141  // # v0 = v1 + v2
142  // # v1 = t1 * a0 + 0.5*j*t1**2
143  // # v2 = 0.5*j*t2**2
144  // # a1 = t2 * j ^ a1 = a0 - t1 * j -> t2 * j = a0 - t1 * j
145  // # t2 = (a0 - t1 * j) / j
146  // # t2 = a0/j - t1
147  // # t1_1 = -(math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) + 2*a0*j)/(2*j**2)
148  // # t1_2 = (math.sqrt(2*j**2 * (a0**2 + 2*j*v0)) - 2*a0*j)/(2*j**2)
149  // # t1 = t1_2
150  double d = math::MathUtils::Sign(v0);
151  v0 *= d;
152  a0 *= d;
153  // j *= d;
154  double part = j * v0 - a0 * a0 / 2.0;
155  if (part < 0)
156  {
158  r.s_total = -1;
159  r.dt2 = -1;
160  return r;
161  throw std::runtime_error("unsuitable parameters! : j: " + std::to_string(j) +
162  " a0: " + std::to_string(a0) +
163  " v0: " + std::to_string(v0));
164  }
165  double t1 = std::sqrt(part) / j;
166  double t2 = (a0 / j) + t1;
167  double v1 = v(t1, v0, a0, -j);
168  // double dv1 = v(t1, 0, a0, -j);
169  // double diff_v1 = v0 - v1;
170  double a1 = a(t1, -a0, -j);
171  // double a1_2 = a(t2, 0, j);
172  double v2 = v(t2, v1, a1, j);
173  // double dv2 = v(t2, 0, 0, j);
174  // double v_sum = abs(dv1)+dv2;
175  double a2 = a(t2, a1, j);
176  // assert(abs(v_sum - v0) < 0.001);
177  // assert(abs(v2) < 0.0001);
178  double s1 = s(t1, 0, v0, a0, -j);
179  double s2 = s(t2, 0, v1, a1, j);
180  double s_total = s1 + s2;
181  return {s_total, s1, s2, d * v1, d * v2, d * a1, d * a2, t1 + t2, t1, t2};
182  }
183 }
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::ctrlutil::WedgeBrakingData::dt2
double dt2
Definition: CtrlUtil.h:116
armarx::ctrlutil::WedgeBrakingData
Definition: CtrlUtil.h:114
armarx::ctrlutil::BrakingData
Definition: CtrlUtil.h:65
MathUtils.h
armarx::ctrlutil::WedgeBrakingData::a1
double a1
Definition: CtrlUtil.h:116
armarx::ctrlutil::WedgeBrakingData::v2
double v2
Definition: CtrlUtil.h:116
armarx::ctrlutil::calcAccAndJerk
std::tuple< double, double, double > calcAccAndJerk(double s0, double v0)
Definition: CtrlUtil.h:128
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
Definition: license.txt:18
armarx::ctrlutil::brakingDistance
double brakingDistance(double v0, double a0, double j)
Definition: CtrlUtil.h:70
armarx::ctrlutil::BrakingData::v3
double v3
Definition: CtrlUtil.h:67
armarx::ctrlutil::WedgeBrakingData::s1
double s1
Definition: CtrlUtil.h:116
armarx::ctrlutil::WedgeBrakingData::t
double t
Definition: CtrlUtil.h:116
armarx::ctrlutil::isPossibleToBrake
bool isPossibleToBrake(double v0, double a0, double j)
Definition: CtrlUtil.h:118
armarx::ctrlutil::WedgeBrakingData::dt1
double dt1
Definition: CtrlUtil.h:116
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:39
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::ctrlutil::BrakingData::dt1
double dt1
Definition: CtrlUtil.h:67
armarx::ctrlutil::BrakingData::dt2
double dt2
Definition: CtrlUtil.h:67
armarx::ctrlutil::braking_distance_with_wedge
WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
Definition: CtrlUtil.h:139
armarx::ctrlutil::BrakingData::v2
double v2
Definition: CtrlUtil.h:67
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:116
armarx::ctrlutil::BrakingData::dt3
double dt3
Definition: CtrlUtil.h:67
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:35
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:116
armarx::ctrlutil::WedgeBrakingData::v1
double v1
Definition: CtrlUtil.h:116
armarx::ctrlutil
Definition: CtrlUtil.h:29
armarx::ctrlutil::BrakingData::s_total
double s_total
Definition: CtrlUtil.h:67
armarx::ctrlutil::jerk
double jerk(double t, double s0, double v0, double a0)
Definition: CtrlUtil.h:123
armarx::ctrlutil::BrakingData::v1
double v1
Definition: CtrlUtil.h:67
armarx::ctrlutil::t_to_v_with_wedge_acc
double t_to_v_with_wedge_acc(double v, double a0, double j)
Definition: CtrlUtil.h:59
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:31
armarx::ctrlutil::WedgeBrakingData::s2
double s2
Definition: CtrlUtil.h:116
armarx::ctrlutil::t_to_v
double t_to_v(double v, double a, double j)
Definition: CtrlUtil.h:45