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>
26#include <iostream>
27
28#include <RobotAPI/libraries/core/math/MathUtils.h>
29
30namespace armarx::ctrlutil
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
129 struct WedgeBrakingData
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 {
175 WedgeBrakingData r;
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
armarx::math::MathUtils::Sign
static int Sign(double value)
Definition MathUtils.h:37
armarx::math::MathUtils::LimitTo
static double LimitTo(double value, double absThreshold)
Definition MathUtils.h:71
armarx::ctrlutil::braking_distance_with_wedge
WedgeBrakingData braking_distance_with_wedge(double v0, double a0, double j)
Definition CtrlUtil.h:157
armarx::ctrlutil::isPossibleToBrake
bool isPossibleToBrake(double v0, double a0, double j)
Definition CtrlUtil.h:135
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::a
double a(double t, double a0, double j)
Definition CtrlUtil.h:45
armarx::ctrlutil::calcAccAndJerk
std::tuple< double, double, double > calcAccAndJerk(double s0, double v0)
Definition CtrlUtil.h:147
armarx::ctrlutil::t_to_v
double t_to_v(double v, double a, double j)
Definition CtrlUtil.h:51
armarx::ctrlutil::brakingDistance
double brakingDistance(double v0, double a0, double j)
Definition CtrlUtil.h:76
armarx::ctrlutil::jerk
double jerk(double t, double s0, double v0, double a0)
Definition CtrlUtil.h:141
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition CtrlUtil.h:39