test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp@27:79b4b932a6dd, 2019-02-27 (annotated)
- Committer:
- shimizuta
- Date:
- Wed Feb 27 01:24:00 2019 +0000
- Revision:
- 27:79b4b932a6dd
- Parent:
- 26:24ae5a4f5b1f
- Child:
- 29:7d8b8011a88d
debug was ok. ROS is not debuged
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 27:79b4b932a6dd | 1 | #define _USE_MATH_DEFINES |
shimizuta | 11:e81425872740 | 2 | #include "math.h" |
shimizuta | 27:79b4b932a6dd | 3 | #include <stdio.h> |
shimizuta | 27:79b4b932a6dd | 4 | #include "stdlib.h" |
shimizuta | 27:79b4b932a6dd | 5 | #include "Walk.h" |
shimizuta | 27:79b4b932a6dd | 6 | #include "pi.h" |
yuto17320508 | 13:e7ecdb20665a | 7 | //Orbitは足毎の軌道をあらわす。 |
shimizuta | 27:79b4b932a6dd | 8 | const float kGravity = 9.8; |
shimizuta | 27:79b4b932a6dd | 9 | void EllipseOrbit::SetParam(float stridetime_s, float risetime_s, |
shimizuta | 27:79b4b932a6dd | 10 | float stride_m, float height_m, float ground_m, |
shimizuta | 27:79b4b932a6dd | 11 | float ellipsecenter_x_m, float ellipsecenter_y_m) |
shimizuta | 14:d7cb429946f4 | 12 | { |
shimizuta | 14:d7cb429946f4 | 13 | stridetime_s_ = stridetime_s; |
shimizuta | 14:d7cb429946f4 | 14 | risetime_s_ = risetime_s; |
shimizuta | 14:d7cb429946f4 | 15 | stride_m_ = stride_m; |
shimizuta | 14:d7cb429946f4 | 16 | height_m_ = height_m; |
shimizuta | 21:61971fc18b90 | 17 | ground_m_ = ground_m; |
shimizuta | 27:79b4b932a6dd | 18 | ellipsecenter_x_m_ = ellipsecenter_x_m; |
shimizuta | 27:79b4b932a6dd | 19 | ellipsecenter_y_m_ = ellipsecenter_y_m; |
shimizuta | 14:d7cb429946f4 | 20 | } |
shimizuta | 14:d7cb429946f4 | 21 | |
shimizuta | 11:e81425872740 | 22 | //着地中の動き.直線軌道.等速 |
shimizuta | 27:79b4b932a6dd | 23 | int EllipseOrbit::StrideLine_(OneLeg &leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 24 | { |
shimizuta | 27:79b4b932a6dd | 25 | float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipsecenter_x_m_; |
shimizuta | 27:79b4b932a6dd | 26 | float y_m = ellipsecenter_y_m_; |
shimizuta | 27:79b4b932a6dd | 27 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 11:e81425872740 | 28 | } |
shimizuta | 27:79b4b932a6dd | 29 | int EllipseOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s) |
shimizuta | 23:97f34a7dc63c | 30 | { |
shimizuta | 27:79b4b932a6dd | 31 | ///////////x,yを計算 |
shimizuta | 27:79b4b932a6dd | 32 | float s0 = stride_m_ * 0.5 + ellipsecenter_x_m_; |
shimizuta | 27:79b4b932a6dd | 33 | float s1 = -stride_m_ * 0.5 + ellipsecenter_x_m_; |
shimizuta | 27:79b4b932a6dd | 34 | float g_h = sqrtf(kGravity / ground_m_); |
shimizuta | 27:79b4b932a6dd | 35 | float t = phasetime_s / stridetime_s_; |
shimizuta | 27:79b4b932a6dd | 36 | float denominator = expf(g_h) - expf(-g_h); //分母 |
shimizuta | 27:79b4b932a6dd | 37 | |
shimizuta | 27:79b4b932a6dd | 38 | float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator; |
A0413 | 24:dec983da455c | 39 | float y_m = ground_m_; |
shimizuta | 23:97f34a7dc63c | 40 | //x,yを代入 |
shimizuta | 27:79b4b932a6dd | 41 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 23:97f34a7dc63c | 42 | } |
shimizuta | 21:61971fc18b90 | 43 | //空中の動き.半分にきれいに切れる楕円軌道 |
shimizuta | 27:79b4b932a6dd | 44 | int EllipseOrbit::RiseEllipse_(OneLeg &leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 45 | { |
shimizuta | 14:d7cb429946f4 | 46 | float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI; |
shimizuta | 27:79b4b932a6dd | 47 | float x_m = stride_m_ * 0.5 * cosf(rad) + ellipsecenter_x_m_; |
shimizuta | 27:79b4b932a6dd | 48 | float y_m = height_m_ * sinf(rad) + ellipsecenter_y_m_; |
shimizuta | 27:79b4b932a6dd | 49 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 21:61971fc18b90 | 50 | } |
shimizuta | 21:61971fc18b90 | 51 | //空中の動き.完全に半分には切れない楕円軌道 |
shimizuta | 27:79b4b932a6dd | 52 | int EllipseOrbit::RiseEllipse2_(OneLeg &leg, float phasetime_s) |
shimizuta | 21:61971fc18b90 | 53 | { |
shimizuta | 27:79b4b932a6dd | 54 | ///////////x,yを計算 |
shimizuta | 27:79b4b932a6dd | 55 | float ellipselong_m = stride_m_ * 0.5 / cosf(asinf(ground_m_ - ellipsecenter_y_m_)); |
shimizuta | 27:79b4b932a6dd | 56 | float ellipseshort_m = 2 * (height_m_ - ground_m_ + ellipsecenter_y_m_); |
shimizuta | 27:79b4b932a6dd | 57 | float theta1 = acosf(stride_m_ / ellipselong_m * 0.5); //stride の始まりの位置の角度(数学基準) |
shimizuta | 27:79b4b932a6dd | 58 | float theta2 = acosf(-stride_m_ / ellipselong_m * 0.5); //stride の終わりの位置の角度(数学基準) |
shimizuta | 27:79b4b932a6dd | 59 | float omega = (2 * M_PI - theta2 + theta1) / risetime_s_; //楕円軌道における角速度、一定の値とする(適当) |
shimizuta | 27:79b4b932a6dd | 60 | float rad = (phasetime_s - stridetime_s_) * omega + theta2; //角度の基準はtheta1とする(適当) |
shimizuta | 27:79b4b932a6dd | 61 | float x_m = ellipselong_m * cos(rad) + ellipsecenter_x_m_; |
shimizuta | 27:79b4b932a6dd | 62 | float y_m = ellipseshort_m * sin(rad) + ellipsecenter_y_m_; |
shimizuta | 21:61971fc18b90 | 63 | //x,yを代入 |
shimizuta | 27:79b4b932a6dd | 64 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 11:e81425872740 | 65 | } |
shimizuta | 11:e81425872740 | 66 | //楕円軌道 |
shimizuta | 27:79b4b932a6dd | 67 | int EllipseOrbit::GetOrbit(OneLeg &leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 68 | { |
shimizuta | 11:e81425872740 | 69 | if (phasetime_s < stridetime_s_) |
shimizuta | 11:e81425872740 | 70 | return StrideLine_(leg, phasetime_s); |
shimizuta | 11:e81425872740 | 71 | else |
shimizuta | 11:e81425872740 | 72 | return RiseEllipse_(leg, phasetime_s); |
shimizuta | 11:e81425872740 | 73 | } |
shimizuta | 27:79b4b932a6dd | 74 | float EllipseOrbit::GetOneWalkTime() |
shimizuta | 27:79b4b932a6dd | 75 | { |
shimizuta | 27:79b4b932a6dd | 76 | return stridetime_s_ + risetime_s_; |
shimizuta | 27:79b4b932a6dd | 77 | } |
shimizuta | 27:79b4b932a6dd | 78 | void EllipseOrbit::ChangeOneParam(EllipseParams param, float val) |
shimizuta | 27:79b4b932a6dd | 79 | { |
shimizuta | 27:79b4b932a6dd | 80 | switch (param) |
shimizuta | 27:79b4b932a6dd | 81 | { |
shimizuta | 27:79b4b932a6dd | 82 | case STRIDE_M: |
shimizuta | 27:79b4b932a6dd | 83 | stride_m_ = val; |
shimizuta | 27:79b4b932a6dd | 84 | break; |
shimizuta | 27:79b4b932a6dd | 85 | case HEIGHT_M: |
shimizuta | 27:79b4b932a6dd | 86 | height_m_ = val; |
shimizuta | 27:79b4b932a6dd | 87 | break; |
shimizuta | 27:79b4b932a6dd | 88 | case GROUND_M: |
shimizuta | 27:79b4b932a6dd | 89 | ground_m_ = val; |
shimizuta | 27:79b4b932a6dd | 90 | break; |
shimizuta | 27:79b4b932a6dd | 91 | case ELLIPSE_CENTER_X_M: |
shimizuta | 27:79b4b932a6dd | 92 | ellipsecenter_x_m_ = val; |
shimizuta | 27:79b4b932a6dd | 93 | break; |
shimizuta | 27:79b4b932a6dd | 94 | case ELLIPSE_CENTER_Y_M: |
shimizuta | 27:79b4b932a6dd | 95 | ellipsecenter_y_m_ = val; |
shimizuta | 27:79b4b932a6dd | 96 | break; |
shimizuta | 27:79b4b932a6dd | 97 | } |
shimizuta | 27:79b4b932a6dd | 98 | } |
shimizuta | 27:79b4b932a6dd | 99 | void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num) |
shimizuta | 27:79b4b932a6dd | 100 | { |
shimizuta | 27:79b4b932a6dd | 101 | point_num_ = point_num; |
shimizuta | 27:79b4b932a6dd | 102 | if (point_num_ == 1) |
shimizuta | 27:79b4b932a6dd | 103 | { |
shimizuta | 27:79b4b932a6dd | 104 | printf("error:point_num = 1. You should put >=2 to make line."); |
shimizuta | 27:79b4b932a6dd | 105 | while (1) |
shimizuta | 27:79b4b932a6dd | 106 | ; |
shimizuta | 27:79b4b932a6dd | 107 | } |
shimizuta | 27:79b4b932a6dd | 108 | for (int i = 0; i < point_num_; i++) |
shimizuta | 27:79b4b932a6dd | 109 | lineparams_[i] = lineparams[i]; |
shimizuta | 27:79b4b932a6dd | 110 | } |
shimizuta | 27:79b4b932a6dd | 111 | int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 112 | { |
shimizuta | 27:79b4b932a6dd | 113 | ; |
shimizuta | 27:79b4b932a6dd | 114 | //現在の最終到達pointを決定 |
shimizuta | 27:79b4b932a6dd | 115 | int arrived_point; |
shimizuta | 27:79b4b932a6dd | 116 | float sum_time = 0; |
shimizuta | 27:79b4b932a6dd | 117 | for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num-1(これが配列の最大index)の状態で終わらないように-2している |
shimizuta | 27:79b4b932a6dd | 118 | { |
shimizuta | 27:79b4b932a6dd | 119 | sum_time += lineparams_[arrived_point].time_s; |
shimizuta | 27:79b4b932a6dd | 120 | if (phasetime_s < sum_time) |
shimizuta | 27:79b4b932a6dd | 121 | break; |
shimizuta | 27:79b4b932a6dd | 122 | } |
shimizuta | 27:79b4b932a6dd | 123 | //x,yを計算 |
shimizuta | 27:79b4b932a6dd | 124 | float x_m, y_m; |
shimizuta | 27:79b4b932a6dd | 125 | x_m = lineparams_[arrived_point].x_m + |
shimizuta | 27:79b4b932a6dd | 126 | (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * phasetime_s / lineparams_[arrived_point].time_s; |
shimizuta | 27:79b4b932a6dd | 127 | y_m = lineparams_[arrived_point].y_m + |
shimizuta | 27:79b4b932a6dd | 128 | (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * phasetime_s / lineparams_[arrived_point].time_s; |
shimizuta | 27:79b4b932a6dd | 129 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 27:79b4b932a6dd | 130 | } |
shimizuta | 27:79b4b932a6dd | 131 | float FreeLineOrbit::GetOneWalkTime() //足一周の時間 |
shimizuta | 27:79b4b932a6dd | 132 | { |
shimizuta | 27:79b4b932a6dd | 133 | float sum_time = 0; |
shimizuta | 27:79b4b932a6dd | 134 | for (int i = 0; i < point_num_; i++) |
shimizuta | 27:79b4b932a6dd | 135 | sum_time += lineparams_[i].time_s; |
shimizuta | 27:79b4b932a6dd | 136 | return sum_time; |
shimizuta | 27:79b4b932a6dd | 137 | } |
shimizuta | 27:79b4b932a6dd | 138 | void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s) |
shimizuta | 27:79b4b932a6dd | 139 | { |
shimizuta | 27:79b4b932a6dd | 140 | point_num_ = 1; |
shimizuta | 27:79b4b932a6dd | 141 | lineparams_[0].x_m = x_m; |
shimizuta | 27:79b4b932a6dd | 142 | lineparams_[0].y_m = y_m; |
shimizuta | 27:79b4b932a6dd | 143 | lineparams_[0].time_s = time_s; |
shimizuta | 27:79b4b932a6dd | 144 | lineparams_[1].x_m = x_m; |
shimizuta | 27:79b4b932a6dd | 145 | lineparams_[1].y_m = y_m; |
shimizuta | 27:79b4b932a6dd | 146 | lineparams_[1].time_s = 0; |
shimizuta | 27:79b4b932a6dd | 147 | } |
shimizuta | 27:79b4b932a6dd | 148 | |
shimizuta | 27:79b4b932a6dd | 149 | Orbit::Orbit(OrbitPattern pattern) |
shimizuta | 27:79b4b932a6dd | 150 | { |
shimizuta | 27:79b4b932a6dd | 151 | pattern_ = pattern; |
shimizuta | 11:e81425872740 | 152 | } |
shimizuta | 11:e81425872740 | 153 | float Orbit::GetOneWalkTime() |
shimizuta | 11:e81425872740 | 154 | { |
shimizuta | 27:79b4b932a6dd | 155 | float time_s; |
shimizuta | 27:79b4b932a6dd | 156 | switch (pattern_) |
shimizuta | 27:79b4b932a6dd | 157 | { |
shimizuta | 27:79b4b932a6dd | 158 | case FREELINES: |
shimizuta | 27:79b4b932a6dd | 159 | time_s = FreeLineOrbit::GetOneWalkTime(); |
shimizuta | 27:79b4b932a6dd | 160 | break; |
shimizuta | 27:79b4b932a6dd | 161 | |
shimizuta | 27:79b4b932a6dd | 162 | default: |
shimizuta | 27:79b4b932a6dd | 163 | time_s = EllipseOrbit::GetOneWalkTime(); |
shimizuta | 27:79b4b932a6dd | 164 | break; |
shimizuta | 27:79b4b932a6dd | 165 | } |
shimizuta | 27:79b4b932a6dd | 166 | return time_s; |
shimizuta | 27:79b4b932a6dd | 167 | } |
shimizuta | 27:79b4b932a6dd | 168 | int Orbit::GetOrbit(OneLeg &leg, float phasetime_s) |
shimizuta | 27:79b4b932a6dd | 169 | { |
shimizuta | 27:79b4b932a6dd | 170 | int ret; |
shimizuta | 27:79b4b932a6dd | 171 | switch (pattern_) |
shimizuta | 27:79b4b932a6dd | 172 | { |
shimizuta | 27:79b4b932a6dd | 173 | case FREELINES: |
shimizuta | 27:79b4b932a6dd | 174 | ret = FreeLineOrbit::GetOrbit(leg, phasetime_s); |
shimizuta | 27:79b4b932a6dd | 175 | break; |
shimizuta | 27:79b4b932a6dd | 176 | default: |
shimizuta | 27:79b4b932a6dd | 177 | ret = EllipseOrbit::GetOrbit(leg, phasetime_s); |
shimizuta | 27:79b4b932a6dd | 178 | break; |
shimizuta | 27:79b4b932a6dd | 179 | } |
shimizuta | 27:79b4b932a6dd | 180 | return ret; |
shimizuta | 11:e81425872740 | 181 | } |
shimizuta | 11:e81425872740 | 182 | |
shimizuta | 27:79b4b932a6dd | 183 | Walk::Walk() |
shimizuta | 27:79b4b932a6dd | 184 | { |
shimizuta | 27:79b4b932a6dd | 185 | for (int i = 0; i < 4; i++) |
shimizuta | 27:79b4b932a6dd | 186 | offset_multi_[i] = 0; |
shimizuta | 27:79b4b932a6dd | 187 | } |
shimizuta | 27:79b4b932a6dd | 188 | float Walk::calctime_s_; |
shimizuta | 27:79b4b932a6dd | 189 | void Walk::Cal4LegsPosi(OneLeg leg[4]) |
shimizuta | 11:e81425872740 | 190 | { |
shimizuta | 11:e81425872740 | 191 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 192 | { |
shimizuta | 27:79b4b932a6dd | 193 | float one_walk_time = orbit_[i].GetOneWalkTime(); |
shimizuta | 27:79b4b932a6dd | 194 | phasetime_s_[i] += calctime_s_; |
shimizuta | 27:79b4b932a6dd | 195 | while (phasetime_s_[i] > one_walk_time) |
shimizuta | 27:79b4b932a6dd | 196 | phasetime_s_[i] -= one_walk_time; |
shimizuta | 27:79b4b932a6dd | 197 | orbit_[i].GetOrbit(leg[i], phasetime_s_[i]); |
shimizuta | 11:e81425872740 | 198 | } |
shimizuta | 11:e81425872740 | 199 | } |
shimizuta | 27:79b4b932a6dd | 200 | float Walk::GetOneWalkTime() |
shimizuta | 27:79b4b932a6dd | 201 | { |
shimizuta | 27:79b4b932a6dd | 202 | return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している. |
shimizuta | 27:79b4b932a6dd | 203 | }; |
shimizuta | 27:79b4b932a6dd | 204 | void Walk::SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3) |
shimizuta | 11:e81425872740 | 205 | { |
shimizuta | 27:79b4b932a6dd | 206 | offset_multi_[0] = offset_multi0; |
shimizuta | 27:79b4b932a6dd | 207 | offset_multi_[1] = offset_multi1; |
shimizuta | 27:79b4b932a6dd | 208 | offset_multi_[2] = offset_multi2; |
shimizuta | 27:79b4b932a6dd | 209 | offset_multi_[3] = offset_multi3; |
shimizuta | 27:79b4b932a6dd | 210 | } |
shimizuta | 27:79b4b932a6dd | 211 | //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out |
shimizuta | 27:79b4b932a6dd | 212 | int Walk::CheckOrbit(OneLeg templateleg) |
shimizuta | 27:79b4b932a6dd | 213 | { |
shimizuta | 11:e81425872740 | 214 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 215 | { |
shimizuta | 27:79b4b932a6dd | 216 | //軌道が値域の外に出ないか計算で確かめる |
shimizuta | 27:79b4b932a6dd | 217 | float one_walk_time = GetOneWalkTime(); |
shimizuta | 27:79b4b932a6dd | 218 | int step = one_walk_time / calctime_s_ * 2; |
shimizuta | 27:79b4b932a6dd | 219 | for (float j = 0; j < one_walk_time; j += step) |
shimizuta | 27:79b4b932a6dd | 220 | { |
shimizuta | 27:79b4b932a6dd | 221 | if (orbit_[i].GetOrbit(templateleg, j) == 1) |
shimizuta | 27:79b4b932a6dd | 222 | return 1; //解が出ないときは1を返す |
shimizuta | 27:79b4b932a6dd | 223 | } |
shimizuta | 27:79b4b932a6dd | 224 | phasetime_s_[i] = one_walk_time * offset_multi_[i]; |
shimizuta | 11:e81425872740 | 225 | } |
shimizuta | 27:79b4b932a6dd | 226 | return 0; |
shimizuta | 11:e81425872740 | 227 | } |
shimizuta | 27:79b4b932a6dd | 228 | void Walk::SetOneOrbit(int legnum, Orbit orbit) |
shimizuta | 27:79b4b932a6dd | 229 | { |
shimizuta | 27:79b4b932a6dd | 230 | orbit_[legnum] = orbit; |
shimizuta | 27:79b4b932a6dd | 231 | } |
shimizuta | 27:79b4b932a6dd | 232 | void Walk::SetAllOrbit(Orbit orbit) |
shimizuta | 27:79b4b932a6dd | 233 | { |
shimizuta | 27:79b4b932a6dd | 234 | for (int i = 0; i < 4; i++) |
shimizuta | 27:79b4b932a6dd | 235 | SetOneOrbit(i, orbit); |
shimizuta | 27:79b4b932a6dd | 236 | } |
shimizuta | 27:79b4b932a6dd | 237 | |
shimizuta | 27:79b4b932a6dd | 238 | void Walk::ChangeOneParam(int legnum, EllipseParams param, float val) |
shimizuta | 27:79b4b932a6dd | 239 | { |
shimizuta | 27:79b4b932a6dd | 240 | orbit_[legnum].ChangeOneParam(param, val); |
shimizuta | 27:79b4b932a6dd | 241 | } |
shimizuta | 27:79b4b932a6dd | 242 | void Walk::SetAllLegEllipseParam(float stridetime_s, float risetime_s, |
shimizuta | 27:79b4b932a6dd | 243 | float stride_m, float height_m, float ground_m, |
shimizuta | 27:79b4b932a6dd | 244 | float ellipsecenter_x_m, float ellipsecenter_y_m) |
shimizuta | 27:79b4b932a6dd | 245 | { |
shimizuta | 27:79b4b932a6dd | 246 | for (int i = 0; i < 4; i++) |
shimizuta | 27:79b4b932a6dd | 247 | orbit_[i].SetParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, ellipsecenter_x_m, ellipsecenter_y_m); |
shimizuta | 27:79b4b932a6dd | 248 | } |
shimizuta | 27:79b4b932a6dd | 249 | void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s) |
shimizuta | 27:79b4b932a6dd | 250 | { |
shimizuta | 27:79b4b932a6dd | 251 | Orbit stand(FREELINES); |
shimizuta | 27:79b4b932a6dd | 252 | stand.SetStandParam(x_m, y_m, time_s); //defaultとなる軌道設定 |
shimizuta | 27:79b4b932a6dd | 253 | SetAllOrbit(stand); //4足にコピー |
shimizuta | 27:79b4b932a6dd | 254 | } |