test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
00001 /* NHK2019MR2 馬型機構プログラム. 00002 //基本プログラム 00003 SetWalk(walk, 歩行パターン);//歩行パターン設定 00004 MoveOneCycle(walk, leg);//1回呼び出すと一サイクル分進む。2歩以上進みたいときはforで回して回数分呼び出す 00005 00006 で動く。 00007 SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。 00008 ROSありだと、ROSからのstateに従いswitchで切り替えている。 00009 */ 00010 #include "pi.h" 00011 #include <math.h> 00012 #include <stdio.h> 00013 #ifndef VSCODE 00014 #include "mbed.h" 00015 #include "pinnames.h" 00016 #include "can.h" 00017 //#define USE_ROS //ROSを使うときはコメントアウトを外す 00018 #include "ros.h" 00019 #include "geometry_msgs/Vector3.h" 00020 #include "std_msgs/Int16.h" 00021 #endif 00022 //#define DEBUG_ON //デバッグ用。使わないときはコメントアウト 00023 #include "debug.h" 00024 #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 00025 #include "Walk.h" //歩き方に関するファイル 00026 #include "OverCome.h" 00027 #include "change_walk.h" 00028 #include "servo_and_movefunc.h" 00029 enum ROS_STATE 00030 { 00031 STOP, 00032 AREA1_LRFWALK_STATE, 00033 SANDDUNE, 00034 ROPE_STATE, 00035 SLOPE_STATE, 00036 }; 00037 ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる 00038 #ifdef USE_ROS 00039 ros::NodeHandle nh_mbed; 00040 void callback(const geometry_msgs::Vector3 &cmd_vel); 00041 void callback_state(const std_msgs::Int16 &cmd_vel); 00042 geometry_msgs::Vector3 back_vel; 00043 ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback); 00044 ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state); 00045 ros::Publisher pub_vel("/back_vel", &back_vel); 00046 #endif 00047 ////////////あまり変化させないパラメーター 00048 const float kBetweenServoHalf_m = 0.034 * 0.5; //サーボ間の距離の半分 00049 float kLegLength1[2] = {0.15, 0.15}; 00050 float kLegLength2[2] = {0.33, 0.34}; 00051 /////////////// 00052 ////////調整するべきパラメータ 00053 enum WalkWay //歩行軌道 00054 { 00055 STANDUP, //受け渡し用に待つ 00056 AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE 00057 AREA2_LRFWALK, //段差後からLRFありで歩く 00058 STANDUP_SANDDUNE, //段差前で立つ 00059 FRONTLEG_ON_SANDDUNE, //前足を段差にかける 00060 OVERCOME, //前足が乗った状態で進む 00061 FALL_FRONTLEG, //前足を段差から降ろす 00062 BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる 00063 OVERCOME2, //後ろ脚が乗った状態で進む 00064 AFTER_OVERCOME, //段差を終了したら座って指示まち 00065 WALK_BEFORE_ROPE, //紐前で歩く 00066 ROPE, //rope前足超える 00067 ROPE_BACK, //rope後ろ足超える 00068 SLOPE, 00069 TRUNRIGHT, 00070 TRUNLEFT, 00071 CHECK, //check用に最後に置いておく 00072 }; 00073 //LRF使うやつだけだけspinOnceで変更するためグローバルで 00074 float stride_m[4] = {0.2, 0.2, 0.2, 0.2}; //ropeのときは0.15だった 00075 //各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。 00076 //param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。 00077 int SetWalk(Walk &walk, WalkWay way) 00078 { 00079 //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。 00080 //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。 00081 float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3}; 00082 float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27}; 00083 float offset_x_m[4] = {0, 0, 0, 0}; 00084 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02, 00085 stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1; 00086 00087 //段差 00088 float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38}; 00089 float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27}; 00090 float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38}; 00091 float d_time = 0.2, d_time_slow = 0.3; 00092 //旋回 00093 float turn_start_x_m = 0, turn_start_y_m = 0.275, 00094 turn_stride_m = 0.075, turn_height_m = 0.05, 00095 turn_stridetime_s = 0.5, turn_risetime_s = 0.2; 00096 float d_time_rope = 0.6, d_time_slow_rope = 0.3; 00097 switch (way) 00098 { 00099 case STANDUP: 00100 { //受け渡し用に待つ 00101 float offset_x_m[4] = {}, 00102 offset_y_m[4] = {0.3, 0.3, 0.3, 0.3}; 00103 for (int i = 0; i < 4; i++) 00104 SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5); 00105 walk.SetOffsetTime(0, 0, 0, 0); 00106 break; 00107 } 00108 case AREA1_LRFWALK: //LRF用に変数はグローバルにある 00109 for (int i = 0; i < 4; i++) 00110 SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 00111 stride_m[i], height_m[i], buffer_height_m, 00112 stridetime_s, toptime_s, buffer_time_s); 00113 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00114 break; 00115 case AREA2_LRFWALK: 00116 for (int i = 0; i < 4; i++) 00117 SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_offset_y_m[i], 00118 stride_m[i], height_m[i], buffer_height_m, 00119 stridetime_s, toptime_s, buffer_time_s); 00120 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00121 break; 00122 case STANDUP_SANDDUNE: 00123 { 00124 for (int i = 0; i < 4; i++) 00125 { 00126 LineParam lines[] = { 00127 {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0}, 00128 {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0}, 00129 {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0}, 00130 }; 00131 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0])); 00132 } 00133 walk.SetOffsetTime(0, 0, 0, 0); 00134 break; 00135 } 00136 case FRONTLEG_ON_SANDDUNE: 00137 { //前足を段差にかける 00138 float d_x_m = 0.1; 00139 float raise_offset_x_m[4] = {0, -0.02, 0, 0}; 00140 float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15}; 00141 float gravity_dist[] = {0.05, 0, 0.05, -0.05}; 00142 OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m, 00143 overcome_height_m, gravity_dist, walk.leg, 00144 raise_offset_x_m, d_time, d_time_slow); 00145 walk.Copy(overcome.walk); 00146 break; 00147 } 00148 case OVERCOME: 00149 { //前足が乗った状態で進む 00150 float stride_m[4] = {0.07, 0.07, 0.07, 0.07}; 00151 for (int i = 0; i < 4; i++) 00152 SetOneLegFourPointParam(walk, i, offset_x_m[i], flontleg_sand_goal_y_m[i], 00153 stride_m[i], height_m[i], buffer_height_m, 00154 stridetime_s, toptime_s, buffer_time_s); 00155 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00156 break; 00157 } 00158 case FALL_FRONTLEG: //前足を段差から降ろす 00159 { 00160 float d_x_m = 0.15; 00161 float start_x_m[4] = {0, 0, 0, 0}; 00162 float raise_offset_x_m[4] = {}; 00163 float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05}; 00164 float gravity_dist[] = {0.1, 0, 0.1, 0}; 00165 OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m, 00166 overcome_height_m, gravity_dist, walk.leg, 00167 raise_offset_x_m, d_time, d_time_slow); 00168 walk.Copy(overcome.walk); 00169 break; 00170 } 00171 case BACKLEG_ON_SANDDUNE: 00172 { //後ろ脚を乗せる 00173 float d_x_m = 0.15; 00174 float start_x_m[4] = {0, 0, 0, 0}; 00175 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; 00176 00177 float raise_offset_x_m[4] = {}; 00178 float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1}; 00179 float gravity_dist[] = {0.1, 0, 0.1, 0}; 00180 OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m, 00181 overcome_height_m, gravity_dist, walk.leg, 00182 raise_offset_x_m, d_time, d_time_slow); 00183 walk.Copy(overcome.walk); 00184 break; 00185 } 00186 case OVERCOME2: 00187 { //後ろ脚が乗った状態で進む 00188 float offset_x_m[4] = {0.1, 0, 0.1, 0}; 00189 float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33}; 00190 float stride_m[4] = {0.2, 0.2, 0.2, 0.2}, 00191 height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05, 00192 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2; 00193 for (int i = 0; i < 4; i++) 00194 SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i], 00195 stride_m[i], height_m[i], buffer_height_m, 00196 stridetime_s, toptime_s, buffer_time_s); 00197 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00198 break; 00199 } 00200 case AFTER_OVERCOME: 00201 { 00202 for (int i = 0; i < 4; i++) 00203 SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5); 00204 walk.SetOffsetTime(0, 0, 0, 0); 00205 break; 00206 } 00207 case WALK_BEFORE_ROPE: 00208 { 00209 // float stride_m[] = {0.2, 0.2, 0.125, 0.125}; 00210 float stride_m[] = {0.075, 0.075, 0.2, 0.2}; 00211 for (int i = 0; i < 4; i++) 00212 SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 00213 stride_m[i], height_m[i], buffer_height_m, 00214 stridetime_s, toptime_s, buffer_time_s); 00215 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00216 break; 00217 } 00218 case ROPE: 00219 { 00220 float offset_x_m[4] = {-0.05, 0, -0.05, 0}; 00221 float d_x_m = 0.2; 00222 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; 00223 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41}; 00224 float raise_offset_x_m[] = {0, -0.15, 0, -0.15}; 00225 float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2}; 00226 float gravity_dist[] = {0.05, -0.05, 0.05, -0.05}; 00227 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, 00228 overcome_height_m, gravity_dist, walk.leg, 00229 raise_offset_x_m, d_time_rope, d_time_slow_rope); 00230 walk.Copy(overcome.walk); 00231 break; 00232 } 00233 case ROPE_BACK: 00234 { 00235 float offset_x_m[4] = {}; 00236 float d_x_m = 0.2; 00237 float start_y_m[4] = {0.41, 0.38, 0.41, 0.38}; 00238 float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38}; 00239 float raise_offset_x_m[] = {-0.1, 0, -0.1, 0}; 00240 float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05}; 00241 float gravity_dist[] = {0.25, -0.05, 0.05, -0.05}; 00242 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, 00243 overcome_height_m, gravity_dist, walk.leg, 00244 raise_offset_x_m, d_time_rope, d_time_slow_rope); 00245 walk.Copy(overcome.walk); 00246 break; 00247 } 00248 case SLOPE: 00249 { 00250 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, 00251 offset_y_m[4] = {0.32, 0.22, 0.32, 0.22}; 00252 float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15 00253 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02, 00254 stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05; 00255 for (int i = 0; i < 4; i++) 00256 SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i], 00257 stride_m[i], height_m[i], buffer_height_m, 00258 stridetime_s, toptime_s, buffer_time_s); 00259 walk.SetOffsetTime(0, 0.5, 0.5, 0); 00260 break; 00261 } 00262 case TRUNRIGHT: 00263 Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m, 00264 turn_height_m, turn_stridetime_s, turn_risetime_s); 00265 break; 00266 case TRUNLEFT: 00267 Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m, 00268 turn_height_m, turn_stridetime_s, turn_risetime_s); 00269 break; 00270 default: 00271 printf("error: there is no WalkWay\r\n"); 00272 return 1; //以上終了 00273 } 00274 walk.ResetPhase(); 00275 return 0; //正常終了 00276 } 00277 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s); 00278 int main() 00279 { 00280 printf("program start\r\n"); 00281 #ifdef VSCODE 00282 if (FileOpen()) //csv fileに書き込み 00283 return 1; //異常なら強制終了 00284 #endif 00285 00286 /////足の長さ、計算周期の設定 00287 OneLeg leg[4]; //各足の位置 00288 for (int i = 0; i < 4; i++) 00289 leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2); 00290 Walk walk(leg); //歩行法はここに入れていく 00291 Walk::calctime_s_ = 0.03; 00292 /////事前に軌道を全チェック 00293 for (int i = 0; i < CHECK; i++) 00294 { 00295 SetWalk(walk, (WalkWay)i); 00296 if (walk.CheckOrbit() == 1) 00297 { 00298 printf("error: move %d\r\n", i); 00299 return 1; //強制終了.errorは内部の関数からprintfで知らせる 00300 } 00301 } 00302 //軌道チェックしてからROS完全起動 00303 #ifdef USE_ROS 00304 nh_mbed.getHardware()->setBaud(115200); 00305 nh_mbed.initNode(); 00306 nh_mbed.subscribe(sub_vel); 00307 nh_mbed.subscribe(sub_state); 00308 nh_mbed.advertise(pub_vel); 00309 nh_mbed.spinOnce(); //一度受信 00310 #endif 00311 00312 /////立つ 00313 state_from_ros = ROPE_STATE; 00314 switch (state_from_ros) 00315 { 00316 case SANDDUNE: 00317 SetWalk(walk, STANDUP_SANDDUNE); 00318 break; 00319 default: 00320 SetWalk(walk, STANDUP); 00321 break; 00322 } 00323 printf("Stand up?\r\n"); 00324 WaitStdin('y'); // キーボード入力されるまでまで待つ 00325 MoveOneCycle(walk, leg); 00326 printf("Move?\r\n"); 00327 WaitStdin('y'); 00328 //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける 00329 switch (state_from_ros) 00330 { 00331 case STOP: 00332 break; 00333 case AREA1_LRFWALK_STATE: //キーボード入力されるまでまで待つ 00334 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え 00335 for (int i = 0; i < 20; i++) 00336 MoveOneCycle(walk, leg); 00337 break; 00338 case SANDDUNE: 00339 //前足を段差にかける //キーボード入力されるまでまで待つ 00340 SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ 00341 MoveOneCycle(walk, leg); 00342 //前足が段差に乗った状態で進む 00343 SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ 00344 for (int i = 0; i < 2; i++) 00345 MoveOneCycle(walk, leg); 00346 //前足を降ろす 00347 SmoothChange(walk, leg, FALL_FRONTLEG, 0.5); 00348 MoveOneCycle(walk, leg); 00349 //またいだ状態で歩く 00350 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); 00351 MoveOneCycle(walk, leg); 00352 //後ろ脚載せる 00353 SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ 00354 MoveOneCycle(walk, leg); 00355 //後ろ脚乗った状態で進む 00356 SetWalk(walk, OVERCOME2); 00357 for (int i = 0; i < 3; i++) 00358 MoveOneCycle(walk, leg); 00359 //普通に進む 00360 SetWalk(walk, AREA1_LRFWALK); 00361 for (int i = 0; i < 4; i++) 00362 MoveOneCycle(walk, leg); 00363 break; 00364 00365 case ROPE_STATE: 00366 //紐前歩く 00367 SetWalk(walk, WALK_BEFORE_ROPE); 00368 for (int i = 0; i < 5; i++) 00369 MoveOneCycle(walk, leg); 00370 int is_arrived = 0; 00371 while (is_arrived == 0) 00372 { 00373 MoveOneCycle(walk, leg); 00374 unsigned int dist_cm = GetDist_cm(); 00375 if (dist_cm > 90 && dist_cm < 150) 00376 ++is_arrived; 00377 printf("%d\r\n", dist_cm); 00378 } 00379 /* 00380 SetWalk(walk, AREA1_LRFWALK); 00381 for (int i = 0; i < 5; i++) 00382 MoveOneCycle(walk, leg); 00383 SmoothChange(walk, leg, TRUNRIGHT, 0.5); 00384 for (int i = 0; i < 2; i++) 00385 MoveOneCycle(walk, leg); 00386 */ 00387 /*曲がりながら歩く 00388 SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5); 00389 for (int i = 0; i < 7; i++) 00390 MoveOneCycle(walk, leg); 00391 */ 00392 //前足だけ紐越え 00393 SmoothChange(walk, leg, ROPE, 0.5); 00394 MoveOneCycle(walk, leg); 00395 //LRFが取れる高さで歩行 00396 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); 00397 for (int i = 0; i < 3; i++) 00398 MoveOneCycle(walk, leg); 00399 //後ろ足越え 00400 SmoothChange(walk, leg, ROPE_BACK, 0.5); 00401 MoveOneCycle(walk, leg); 00402 //紐間歩く 00403 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); 00404 for (int i = 0; i < 1; i++) 00405 MoveOneCycle(walk, leg); 00406 //2本目 00407 //前足だけ紐越え 00408 SmoothChange(walk, leg, ROPE, 0.5); 00409 MoveOneCycle(walk, leg); 00410 //LRFが取れる高さで歩行 00411 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); 00412 for (int i = 0; i < 3; i++) 00413 MoveOneCycle(walk, leg); 00414 //後ろ足越え 00415 SmoothChange(walk, leg, ROPE_BACK, 0.5); 00416 MoveOneCycle(walk, leg); 00417 //紐後歩く 00418 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); 00419 for (int i = 0; i < 10; i++) 00420 MoveOneCycle(walk, leg); 00421 break; 00422 00423 case SLOPE_STATE: 00424 SmoothChange(walk, leg, SLOPE, 0.5); 00425 for (int i = 0; i < 30; i++) 00426 MoveOneCycle(walk, leg); 00427 break; 00428 } 00429 #ifdef USE_ROS 00430 nh_mbed.spinOnce(); 00431 #endif 00432 printf("program end\r\n"); 00433 #ifdef VSCODE 00434 fclose(fp); 00435 #endif 00436 } 00437 #ifdef USE_ROS 00438 void callback(const geometry_msgs::Vector3 &cmd_vel) 00439 { 00440 if (state_from_ros == STOP) 00441 state_from_ros = AREA1_LRFWALK_STATE; 00442 stride_m[LEFT_F] = cmd_vel.x; 00443 stride_m[LEFT_B] = cmd_vel.x; 00444 stride_m[RIGHT_F] = cmd_vel.y; 00445 stride_m[RIGHT_B] = cmd_vel.y; 00446 back_vel = cmd_vel; 00447 pub_vel.publish(&back_vel); 00448 } 00449 void callback_state(const std_msgs::Int16 &cmd) 00450 { 00451 state_from_ros = (ROS_STATE)cmd.data; 00452 led[state_from_ros % 4] = 1; 00453 } 00454 #endif 00455 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s) 00456 { 00457 LineParam lines[4][2]; 00458 //前回位置を保存 00459 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++) 00460 { 00461 lines[i][0].time_s = 0; 00462 lines[i][0].x_m = leg[i].GetX_m(); 00463 lines[i][0].y_m = leg[i].GetY_m(); 00464 } 00465 Walk tempwalk(leg); 00466 SetWalk(tempwalk, nextway); 00467 tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む 00468 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++) 00469 { 00470 lines[i][1].time_s = time_s; 00471 lines[i][1].x_m = tempwalk.leg[i].GetX_m(); 00472 lines[i][1].y_m = tempwalk.leg[i].GetY_m(); 00473 lines[i][1].is_point_to_point = 0; 00474 } 00475 //前回終点から次回始点まで直線移動 00476 for (int i = 0; i < 4; i++) 00477 SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0])); 00478 tempwalk.ResetPhase(); 00479 MoveOneCycle(tempwalk, leg); 00480 SetWalk(walk, nextway); 00481 };
Generated on Thu Jul 14 2022 13:15:26 by 1.7.2