岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@32:dc684a0b8448, 2019-02-28 (annotated)

Committer:
shimizuta
Date:
Thu Feb 28 08:48:21 2019 +0000
Revision:
32:dc684a0b8448
Parent:
29:7d8b8011a88d
Child:
34:89d701e15cdf
a

Who changed what in which revision?

UserRevisionLine numberNew 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
shimizuta 27:79b4b932a6dd 10 Orbit::Orbit(OrbitPattern pattern)
shimizuta 27:79b4b932a6dd 11 {
shimizuta 27:79b4b932a6dd 12 pattern_ = pattern;
shimizuta 11:e81425872740 13 }
shimizuta 11:e81425872740 14 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 15 {
shimizuta 27:79b4b932a6dd 16 float time_s;
shimizuta 27:79b4b932a6dd 17 switch (pattern_)
shimizuta 27:79b4b932a6dd 18 {
shimizuta 32:dc684a0b8448 19 case FREELINES:
shimizuta 32:dc684a0b8448 20 time_s = FreeLineOrbit::GetOneWalkTime();
shimizuta 32:dc684a0b8448 21 break;
shimizuta 27:79b4b932a6dd 22 default:
shimizuta 29:7d8b8011a88d 23 time_s = TriangleOrbit::GetOneWalkTime();
shimizuta 27:79b4b932a6dd 24 break;
shimizuta 27:79b4b932a6dd 25 }
shimizuta 27:79b4b932a6dd 26 return time_s;
shimizuta 27:79b4b932a6dd 27 }
shimizuta 27:79b4b932a6dd 28 int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 29 {
shimizuta 27:79b4b932a6dd 30 int ret;
shimizuta 27:79b4b932a6dd 31 switch (pattern_)
shimizuta 27:79b4b932a6dd 32 {
shimizuta 32:dc684a0b8448 33 case FREELINES:
shimizuta 32:dc684a0b8448 34 ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
shimizuta 32:dc684a0b8448 35 break;
shimizuta 27:79b4b932a6dd 36 default:
shimizuta 29:7d8b8011a88d 37 ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
shimizuta 27:79b4b932a6dd 38 break;
shimizuta 27:79b4b932a6dd 39 }
shimizuta 27:79b4b932a6dd 40 return ret;
shimizuta 11:e81425872740 41 }
shimizuta 11:e81425872740 42
shimizuta 27:79b4b932a6dd 43 Walk::Walk()
shimizuta 27:79b4b932a6dd 44 {
shimizuta 27:79b4b932a6dd 45 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 46 offset_multi_[i] = 0;
shimizuta 27:79b4b932a6dd 47 }
shimizuta 27:79b4b932a6dd 48 float Walk::calctime_s_;
shimizuta 29:7d8b8011a88d 49 int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
shimizuta 11:e81425872740 50 {
shimizuta 29:7d8b8011a88d 51 int is_out = 0;
shimizuta 11:e81425872740 52 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 53 {
shimizuta 27:79b4b932a6dd 54 float one_walk_time = orbit_[i].GetOneWalkTime();
shimizuta 27:79b4b932a6dd 55 phasetime_s_[i] += calctime_s_;
shimizuta 27:79b4b932a6dd 56 while (phasetime_s_[i] > one_walk_time)
shimizuta 27:79b4b932a6dd 57 phasetime_s_[i] -= one_walk_time;
shimizuta 29:7d8b8011a88d 58 if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
shimizuta 29:7d8b8011a88d 59 {
shimizuta 29:7d8b8011a88d 60 printf("error:leg %d in Cal4LegsPosi\r\n", i);
shimizuta 29:7d8b8011a88d 61 is_out = 1;
shimizuta 29:7d8b8011a88d 62 }
shimizuta 11:e81425872740 63 }
shimizuta 29:7d8b8011a88d 64 return is_out;
shimizuta 11:e81425872740 65 }
shimizuta 27:79b4b932a6dd 66 float Walk::GetOneWalkTime()
shimizuta 27:79b4b932a6dd 67 {
shimizuta 27:79b4b932a6dd 68 return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
shimizuta 27:79b4b932a6dd 69 };
shimizuta 29:7d8b8011a88d 70 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
shimizuta 11:e81425872740 71 {
shimizuta 27:79b4b932a6dd 72 offset_multi_[0] = offset_multi0;
shimizuta 27:79b4b932a6dd 73 offset_multi_[1] = offset_multi1;
shimizuta 27:79b4b932a6dd 74 offset_multi_[2] = offset_multi2;
shimizuta 27:79b4b932a6dd 75 offset_multi_[3] = offset_multi3;
shimizuta 27:79b4b932a6dd 76 }
shimizuta 27:79b4b932a6dd 77 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 27:79b4b932a6dd 78 int Walk::CheckOrbit(OneLeg templateleg)
shimizuta 27:79b4b932a6dd 79 {
shimizuta 11:e81425872740 80 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 81 {
shimizuta 27:79b4b932a6dd 82 //軌道が値域の外に出ないか計算で確かめる
shimizuta 27:79b4b932a6dd 83 float one_walk_time = GetOneWalkTime();
shimizuta 27:79b4b932a6dd 84 int step = one_walk_time / calctime_s_ * 2;
shimizuta 27:79b4b932a6dd 85 for (float j = 0; j < one_walk_time; j += step)
shimizuta 27:79b4b932a6dd 86 {
shimizuta 27:79b4b932a6dd 87 if (orbit_[i].GetOrbit(templateleg, j) == 1)
shimizuta 29:7d8b8011a88d 88 {
shimizuta 29:7d8b8011a88d 89 printf("error:leg %d, time %f", i, j);
shimizuta 27:79b4b932a6dd 90 return 1; //解が出ないときは1を返す
shimizuta 29:7d8b8011a88d 91 }
shimizuta 27:79b4b932a6dd 92 }
shimizuta 27:79b4b932a6dd 93 phasetime_s_[i] = one_walk_time * offset_multi_[i];
shimizuta 11:e81425872740 94 }
shimizuta 27:79b4b932a6dd 95 return 0;
shimizuta 11:e81425872740 96 }
shimizuta 27:79b4b932a6dd 97 void Walk::SetOneOrbit(int legnum, Orbit orbit)
shimizuta 27:79b4b932a6dd 98 {
shimizuta 27:79b4b932a6dd 99 orbit_[legnum] = orbit;
shimizuta 27:79b4b932a6dd 100 }
shimizuta 27:79b4b932a6dd 101 void Walk::SetAllOrbit(Orbit orbit)
shimizuta 27:79b4b932a6dd 102 {
shimizuta 27:79b4b932a6dd 103 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 104 SetOneOrbit(i, orbit);
shimizuta 27:79b4b932a6dd 105 }
shimizuta 32:dc684a0b8448 106 void Walk::SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s)
shimizuta 29:7d8b8011a88d 107 {
shimizuta 29:7d8b8011a88d 108 Orbit triangle(TRIANGLE);
shimizuta 32:dc684a0b8448 109 triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0, 0);
shimizuta 29:7d8b8011a88d 110 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 111 }
shimizuta 32:dc684a0b8448 112 void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
shimizuta 27:79b4b932a6dd 113 {
shimizuta 32:dc684a0b8448 114 for (int i = 0; i < 4; i++)
shimizuta 29:7d8b8011a88d 115 SetOneLegStandParam(i, x_m, y_m, time_s);
shimizuta 27:79b4b932a6dd 116 }
shimizuta 32:dc684a0b8448 117 void Walk::SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 32:dc684a0b8448 118 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 119 {
shimizuta 29:7d8b8011a88d 120 Orbit triangle(TRIANGLE);
shimizuta 32:dc684a0b8448 121 triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 122 stridetime_s, toptime_s, buffer_time_s);
shimizuta 29:7d8b8011a88d 123 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 124 }
shimizuta 32:dc684a0b8448 125 void Walk::SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 126 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 27:79b4b932a6dd 127 {
shimizuta 27:79b4b932a6dd 128 for (int i = 0; i < 4; i++)
shimizuta 32:dc684a0b8448 129 SetOneLegTriangleParam(i, offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 130 stridetime_s, toptime_s, buffer_time_s);
shimizuta 27:79b4b932a6dd 131 }
shimizuta 29:7d8b8011a88d 132
shimizuta 32:dc684a0b8448 133 void Walk::SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 134 {
shimizuta 32:dc684a0b8448 135 Orbit freeline(FREELINES);
shimizuta 32:dc684a0b8448 136 freeline.SetFreeLinesParam(lineparams, point_num);
shimizuta 32:dc684a0b8448 137 SetOneOrbit(legnum, freeline);
shimizuta 32:dc684a0b8448 138 }
shimizuta 32:dc684a0b8448 139 void Walk::SetAllLegFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 140 {
shimizuta 32:dc684a0b8448 141 for (int i = 0; i < 4; i++)
shimizuta 32:dc684a0b8448 142 SetOneLegFreeLinesParam(i, lineparams, point_num);
shimizuta 32:dc684a0b8448 143 }
shimizuta 32:dc684a0b8448 144 void Walk::ChangeOneParam(int legnum, TriangleParams param, float val)
shimizuta 32:dc684a0b8448 145 {
shimizuta 32:dc684a0b8448 146 orbit_[legnum].ChangeOneParam(param, val);
shimizuta 32:dc684a0b8448 147 }
shimizuta 32:dc684a0b8448 148 void TriangleOrbit::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 149 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 150 {
shimizuta 29:7d8b8011a88d 151 reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
shimizuta 32:dc684a0b8448 152 offset_x_m_ = offset_x_m;
shimizuta 32:dc684a0b8448 153 offset_y_m_ = offset_y_m;
shimizuta 29:7d8b8011a88d 154 stride_m_ = stride_m;
shimizuta 29:7d8b8011a88d 155 height_m_ = height_m; //足上げ幅
shimizuta 29:7d8b8011a88d 156 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
shimizuta 29:7d8b8011a88d 157 stridetime_s_ = stridetime_s;
shimizuta 29:7d8b8011a88d 158 toptime_s_ = toptime_s; //頂点に行くまでの時間
shimizuta 29:7d8b8011a88d 159 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
shimizuta 29:7d8b8011a88d 160
shimizuta 29:7d8b8011a88d 161 //事前に計算しておく
shimizuta 32:dc684a0b8448 162 top_x_m_ = offset_x_m_ + stride_m * 0.5 - height_m * reverse_tanbeta_;
shimizuta 32:dc684a0b8448 163 top_y_m_ = -height_m_ + offset_y_m_;
shimizuta 32:dc684a0b8448 164 buffer_x_m_ = offset_x_m_ + stride_m * 0.5 - buffer_height_m_ * reverse_tanbeta_;
shimizuta 32:dc684a0b8448 165 buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
shimizuta 29:7d8b8011a88d 166 }
shimizuta 29:7d8b8011a88d 167 //足一周の時間
shimizuta 29:7d8b8011a88d 168 float TriangleOrbit::GetOneWalkTime()
shimizuta 29:7d8b8011a88d 169 {
shimizuta 29:7d8b8011a88d 170 return stridetime_s_ + toptime_s_ + buffer_time_s_;
shimizuta 29:7d8b8011a88d 171 };
shimizuta 29:7d8b8011a88d 172 int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 173 {
shimizuta 29:7d8b8011a88d 174 int ret = 0;
shimizuta 29:7d8b8011a88d 175 if (phasetime_s < stridetime_s_)
shimizuta 29:7d8b8011a88d 176 ret = StrideLineAccel_(leg, phasetime_s);
shimizuta 29:7d8b8011a88d 177 else if (phasetime_s < stridetime_s_ + toptime_s_)
shimizuta 29:7d8b8011a88d 178 ret = leg.SetXY_m(top_x_m_, top_y_m_);
shimizuta 29:7d8b8011a88d 179 else
shimizuta 29:7d8b8011a88d 180 ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
shimizuta 29:7d8b8011a88d 181 return ret;
shimizuta 29:7d8b8011a88d 182 };
shimizuta 29:7d8b8011a88d 183 int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 184 {
shimizuta 32:dc684a0b8448 185 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
shimizuta 32:dc684a0b8448 186 float y_m = offset_y_m_;
shimizuta 29:7d8b8011a88d 187 return leg.SetXY_m(x_m, y_m);
shimizuta 27:79b4b932a6dd 188 }
shimizuta 29:7d8b8011a88d 189 int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 190 {
shimizuta 29:7d8b8011a88d 191 ///////////x,yを計算
shimizuta 32:dc684a0b8448 192 float s0 = stride_m_ * 0.5 + offset_x_m_;
shimizuta 32:dc684a0b8448 193 float s1 = offset_x_m_ -stride_m_ * 0.5;
shimizuta 32:dc684a0b8448 194 float g_h = sqrtf(kGravity / offset_y_m_);
shimizuta 29:7d8b8011a88d 195 float t = phasetime_s / stridetime_s_;
shimizuta 29:7d8b8011a88d 196 float denominator = expf(g_h) - expf(-g_h); //分母
shimizuta 29:7d8b8011a88d 197
shimizuta 29:7d8b8011a88d 198 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
shimizuta 32:dc684a0b8448 199 float y_m = offset_y_m_;
shimizuta 29:7d8b8011a88d 200 //x,yを代入
shimizuta 29:7d8b8011a88d 201 return leg.SetXY_m(x_m, y_m);
shimizuta 32:dc684a0b8448 202 }
shimizuta 32:dc684a0b8448 203 void TriangleOrbit::ChangeOneParam(TriangleParams param, float val)
shimizuta 32:dc684a0b8448 204 {
shimizuta 32:dc684a0b8448 205 switch (param)
shimizuta 32:dc684a0b8448 206 {
shimizuta 32:dc684a0b8448 207 case OFFSET_X_M:
shimizuta 32:dc684a0b8448 208 offset_x_m_ = val;
shimizuta 32:dc684a0b8448 209 break;
shimizuta 32:dc684a0b8448 210 case OFFSET_Y_M:
shimizuta 32:dc684a0b8448 211 offset_y_m_ = val;
shimizuta 32:dc684a0b8448 212 break;
shimizuta 32:dc684a0b8448 213 case STRIDE_M:
shimizuta 32:dc684a0b8448 214 stride_m_ = val;
shimizuta 32:dc684a0b8448 215 break;
shimizuta 32:dc684a0b8448 216 case HEIGHT_M:
shimizuta 32:dc684a0b8448 217 height_m_ = val;
shimizuta 32:dc684a0b8448 218 break;
shimizuta 32:dc684a0b8448 219 case BUFFER_HEIGHT_M:
shimizuta 32:dc684a0b8448 220 buffer_height_m_ = val;
shimizuta 32:dc684a0b8448 221 break;
shimizuta 32:dc684a0b8448 222 }
shimizuta 32:dc684a0b8448 223 }
shimizuta 32:dc684a0b8448 224
shimizuta 32:dc684a0b8448 225 void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 226 {
shimizuta 32:dc684a0b8448 227 point_num_ = point_num;
shimizuta 32:dc684a0b8448 228 if (point_num_ == 1)
shimizuta 32:dc684a0b8448 229 {
shimizuta 32:dc684a0b8448 230 printf("error:point_num = 1. You should put >=2 to make line.");
shimizuta 32:dc684a0b8448 231 while (1)
shimizuta 32:dc684a0b8448 232 ;
shimizuta 32:dc684a0b8448 233 }
shimizuta 32:dc684a0b8448 234 for (int i = 0; i < point_num_; i++)
shimizuta 32:dc684a0b8448 235 lineparams_[i] = lineparams[i];
shimizuta 32:dc684a0b8448 236 }
shimizuta 32:dc684a0b8448 237 int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 32:dc684a0b8448 238 {
shimizuta 32:dc684a0b8448 239 ;
shimizuta 32:dc684a0b8448 240 //現在の最終到達pointを決定
shimizuta 32:dc684a0b8448 241 int arrived_point;
shimizuta 32:dc684a0b8448 242 float sum_time = 0;
shimizuta 32:dc684a0b8448 243 for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num - 1(これが配列の最大index)の状態で終わらないように-2している
shimizuta 32:dc684a0b8448 244 {
shimizuta 32:dc684a0b8448 245 sum_time += lineparams_[arrived_point].time_s;
shimizuta 32:dc684a0b8448 246 if (phasetime_s < sum_time)
shimizuta 32:dc684a0b8448 247 break;
shimizuta 32:dc684a0b8448 248 }
shimizuta 32:dc684a0b8448 249 //x,yを計算
shimizuta 32:dc684a0b8448 250 float x_m, y_m;
shimizuta 32:dc684a0b8448 251 x_m = lineparams_[arrived_point].x_m +
shimizuta 32:dc684a0b8448 252 (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) *
shimizuta 32:dc684a0b8448 253 (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s;
shimizuta 32:dc684a0b8448 254 y_m = lineparams_[arrived_point].y_m +
shimizuta 32:dc684a0b8448 255 (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) *
shimizuta 32:dc684a0b8448 256 (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s;
shimizuta 32:dc684a0b8448 257 return leg.SetXY_m(x_m, y_m);
shimizuta 32:dc684a0b8448 258 }
shimizuta 32:dc684a0b8448 259 float FreeLineOrbit::GetOneWalkTime() //足一周の時間
shimizuta 32:dc684a0b8448 260 {
shimizuta 32:dc684a0b8448 261 float sum_time = 0;
shimizuta 32:dc684a0b8448 262 for (int i = 0; i < point_num_; i++)
shimizuta 32:dc684a0b8448 263 sum_time += lineparams_[i].time_s;
shimizuta 32:dc684a0b8448 264 return sum_time;
shimizuta 32:dc684a0b8448 265 }
shimizuta 32:dc684a0b8448 266 void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
shimizuta 32:dc684a0b8448 267 {
shimizuta 32:dc684a0b8448 268 point_num_ = 2;
shimizuta 32:dc684a0b8448 269 lineparams_[0].x_m = x_m;
shimizuta 32:dc684a0b8448 270 lineparams_[0].y_m = y_m;
shimizuta 32:dc684a0b8448 271 lineparams_[0].time_s = time_s;
shimizuta 32:dc684a0b8448 272 lineparams_[1].x_m = x_m;
shimizuta 32:dc684a0b8448 273 lineparams_[1].y_m = y_m;
shimizuta 32:dc684a0b8448 274 lineparams_[1].time_s = 0;
shimizuta 32:dc684a0b8448 275 }