2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
main.cpp@5:f225e0c61cfc, 2022-02-24 (annotated)
- Committer:
- Kotttaro
- Date:
- Thu Feb 24 06:12:53 2022 +0000
- Revision:
- 5:f225e0c61cfc
- Parent:
- 4:8a50c7822dac
2022.02.24
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kotttaro | 4:8a50c7822dac | 1 | //特別研究Ⅰで用いたプログラムを改良したもの |
Kotttaro | 0:4a5272e014d8 | 2 | //ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する |
Kotttaro | 2:57237f0a4a34 | 3 | //brent法の部分は『numerical recipes in c』 参照 |
Kotttaro | 4:8a50c7822dac | 4 | #include "mbed.h" |
Kotttaro | 4:8a50c7822dac | 5 | #include <PCA9685.h> |
Kotttaro | 0:4a5272e014d8 | 6 | #include "Eigen/Geometry.h" |
Kotttaro | 0:4a5272e014d8 | 7 | #include "Eigen/Dense.h" |
Kotttaro | 0:4a5272e014d8 | 8 | #include <math.h> |
Kotttaro | 4:8a50c7822dac | 9 | |
Kotttaro | 2:57237f0a4a34 | 10 | #define ITMAX 100 |
Kotttaro | 2:57237f0a4a34 | 11 | #define CGOLD 0.3819660 |
Kotttaro | 2:57237f0a4a34 | 12 | #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); |
Kotttaro | 2:57237f0a4a34 | 13 | #define ZEPS 1.0e-10 |
Kotttaro | 4:8a50c7822dac | 14 | #define GOLD 1.618034 |
Kotttaro | 4:8a50c7822dac | 15 | #define TINY 1.0e-20 |
Kotttaro | 4:8a50c7822dac | 16 | #define GLIMIT 100.0 |
Kotttaro | 4:8a50c7822dac | 17 | #define SERVOMIN 700 |
Kotttaro | 4:8a50c7822dac | 18 | #define SERVOMAX 2300 |
Kotttaro | 4:8a50c7822dac | 19 | #define SERVOGAIN 29.6296300 |
Kotttaro | 4:8a50c7822dac | 20 | #define PI 3.14159265358979323846264338327950288 |
Kotttaro | 2:57237f0a4a34 | 21 | |
Kotttaro | 4:8a50c7822dac | 22 | PCA9685 pwm;//クラス宣言 |
Kotttaro | 0:4a5272e014d8 | 23 | Serial pc2(USBTX,USBRX); |
Kotttaro | 0:4a5272e014d8 | 24 | Timer tim; |
Kotttaro | 4:8a50c7822dac | 25 | Timer manage; |
Kotttaro | 5:f225e0c61cfc | 26 | int times= 200;//実行回数:実行時間は (sampling)*(times)秒 |
Kotttaro | 0:4a5272e014d8 | 27 | |
Kotttaro | 0:4a5272e014d8 | 28 | using namespace Eigen; |
Kotttaro | 0:4a5272e014d8 | 29 | |
Kotttaro | 0:4a5272e014d8 | 30 | //以下変数定義 |
Kotttaro | 4:8a50c7822dac | 31 | |
Kotttaro | 4:8a50c7822dac | 32 | //brent法に必要な変数 |
Kotttaro | 5:f225e0c61cfc | 33 | double ax=-0.1*PI/180.0,bx=0.1*PI/180.0,cx=0.0; |
Kotttaro | 4:8a50c7822dac | 34 | double fa,fb,fc; |
Kotttaro | 4:8a50c7822dac | 35 | //サーボの書き込みに必要な変数 |
Kotttaro | 5:f225e0c61cfc | 36 | double servo0[16]={7700.0, 5850.0, 7050.0, 6850.0, 6000.0, 6300.0, 8400.0, 7200.0, 6700.0, 7000.0, 5650.0, 8400.0, 6000.0, 5100.0, 5600.0, 6570.0};//servoの初期値 |
Kotttaro | 4:8a50c7822dac | 37 | int ch[4][4]={{0 ,1 ,2 ,3} , |
Kotttaro | 4:8a50c7822dac | 38 | {4 ,5 ,6 ,7} , |
Kotttaro | 4:8a50c7822dac | 39 | {8 ,9 ,10,11}, |
Kotttaro | 4:8a50c7822dac | 40 | {12,13,14,15} }; |
Kotttaro | 5:f225e0c61cfc | 41 | double r=0.0*PI/180;//斜面の傾き[°] |
Kotttaro | 5:f225e0c61cfc | 42 | double sampling=0.01;//δtの時間[s] |
Kotttaro | 5:f225e0c61cfc | 43 | double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる |
Kotttaro | 0:4a5272e014d8 | 44 | double tip[4][3];//足先座標 |
Kotttaro | 5:f225e0c61cfc | 45 | double con[4][3] = { {-55.0, 55.0,0.0}, |
Kotttaro | 5:f225e0c61cfc | 46 | { 55.0, 55.0,0.0}, |
Kotttaro | 5:f225e0c61cfc | 47 | { 55.0,-55.0,0.0}, |
Kotttaro | 5:f225e0c61cfc | 48 | {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0 |
Kotttaro | 5:f225e0c61cfc | 49 | double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, |
Kotttaro | 5:f225e0c61cfc | 50 | {45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, |
Kotttaro | 5:f225e0c61cfc | 51 | {-45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180}, |
Kotttaro | 5:f225e0c61cfc | 52 | {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態 |
Kotttaro | 0:4a5272e014d8 | 53 | double th0[4][4]= { 0.0,0.0,0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 54 | 0.0,0.0, 0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 55 | 0.0,0.0, 0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 56 | 0.0,0.0, 0.0,0.0 }; //計算用の関節角度 |
Kotttaro | 5:f225e0c61cfc | 57 | double th[4][4];//サーボに入力する角度 |
Kotttaro | 0:4a5272e014d8 | 58 | double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元 |
Kotttaro | 0:4a5272e014d8 | 59 | double a,a0, h,fi;//評価関数内の変数 fi=φ |
Kotttaro | 0:4a5272e014d8 | 60 | double X,tan_u, tan_d;//計算用 |
Kotttaro | 0:4a5272e014d8 | 61 | |
Kotttaro | 0:4a5272e014d8 | 62 | //ねじ軸 |
Kotttaro | 0:4a5272e014d8 | 63 | //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ |
Kotttaro | 5:f225e0c61cfc | 64 | double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件 |
Kotttaro | 0:4a5272e014d8 | 65 | double dfdth[4];//評価関数のナブラ |
Kotttaro | 0:4a5272e014d8 | 66 | |
Kotttaro | 0:4a5272e014d8 | 67 | |
Kotttaro | 0:4a5272e014d8 | 68 | //以下行列定義 |
Kotttaro | 0:4a5272e014d8 | 69 | MatrixXd Q(3, 3);//Q行列 |
Kotttaro | 0:4a5272e014d8 | 70 | MatrixXd R(3, 4);//R行列 |
Kotttaro | 0:4a5272e014d8 | 71 | Vector3d vP[4];//各脚の速度ベクトル |
Kotttaro | 0:4a5272e014d8 | 72 | |
Kotttaro | 0:4a5272e014d8 | 73 | |
Kotttaro | 0:4a5272e014d8 | 74 | void QR(int leg);//QR分解用関数,引数は脚番号 |
Kotttaro | 0:4a5272e014d8 | 75 | void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する |
Kotttaro | 0:4a5272e014d8 | 76 | void fwd(int leg);//順運動学より脚先の座標を導出する |
Kotttaro | 0:4a5272e014d8 | 77 | void Jac(int leg);//指定した脚のヤコビアンを計算 |
Kotttaro | 0:4a5272e014d8 | 78 | void deff(int leg);//評価関数計算, legは距離と傾きから指定する |
Kotttaro | 0:4a5272e014d8 | 79 | void dfd( int leg);//評価関数の勾配をとる |
Kotttaro | 0:4a5272e014d8 | 80 | double search(int leg);//最大のthetaを探索するための関数 |
Kotttaro | 0:4a5272e014d8 | 81 | void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す |
Kotttaro | 2:57237f0a4a34 | 82 | double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの |
Kotttaro | 2:57237f0a4a34 | 83 | double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数 |
Kotttaro | 3:f824e4d8eef7 | 84 | double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用 |
Kotttaro | 4:8a50c7822dac | 85 | void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数 |
Kotttaro | 3:f824e4d8eef7 | 86 | double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム |
Kotttaro | 4:8a50c7822dac | 87 | //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test) |
Kotttaro | 2:57237f0a4a34 | 88 | double SIGN(double x,double y);//xにyの符号をつけたものを返す |
Kotttaro | 4:8a50c7822dac | 89 | double FMAX(double x,double y);//大きいほうの値が返される |
Kotttaro | 4:8a50c7822dac | 90 | double f_test(double x);//テスト用の関数 |
Kotttaro | 4:8a50c7822dac | 91 | |
Kotttaro | 4:8a50c7822dac | 92 | //以下サーボ関係 |
Kotttaro | 4:8a50c7822dac | 93 | void setup_servo();//サーボセットアップ用関数 |
Kotttaro | 4:8a50c7822dac | 94 | void servo_write(int ch,double ang);//angに |
Kotttaro | 4:8a50c7822dac | 95 | void servo_write7(int ch, double ang); |
Kotttaro | 4:8a50c7822dac | 96 | void servo_calib();//全ての角度を0度にする |
Kotttaro | 5:f225e0c61cfc | 97 | void servo_ready();//初期状態 |
Kotttaro | 0:4a5272e014d8 | 98 | |
Kotttaro | 0:4a5272e014d8 | 99 | int main() |
Kotttaro | 0:4a5272e014d8 | 100 | { |
Kotttaro | 0:4a5272e014d8 | 101 | double t; |
Kotttaro | 0:4a5272e014d8 | 102 | pc2.baud(921600); |
Kotttaro | 5:f225e0c61cfc | 103 | //setup_servo(); |
Kotttaro | 5:f225e0c61cfc | 104 | for(int u=0; u<4; u++) |
Kotttaro | 5:f225e0c61cfc | 105 | { |
Kotttaro | 5:f225e0c61cfc | 106 | for(int i=0; i<4; i++) |
Kotttaro | 5:f225e0c61cfc | 107 | { |
Kotttaro | 5:f225e0c61cfc | 108 | th[u][i]=th_ready[u][i]; |
Kotttaro | 5:f225e0c61cfc | 109 | } |
Kotttaro | 5:f225e0c61cfc | 110 | } |
Kotttaro | 5:f225e0c61cfc | 111 | for(int i=0; i<4; i++) |
Kotttaro | 5:f225e0c61cfc | 112 | { |
Kotttaro | 5:f225e0c61cfc | 113 | fwd(i); |
Kotttaro | 5:f225e0c61cfc | 114 | } |
Kotttaro | 5:f225e0c61cfc | 115 | //servo_ready(); |
Kotttaro | 4:8a50c7822dac | 116 | wait(2); |
Kotttaro | 5:f225e0c61cfc | 117 | //while(1); |
Kotttaro | 5:f225e0c61cfc | 118 | /*for(int u=0; u<4; u++) |
Kotttaro | 4:8a50c7822dac | 119 | { |
Kotttaro | 4:8a50c7822dac | 120 | for(int i=0; i<4; i++) |
Kotttaro | 4:8a50c7822dac | 121 | { |
Kotttaro | 4:8a50c7822dac | 122 | servo_write(ch[u][i],th[u][i]); |
Kotttaro | 4:8a50c7822dac | 123 | } |
Kotttaro | 5:f225e0c61cfc | 124 | }*/ |
Kotttaro | 5:f225e0c61cfc | 125 | //while(1); |
Kotttaro | 4:8a50c7822dac | 126 | wait(2); |
Kotttaro | 0:4a5272e014d8 | 127 | int count = 0; |
Kotttaro | 0:4a5272e014d8 | 128 | //入力したねじ運動を換算する |
Kotttaro | 5:f225e0c61cfc | 129 | Lin[0] = cos(30.0*PI/180); //ねじ軸x |
Kotttaro | 5:f225e0c61cfc | 130 | Lin[1] = sin(30.0*PI/180); //ねじ軸y |
Kotttaro | 5:f225e0c61cfc | 131 | Lin[2] = 0.0;//ねじ軸z |
Kotttaro | 0:4a5272e014d8 | 132 | L0[0] = 0.0;//ねじ軸原点座標 |
Kotttaro | 0:4a5272e014d8 | 133 | L0[1] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 134 | L0[2] = 0.0; |
Kotttaro | 4:8a50c7822dac | 135 | vin = 0.0; |
Kotttaro | 5:f225e0c61cfc | 136 | win = 2.0; |
Kotttaro | 2:57237f0a4a34 | 137 | printf("\r\n\r\n"); |
Kotttaro | 0:4a5272e014d8 | 138 | nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]); |
Kotttaro | 0:4a5272e014d8 | 139 | for (int i = 0; i < 3; i++) |
Kotttaro | 0:4a5272e014d8 | 140 | { |
Kotttaro | 0:4a5272e014d8 | 141 | wg[i] = Lin[i] * win / nol; |
Kotttaro | 0:4a5272e014d8 | 142 | v[i] = Lin[i] * vin / nol; |
Kotttaro | 0:4a5272e014d8 | 143 | } |
Kotttaro | 0:4a5272e014d8 | 144 | //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0)); |
Kotttaro | 0:4a5272e014d8 | 145 | |
Kotttaro | 0:4a5272e014d8 | 146 | //times*δtの時間だけサーボを動かす |
Kotttaro | 0:4a5272e014d8 | 147 | tim.start(); |
Kotttaro | 5:f225e0c61cfc | 148 | |
Kotttaro | 4:8a50c7822dac | 149 | manage.start(); |
Kotttaro | 5:f225e0c61cfc | 150 | //for (int i = 0; i < times;i++){ |
Kotttaro | 5:f225e0c61cfc | 151 | while(tim.read()<=5.0) { |
Kotttaro | 5:f225e0c61cfc | 152 | int l=2; |
Kotttaro | 5:f225e0c61cfc | 153 | //count = count + 1; |
Kotttaro | 0:4a5272e014d8 | 154 | double dth; |
Kotttaro | 5:f225e0c61cfc | 155 | //for(int l=0;l<4;l++) |
Kotttaro | 5:f225e0c61cfc | 156 | //{ |
Kotttaro | 4:8a50c7822dac | 157 | //////////計算部///////////////// |
Kotttaro | 5:f225e0c61cfc | 158 | t=tim.read(); |
Kotttaro | 5:f225e0c61cfc | 159 | fwd(l); |
Kotttaro | 5:f225e0c61cfc | 160 | pc2.printf("%3.3lf %3.4lf %3.4lf %3.4lf\r\n",t,tip[l][0],tip[l][1],tip[l][2]); |
Kotttaro | 5:f225e0c61cfc | 161 | vp(l); |
Kotttaro | 5:f225e0c61cfc | 162 | Jac(l); |
Kotttaro | 5:f225e0c61cfc | 163 | QR(l); |
Kotttaro | 5:f225e0c61cfc | 164 | deff(l); |
Kotttaro | 5:f225e0c61cfc | 165 | dfd(l); |
Kotttaro | 5:f225e0c61cfc | 166 | ax=-0.15*PI/180.0;bx=0.15*PI/180.0;cx=0.0; |
Kotttaro | 5:f225e0c61cfc | 167 | //2:谷,1:山 |
Kotttaro | 5:f225e0c61cfc | 168 | mnbrak(l,2); |
Kotttaro | 5:f225e0c61cfc | 169 | dth=brent(l,ax,bx,cx,0.01,2); |
Kotttaro | 5:f225e0c61cfc | 170 | solve(dth, l, 1); |
Kotttaro | 4:8a50c7822dac | 171 | //////////////////////////////// |
Kotttaro | 5:f225e0c61cfc | 172 | ///* |
Kotttaro | 5:f225e0c61cfc | 173 | //} |
Kotttaro | 5:f225e0c61cfc | 174 | /*for(int u=0; u<4; u++) |
Kotttaro | 4:8a50c7822dac | 175 | { |
Kotttaro | 4:8a50c7822dac | 176 | for(int i=0; i<4; i++) |
Kotttaro | 4:8a50c7822dac | 177 | { |
Kotttaro | 4:8a50c7822dac | 178 | servo_write(ch[u][i],th[u][i]); |
Kotttaro | 4:8a50c7822dac | 179 | } |
Kotttaro | 5:f225e0c61cfc | 180 | }//*/ |
Kotttaro | 5:f225e0c61cfc | 181 | //t=tim.read(); |
Kotttaro | 5:f225e0c61cfc | 182 | //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][0]*180/PI, th[1][0]*180/PI , th[2][0]*180/PI , th[3][0]*180/PI ); |
Kotttaro | 5:f225e0c61cfc | 183 | //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][1]*180/PI, th[1][1]*180/PI , th[2][1]*180/PI , th[3][1]*180/PI ); |
Kotttaro | 5:f225e0c61cfc | 184 | //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][2]*180/PI, th[1][2]*180/PI , th[2][2]*180/PI , th[3][2]*180/PI ); |
Kotttaro | 5:f225e0c61cfc | 185 | //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][3]*180/PI, th[1][3]*180/PI , th[2][3]*180/PI , th[3][3]*180/PI ); |
Kotttaro | 5:f225e0c61cfc | 186 | //pc2.printf("%2.4lf:x( %3.3lf, %3.3lf, %3.3lf, %3.3lf )y( %3.3lf, %3.3lf, %3.3lf, %3.3lf )z( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r", |
Kotttaro | 5:f225e0c61cfc | 187 | //t,tip[0][0],tip[1][0],tip[2][0],tip[3][0],tip[0][1],tip[1][1],tip[2][1],tip[3][1],tip[0][2],tip[1][2],tip[2][2],,tip[3][2]); |
Kotttaro | 5:f225e0c61cfc | 188 | //pc2.printf("\r\n"); |
Kotttaro | 4:8a50c7822dac | 189 | while(1) |
Kotttaro | 4:8a50c7822dac | 190 | { |
Kotttaro | 4:8a50c7822dac | 191 | if(manage.read()>sampling)break; |
Kotttaro | 4:8a50c7822dac | 192 | } |
Kotttaro | 5:f225e0c61cfc | 193 | manage.reset(); |
Kotttaro | 4:8a50c7822dac | 194 | |
Kotttaro | 0:4a5272e014d8 | 195 | } |
Kotttaro | 1:5c2562adca7d | 196 | t=tim.read(); |
Kotttaro | 4:8a50c7822dac | 197 | wait(3); |
Kotttaro | 5:f225e0c61cfc | 198 | servo_ready(); |
Kotttaro | 0:4a5272e014d8 | 199 | return 0; // ソフトの終了 |
Kotttaro | 0:4a5272e014d8 | 200 | } |
Kotttaro | 0:4a5272e014d8 | 201 | |
Kotttaro | 0:4a5272e014d8 | 202 | void QR(int leg) { |
Kotttaro | 0:4a5272e014d8 | 203 | double s, t;//要素計算用 |
Kotttaro | 0:4a5272e014d8 | 204 | MatrixXd ma(3, 4), ma1(3, 4); |
Kotttaro | 0:4a5272e014d8 | 205 | |
Kotttaro | 0:4a5272e014d8 | 206 | ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3], |
Kotttaro | 0:4a5272e014d8 | 207 | Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3], |
Kotttaro | 0:4a5272e014d8 | 208 | Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3]; |
Kotttaro | 4:8a50c7822dac | 209 | |
Kotttaro | 0:4a5272e014d8 | 210 | //ハウスホルダー変換1回目 |
Kotttaro | 0:4a5272e014d8 | 211 | MatrixXd A1(3, 3); |
Kotttaro | 0:4a5272e014d8 | 212 | A1 << 1.0, 0.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 213 | 0.0, 1.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 214 | 0.0, 0.0, 1.0; |
Kotttaro | 4:8a50c7822dac | 215 | |
Kotttaro | 0:4a5272e014d8 | 216 | s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ |
Kotttaro | 4:8a50c7822dac | 217 | |
Kotttaro | 0:4a5272e014d8 | 218 | MatrixXd H1(3, 3);//1回目の行列 |
Kotttaro | 0:4a5272e014d8 | 219 | MatrixXd X11(3, 1), X12(1, 3); |
Kotttaro | 0:4a5272e014d8 | 220 | Vector3d a11, a12;//a11が変換前,a12が変換後 |
Kotttaro | 0:4a5272e014d8 | 221 | a11 << ma(0, 0), ma(1, 0), ma(2, 0); |
Kotttaro | 0:4a5272e014d8 | 222 | a12 << s, 0.0, 0.0; |
Kotttaro | 0:4a5272e014d8 | 223 | X11 = a11 - a12; |
Kotttaro | 0:4a5272e014d8 | 224 | X12 = X11.transpose(); |
Kotttaro | 0:4a5272e014d8 | 225 | t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0)); |
Kotttaro | 0:4a5272e014d8 | 226 | H1 = A1 - 2.0 * (X11 * X12) / (t * t); |
Kotttaro | 0:4a5272e014d8 | 227 | ma1 = H1 * ma; |
Kotttaro | 0:4a5272e014d8 | 228 | //2回目 |
Kotttaro | 0:4a5272e014d8 | 229 | MatrixXd H2(3, 3), A2(2, 2), h2(2, 2); |
Kotttaro | 0:4a5272e014d8 | 230 | A2 << 1.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 231 | 0.0, 1.0; |
Kotttaro | 0:4a5272e014d8 | 232 | Vector2d a21, a22; |
Kotttaro | 0:4a5272e014d8 | 233 | MatrixXd X21(2, 1), X22(1, 2); |
Kotttaro | 0:4a5272e014d8 | 234 | a21 << ma1(1, 1), ma1(2, 1); |
Kotttaro | 0:4a5272e014d8 | 235 | s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1)); |
Kotttaro | 0:4a5272e014d8 | 236 | a22 << s, 0; |
Kotttaro | 0:4a5272e014d8 | 237 | X21 = a21 - a22; |
Kotttaro | 0:4a5272e014d8 | 238 | X22 = X21.transpose(); |
Kotttaro | 0:4a5272e014d8 | 239 | t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0)); |
Kotttaro | 0:4a5272e014d8 | 240 | h2 = A2 - 2 * (X21 * X22) / (t * t); |
Kotttaro | 0:4a5272e014d8 | 241 | H2 << 1.0, 0.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 242 | 0.0, h2(0, 0), h2(0, 1), |
Kotttaro | 0:4a5272e014d8 | 243 | 0.0, h2(1, 0), h2(1, 1); |
Kotttaro | 0:4a5272e014d8 | 244 | R = H2 * ma1; |
Kotttaro | 0:4a5272e014d8 | 245 | MatrixXd H1T(3, 3), H2T(3, 3); |
Kotttaro | 0:4a5272e014d8 | 246 | H1T = H1.transpose(); |
Kotttaro | 0:4a5272e014d8 | 247 | H2T = H2.transpose(); |
Kotttaro | 4:8a50c7822dac | 248 | Q = H1T * H2T; |
Kotttaro | 0:4a5272e014d8 | 249 | } |
Kotttaro | 0:4a5272e014d8 | 250 | |
Kotttaro | 0:4a5272e014d8 | 251 | void vp(int leg) {//5年生の時に作成したもの |
Kotttaro | 0:4a5272e014d8 | 252 | double crosx, crosy, crosz; |
Kotttaro | 5:f225e0c61cfc | 253 | double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) }; |
Kotttaro | 0:4a5272e014d8 | 254 | double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) }; |
Kotttaro | 0:4a5272e014d8 | 255 | double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] }; |
Kotttaro | 0:4a5272e014d8 | 256 | if (Lin[2] != 0.0) |
Kotttaro | 0:4a5272e014d8 | 257 | { |
Kotttaro | 0:4a5272e014d8 | 258 | double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 }; |
Kotttaro | 0:4a5272e014d8 | 259 | for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; } |
Kotttaro | 0:4a5272e014d8 | 260 | AP[2] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 261 | } |
Kotttaro | 0:4a5272e014d8 | 262 | crosx = AP[1] * wA[2] + (-AP[2]) * wA[1]; |
Kotttaro | 0:4a5272e014d8 | 263 | crosy = AP[2] * wA[0] + (-AP[0]) * wA[2]; |
Kotttaro | 0:4a5272e014d8 | 264 | crosz = AP[0] * wA[1] + (-AP[1]) * wA[0]; |
Kotttaro | 0:4a5272e014d8 | 265 | vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2]; |
Kotttaro | 0:4a5272e014d8 | 266 | //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]); |
Kotttaro | 0:4a5272e014d8 | 267 | //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0)); |
Kotttaro | 0:4a5272e014d8 | 268 | //printf("vp finish\n"); |
Kotttaro | 0:4a5272e014d8 | 269 | } |
Kotttaro | 0:4a5272e014d8 | 270 | void fwd(int leg) { |
Kotttaro | 0:4a5272e014d8 | 271 | //printf("fwd start\n"); |
Kotttaro | 0:4a5272e014d8 | 272 | double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]), |
Kotttaro | 0:4a5272e014d8 | 273 | c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]), |
Kotttaro | 0:4a5272e014d8 | 274 | s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); |
Kotttaro | 0:4a5272e014d8 | 275 | tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x |
Kotttaro | 0:4a5272e014d8 | 276 | tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y |
Kotttaro | 0:4a5272e014d8 | 277 | tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z |
Kotttaro | 5:f225e0c61cfc | 278 | //pc2.printf("leg=%d,( %3.3lf, %3.3lf, %3.3lf)) ",leg,tip[leg][0],tip[leg][1],tip[leg][2]); |
Kotttaro | 0:4a5272e014d8 | 279 | } |
Kotttaro | 0:4a5272e014d8 | 280 | void Jac(int leg) { |
Kotttaro | 0:4a5272e014d8 | 281 | //printf("Jac start\n"); |
Kotttaro | 0:4a5272e014d8 | 282 | double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]), |
Kotttaro | 0:4a5272e014d8 | 283 | c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]), |
Kotttaro | 0:4a5272e014d8 | 284 | s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); |
Kotttaro | 0:4a5272e014d8 | 285 | Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); |
Kotttaro | 0:4a5272e014d8 | 286 | Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 287 | Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 288 | Jacbi[leg][0][3] = (-L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 289 | |
Kotttaro | 0:4a5272e014d8 | 290 | Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); |
Kotttaro | 0:4a5272e014d8 | 291 | Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 292 | Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 293 | Jacbi[leg][1][3] = (-L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 294 | |
Kotttaro | 0:4a5272e014d8 | 295 | Jacbi[leg][2][0] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 296 | Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 297 | Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 298 | Jacbi[leg][2][3] = L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 299 | |
Kotttaro | 0:4a5272e014d8 | 300 | |
Kotttaro | 0:4a5272e014d8 | 301 | //printf("Jac finish\n"); |
Kotttaro | 0:4a5272e014d8 | 302 | }//ok |
Kotttaro | 0:4a5272e014d8 | 303 | void deff(int leg) { |
Kotttaro | 0:4a5272e014d8 | 304 | //printf(" 評価関数定義\n"); |
Kotttaro | 0:4a5272e014d8 | 305 | fi = r + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順 |
Kotttaro | 0:4a5272e014d8 | 306 | a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2])); |
Kotttaro | 0:4a5272e014d8 | 307 | a = a0 * (double)cos(fi); |
Kotttaro | 0:4a5272e014d8 | 308 | h = a * (1 / (double)cos(fi) - tan(fi)); |
Kotttaro | 0:4a5272e014d8 | 309 | X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身 |
Kotttaro | 0:4a5272e014d8 | 310 | //tan-1の分母分子 |
Kotttaro | 0:4a5272e014d8 | 311 | tan_u = tip[leg][2]; |
Kotttaro | 0:4a5272e014d8 | 312 | tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])); |
Kotttaro | 0:4a5272e014d8 | 313 | //printf("評価関数計算完了\n"); |
Kotttaro | 0:4a5272e014d8 | 314 | } |
Kotttaro | 0:4a5272e014d8 | 315 | void dfd(int leg) { |
Kotttaro | 0:4a5272e014d8 | 316 | //printf("評価関数微分\n"); |
Kotttaro | 0:4a5272e014d8 | 317 | double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]), s2 = (double)sin(th[leg][2]), s3 = (double)sin(th[leg][2]); |
Kotttaro | 0:4a5272e014d8 | 318 | double c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), s23 = (double)sin(th[leg][2] + th[leg][3]), c23 = (double)cos(th[leg][2] + th[leg][3]); |
Kotttaro | 0:4a5272e014d8 | 319 | double c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]), s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); |
Kotttaro | 1:5c2562adca7d | 320 | double cfi=cos(fi),sfi=sin(fi); |
Kotttaro | 1:5c2562adca7d | 321 | double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2]; |
Kotttaro | 1:5c2562adca7d | 322 | |
Kotttaro | 1:5c2562adca7d | 323 | double df_da=1/cfi-tan(fi); |
Kotttaro | 1:5c2562adca7d | 324 | double df_dfi=a*(-sfi-1)/(cfi*cfi); |
Kotttaro | 1:5c2562adca7d | 325 | double da_dx=x*cfi/sqrt(x*x+y*y); |
Kotttaro | 1:5c2562adca7d | 326 | double da_dy=y*cfi/sqrt(x*x+y*y); |
Kotttaro | 1:5c2562adca7d | 327 | double da_dfi=-sqrt(x*x+y*y)*sfi; |
Kotttaro | 1:5c2562adca7d | 328 | double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); |
Kotttaro | 1:5c2562adca7d | 329 | double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); |
Kotttaro | 1:5c2562adca7d | 330 | double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z); |
Kotttaro | 1:5c2562adca7d | 331 | |
Kotttaro | 1:5c2562adca7d | 332 | dfdth[0]=df_da*(da_dx*Jacbi[leg][0][0]+da_dy*Jacbi[leg][1][0]+da_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0])) |
Kotttaro | 1:5c2562adca7d | 333 | +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); |
Kotttaro | 1:5c2562adca7d | 334 | |
Kotttaro | 1:5c2562adca7d | 335 | dfdth[1]=df_da*(da_dx*Jacbi[leg][0][1]+da_dy*Jacbi[leg][1][1]+da_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1])) |
Kotttaro | 1:5c2562adca7d | 336 | +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]); |
Kotttaro | 1:5c2562adca7d | 337 | |
Kotttaro | 1:5c2562adca7d | 338 | dfdth[2]=df_da*(da_dx*Jacbi[leg][0][2]+da_dy*Jacbi[leg][1][2]+da_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2])) |
Kotttaro | 1:5c2562adca7d | 339 | +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); |
Kotttaro | 1:5c2562adca7d | 340 | |
Kotttaro | 1:5c2562adca7d | 341 | dfdth[3]=df_da*(da_dx*Jacbi[leg][0][3]+da_dy*Jacbi[leg][1][3]+da_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3])) |
Kotttaro | 1:5c2562adca7d | 342 | +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]); |
Kotttaro | 1:5c2562adca7d | 343 | |
Kotttaro | 3:f824e4d8eef7 | 344 | //pc2.printf("df_da=%lf df_dfi=%lf da_dx=%lf da_dy=%lf da_dfi=%lf dfi_dx=%lf dfi_dy=%lf dfi_dz=%lf\r\n",df_da,df_dfi,da_dx,da_dy,da_dfi,dfi_dx,dfi_dy,dfi_dz); |
Kotttaro | 0:4a5272e014d8 | 345 | } |
Kotttaro | 4:8a50c7822dac | 346 | //使わない |
Kotttaro | 2:57237f0a4a34 | 347 | double fe(int leg,double dth3) { |
Kotttaro | 2:57237f0a4a34 | 348 | //brent法のための関数, 事前にdfdを実行してから使う |
Kotttaro | 3:f824e4d8eef7 | 349 | double dfd_nolm,th0_nolm,e=0.0; |
Kotttaro | 2:57237f0a4a34 | 350 | //∇hを正規化する |
Kotttaro | 3:f824e4d8eef7 | 351 | dfd(leg); |
Kotttaro | 2:57237f0a4a34 | 352 | dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]); |
Kotttaro | 2:57237f0a4a34 | 353 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 354 | dfdth[i]=dfdth[i]/dfd_nolm; |
Kotttaro | 2:57237f0a4a34 | 355 | } |
Kotttaro | 2:57237f0a4a34 | 356 | |
Kotttaro | 2:57237f0a4a34 | 357 | solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 |
Kotttaro | 2:57237f0a4a34 | 358 | |
Kotttaro | 2:57237f0a4a34 | 359 | //dthベクトルを正規化 |
Kotttaro | 2:57237f0a4a34 | 360 | th0_nolm = sqrt(th0[leg][0] * th0[leg][0]+ th0[leg][1]* th0[leg][1]+ th0[leg][2]* th0[leg][2]+ th0[leg][3]*th0[leg][3]); |
Kotttaro | 2:57237f0a4a34 | 361 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 362 | th0[leg][i] = th0[leg][i] / th0_nolm; |
Kotttaro | 2:57237f0a4a34 | 363 | } |
Kotttaro | 2:57237f0a4a34 | 364 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 365 | e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]); |
Kotttaro | 4:8a50c7822dac | 366 | } |
Kotttaro | 2:57237f0a4a34 | 367 | return e;//eベクトルのノルムの2乗を返す |
Kotttaro | 0:4a5272e014d8 | 368 | |
Kotttaro | 2:57237f0a4a34 | 369 | } |
Kotttaro | 3:f824e4d8eef7 | 370 | double f(int leg,double dth3) { |
Kotttaro | 3:f824e4d8eef7 | 371 | double f_return=0.0; |
Kotttaro | 3:f824e4d8eef7 | 372 | solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 |
Kotttaro | 3:f824e4d8eef7 | 373 | f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3]; |
Kotttaro | 4:8a50c7822dac | 374 | |
Kotttaro | 4:8a50c7822dac | 375 | return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる |
Kotttaro | 3:f824e4d8eef7 | 376 | |
Kotttaro | 4:8a50c7822dac | 377 | } |
Kotttaro | 4:8a50c7822dac | 378 | void mnbrak(int leg,int discrimination) |
Kotttaro | 2:57237f0a4a34 | 379 | { |
Kotttaro | 4:8a50c7822dac | 380 | double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc; |
Kotttaro | 4:8a50c7822dac | 381 | //fa=f(ax); |
Kotttaro | 4:8a50c7822dac | 382 | //fb=f(bx); |
Kotttaro | 4:8a50c7822dac | 383 | if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);} |
Kotttaro | 4:8a50c7822dac | 384 | if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);} |
Kotttaro | 4:8a50c7822dac | 385 | if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);} |
Kotttaro | 4:8a50c7822dac | 386 | if(discrimination==3){fa=f_test(ax);fb=f_test(bx);} |
Kotttaro | 4:8a50c7822dac | 387 | |
Kotttaro | 4:8a50c7822dac | 388 | if(fb>fa) |
Kotttaro | 4:8a50c7822dac | 389 | { |
Kotttaro | 4:8a50c7822dac | 390 | SHIFT(dum,ax,bx,dum); |
Kotttaro | 4:8a50c7822dac | 391 | SHIFT(dum,fb,fa,dum); |
Kotttaro | 4:8a50c7822dac | 392 | } |
Kotttaro | 4:8a50c7822dac | 393 | cx=bx+GOLD*(bx-ax); |
Kotttaro | 4:8a50c7822dac | 394 | //fc=f(cx); |
Kotttaro | 4:8a50c7822dac | 395 | if(discrimination==0){fc=fe(leg,cx);} |
Kotttaro | 4:8a50c7822dac | 396 | if(discrimination==1){fc=num_nolm(leg,cx);} |
Kotttaro | 4:8a50c7822dac | 397 | if(discrimination==2){fc=f(leg,cx);} |
Kotttaro | 4:8a50c7822dac | 398 | if(discrimination==3){fc=f_test(cx);} |
Kotttaro | 4:8a50c7822dac | 399 | while (fb>fc) |
Kotttaro | 4:8a50c7822dac | 400 | { |
Kotttaro | 4:8a50c7822dac | 401 | r=(bx-ax)*(fb-fc); |
Kotttaro | 4:8a50c7822dac | 402 | q=(bx-cx)*(fb-fa); |
Kotttaro | 4:8a50c7822dac | 403 | u=bx-((bx-cx)*q-(bx-cx)*r)/ |
Kotttaro | 4:8a50c7822dac | 404 | (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r)); |
Kotttaro | 4:8a50c7822dac | 405 | ulim=bx+GLIMIT*(cx-bx); |
Kotttaro | 4:8a50c7822dac | 406 | |
Kotttaro | 4:8a50c7822dac | 407 | if((bx-u)*(u-cx)>0.0) |
Kotttaro | 4:8a50c7822dac | 408 | { |
Kotttaro | 4:8a50c7822dac | 409 | //fu=f(u); |
Kotttaro | 4:8a50c7822dac | 410 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 411 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 412 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 413 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 414 | if(fu<fc) |
Kotttaro | 4:8a50c7822dac | 415 | { |
Kotttaro | 4:8a50c7822dac | 416 | ax=bx; |
Kotttaro | 4:8a50c7822dac | 417 | bx=u; |
Kotttaro | 4:8a50c7822dac | 418 | fa=fb; |
Kotttaro | 4:8a50c7822dac | 419 | fb=fu; |
Kotttaro | 4:8a50c7822dac | 420 | return; |
Kotttaro | 4:8a50c7822dac | 421 | } |
Kotttaro | 4:8a50c7822dac | 422 | else if(fu>fb) |
Kotttaro | 4:8a50c7822dac | 423 | { |
Kotttaro | 4:8a50c7822dac | 424 | cx=u; |
Kotttaro | 4:8a50c7822dac | 425 | fc=fu; |
Kotttaro | 4:8a50c7822dac | 426 | return; |
Kotttaro | 4:8a50c7822dac | 427 | } |
Kotttaro | 4:8a50c7822dac | 428 | u=cx*+GOLD*(cx-bx); |
Kotttaro | 4:8a50c7822dac | 429 | //fu=f(u); |
Kotttaro | 4:8a50c7822dac | 430 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 431 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 432 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 433 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 434 | } |
Kotttaro | 4:8a50c7822dac | 435 | else if((cx-u)*(u-ulim)>0.0) |
Kotttaro | 4:8a50c7822dac | 436 | { |
Kotttaro | 4:8a50c7822dac | 437 | //fu=f(u); |
Kotttaro | 4:8a50c7822dac | 438 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 439 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 440 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 441 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 442 | if(fu<fc) |
Kotttaro | 4:8a50c7822dac | 443 | { |
Kotttaro | 4:8a50c7822dac | 444 | SHIFT(bx,cx,u,cx+GOLD*(cx-bx)); |
Kotttaro | 4:8a50c7822dac | 445 | if(discrimination==0){fu_2=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 446 | if(discrimination==1){fu_2=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 447 | if(discrimination==2){fu_2=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 448 | if(discrimination==3){fu_2=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 449 | //SHIFT(fb,fc,fu,f(u)); |
Kotttaro | 4:8a50c7822dac | 450 | SHIFT(fb,fc,fu,fu_2); |
Kotttaro | 4:8a50c7822dac | 451 | } |
Kotttaro | 4:8a50c7822dac | 452 | |
Kotttaro | 4:8a50c7822dac | 453 | } |
Kotttaro | 4:8a50c7822dac | 454 | else if((u-ulim)*(ulim-cx)>=0.0) |
Kotttaro | 4:8a50c7822dac | 455 | { |
Kotttaro | 4:8a50c7822dac | 456 | u=ulim; |
Kotttaro | 4:8a50c7822dac | 457 | //fu=f(u); |
Kotttaro | 4:8a50c7822dac | 458 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 459 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 460 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 461 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 462 | } |
Kotttaro | 4:8a50c7822dac | 463 | else |
Kotttaro | 4:8a50c7822dac | 464 | { |
Kotttaro | 4:8a50c7822dac | 465 | u=cx+GOLD*(cx-bx); |
Kotttaro | 4:8a50c7822dac | 466 | //fu=f(u); |
Kotttaro | 4:8a50c7822dac | 467 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 468 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 469 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 470 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 471 | } |
Kotttaro | 4:8a50c7822dac | 472 | SHIFT(ax,bx,cx,u); |
Kotttaro | 4:8a50c7822dac | 473 | SHIFT(fa,fb,fc,fu); |
Kotttaro | 4:8a50c7822dac | 474 | } |
Kotttaro | 4:8a50c7822dac | 475 | } |
Kotttaro | 4:8a50c7822dac | 476 | double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ |
Kotttaro | 4:8a50c7822dac | 477 | { |
Kotttaro | 4:8a50c7822dac | 478 | |
Kotttaro | 2:57237f0a4a34 | 479 | int iter; |
Kotttaro | 2:57237f0a4a34 | 480 | double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin; |
Kotttaro | 2:57237f0a4a34 | 481 | double e=0.0; |
Kotttaro | 4:8a50c7822dac | 482 | a=(ax <cx ? ax : cx); |
Kotttaro | 4:8a50c7822dac | 483 | b=(ax >cx ? ax : cx); |
Kotttaro | 4:8a50c7822dac | 484 | x=w=v=bx; |
Kotttaro | 4:8a50c7822dac | 485 | if(discrimination==0){fw=fv=fx=fe(leg,x);} |
Kotttaro | 4:8a50c7822dac | 486 | if(discrimination==1){fw=fv=fx=num_nolm(leg,x);} |
Kotttaro | 4:8a50c7822dac | 487 | if(discrimination==2){fw=fv=fx=f(leg,x);} |
Kotttaro | 4:8a50c7822dac | 488 | if(discrimination==3){fw=fv=fx=f_test(x);} |
Kotttaro | 4:8a50c7822dac | 489 | //fw=fv=fx=f(x);//関数部分 |
Kotttaro | 2:57237f0a4a34 | 490 | for(iter=1;iter<=ITMAX;iter++) |
Kotttaro | 2:57237f0a4a34 | 491 | { |
Kotttaro | 2:57237f0a4a34 | 492 | xm=0.5*(a+b); |
Kotttaro | 2:57237f0a4a34 | 493 | tol2=2.0*(tol1=tol*fabs(x)+ZEPS); |
Kotttaro | 4:8a50c7822dac | 494 | //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u); |
Kotttaro | 2:57237f0a4a34 | 495 | if(fabs(x-xm)<=(tol2-0.5*(b-a))) |
Kotttaro | 2:57237f0a4a34 | 496 | { |
Kotttaro | 4:8a50c7822dac | 497 | //pc.printf("bernt out"); |
Kotttaro | 2:57237f0a4a34 | 498 | xmin=x; |
Kotttaro | 4:8a50c7822dac | 499 | //pc2.printf("xmin=%lf\r\n",x); |
Kotttaro | 4:8a50c7822dac | 500 | //pc2.printf("fx=%lf\r\n",fx); |
Kotttaro | 2:57237f0a4a34 | 501 | return xmin; |
Kotttaro | 2:57237f0a4a34 | 502 | } |
Kotttaro | 2:57237f0a4a34 | 503 | if(fabs(e)>tol1) |
Kotttaro | 2:57237f0a4a34 | 504 | { |
Kotttaro | 2:57237f0a4a34 | 505 | r=(x-w)*(fx-fv); |
Kotttaro | 2:57237f0a4a34 | 506 | q=(x-v)*(fx-fw); |
Kotttaro | 2:57237f0a4a34 | 507 | p=(x-v)*q-(x-w)*r; |
Kotttaro | 2:57237f0a4a34 | 508 | q=2.0*(q-r); |
Kotttaro | 2:57237f0a4a34 | 509 | if(q>0.0)p=-p; |
Kotttaro | 2:57237f0a4a34 | 510 | q=fabs(q); |
Kotttaro | 2:57237f0a4a34 | 511 | etemp=e; |
Kotttaro | 2:57237f0a4a34 | 512 | e=d; |
Kotttaro | 2:57237f0a4a34 | 513 | if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x)) |
Kotttaro | 2:57237f0a4a34 | 514 | { d=CGOLD*(e= (x>=xm ? a-x : b-x));} |
Kotttaro | 2:57237f0a4a34 | 515 | else |
Kotttaro | 2:57237f0a4a34 | 516 | { |
Kotttaro | 2:57237f0a4a34 | 517 | d=p/q; |
Kotttaro | 2:57237f0a4a34 | 518 | u=x+d; |
Kotttaro | 2:57237f0a4a34 | 519 | if(u-a < tol2 || b-u < tol2) |
Kotttaro | 2:57237f0a4a34 | 520 | {d=SIGN(tol1,xm-x);} |
Kotttaro | 2:57237f0a4a34 | 521 | } |
Kotttaro | 2:57237f0a4a34 | 522 | } |
Kotttaro | 2:57237f0a4a34 | 523 | else |
Kotttaro | 2:57237f0a4a34 | 524 | { |
Kotttaro | 2:57237f0a4a34 | 525 | d=CGOLD*(e= (x>=xm ? a-x : b-x)); |
Kotttaro | 2:57237f0a4a34 | 526 | } |
Kotttaro | 2:57237f0a4a34 | 527 | u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d)); |
Kotttaro | 4:8a50c7822dac | 528 | if(discrimination==0){fu=fe(leg,u);} |
Kotttaro | 4:8a50c7822dac | 529 | if(discrimination==1){fu=num_nolm(leg,u);} |
Kotttaro | 4:8a50c7822dac | 530 | if(discrimination==2){fu=f(leg,u);} |
Kotttaro | 4:8a50c7822dac | 531 | if(discrimination==3){fu=f_test(u);} |
Kotttaro | 4:8a50c7822dac | 532 | //fu=f(u);//関数部分 |
Kotttaro | 2:57237f0a4a34 | 533 | if(fu <= fx) |
Kotttaro | 2:57237f0a4a34 | 534 | { |
Kotttaro | 2:57237f0a4a34 | 535 | if(u >= x)a=x; else b=x; |
Kotttaro | 2:57237f0a4a34 | 536 | SHIFT(v,w,x,u); |
Kotttaro | 2:57237f0a4a34 | 537 | SHIFT(fv,fw,fx,fu); |
Kotttaro | 2:57237f0a4a34 | 538 | } |
Kotttaro | 2:57237f0a4a34 | 539 | else{ |
Kotttaro | 2:57237f0a4a34 | 540 | if(u < x){a=u;} |
Kotttaro | 2:57237f0a4a34 | 541 | else {b=u;} |
Kotttaro | 2:57237f0a4a34 | 542 | if(fu <= fw || w==x) |
Kotttaro | 2:57237f0a4a34 | 543 | { |
Kotttaro | 2:57237f0a4a34 | 544 | v=w; |
Kotttaro | 2:57237f0a4a34 | 545 | w=u; |
Kotttaro | 2:57237f0a4a34 | 546 | fv=fw; |
Kotttaro | 2:57237f0a4a34 | 547 | fw=fu; |
Kotttaro | 2:57237f0a4a34 | 548 | } |
Kotttaro | 2:57237f0a4a34 | 549 | else if (fu <= fv || v==x || v==w) |
Kotttaro | 2:57237f0a4a34 | 550 | { |
Kotttaro | 2:57237f0a4a34 | 551 | v=u; |
Kotttaro | 2:57237f0a4a34 | 552 | fv=fu; |
Kotttaro | 2:57237f0a4a34 | 553 | } |
Kotttaro | 2:57237f0a4a34 | 554 | } |
Kotttaro | 2:57237f0a4a34 | 555 | |
Kotttaro | 2:57237f0a4a34 | 556 | } |
Kotttaro | 4:8a50c7822dac | 557 | //pc2.printf("xmin=%lf\r\n",x); |
Kotttaro | 4:8a50c7822dac | 558 | //pc2.printf("fx=%lf\r\n",fx); |
Kotttaro | 2:57237f0a4a34 | 559 | return xmin; |
Kotttaro | 4:8a50c7822dac | 560 | } |
Kotttaro | 4:8a50c7822dac | 561 | |
Kotttaro | 2:57237f0a4a34 | 562 | double SIGN(double x,double y) |
Kotttaro | 2:57237f0a4a34 | 563 | { |
Kotttaro | 2:57237f0a4a34 | 564 | double x_return; |
Kotttaro | 2:57237f0a4a34 | 565 | x_return=abs(x); |
Kotttaro | 2:57237f0a4a34 | 566 | if(y<0.0)x_return=-x_return; |
Kotttaro | 2:57237f0a4a34 | 567 | |
Kotttaro | 2:57237f0a4a34 | 568 | return x_return; |
Kotttaro | 2:57237f0a4a34 | 569 | } |
Kotttaro | 4:8a50c7822dac | 570 | double FMAX(double x, double y) |
Kotttaro | 4:8a50c7822dac | 571 | { |
Kotttaro | 4:8a50c7822dac | 572 | if(x>y){return x;} |
Kotttaro | 4:8a50c7822dac | 573 | if(y>x){return y;} |
Kotttaro | 4:8a50c7822dac | 574 | return 0; |
Kotttaro | 4:8a50c7822dac | 575 | } |
Kotttaro | 2:57237f0a4a34 | 576 | double num_nolm(int leg,double dth3) |
Kotttaro | 2:57237f0a4a34 | 577 | { |
Kotttaro | 4:8a50c7822dac | 578 | |
Kotttaro | 2:57237f0a4a34 | 579 | double nolm_return=0.0; |
Kotttaro | 4:8a50c7822dac | 580 | solve(dth3,leg,2); |
Kotttaro | 4:8a50c7822dac | 581 | nolm_return=th0[leg][0]*th0[leg][0]+th0[leg][1]*th0[leg][1]+th0[leg][2]*th0[leg][2]+th0[leg][3]*th0[leg][3]; |
Kotttaro | 4:8a50c7822dac | 582 | |
Kotttaro | 2:57237f0a4a34 | 583 | return nolm_return; |
Kotttaro | 4:8a50c7822dac | 584 | } |
Kotttaro | 4:8a50c7822dac | 585 | //brent法のテスト用の関数 |
Kotttaro | 4:8a50c7822dac | 586 | //極小値を求めたい関数を定義 |
Kotttaro | 4:8a50c7822dac | 587 | double f_test(double x){ |
Kotttaro | 4:8a50c7822dac | 588 | double x_return; |
Kotttaro | 4:8a50c7822dac | 589 | x_return=x*x-2*x+1; |
Kotttaro | 4:8a50c7822dac | 590 | return x_return; |
Kotttaro | 4:8a50c7822dac | 591 | } |
Kotttaro | 0:4a5272e014d8 | 592 | void solve(double w3, int leg,int det) { |
Kotttaro | 4:8a50c7822dac | 593 | |
Kotttaro | 0:4a5272e014d8 | 594 | double dth[4]; |
Kotttaro | 0:4a5272e014d8 | 595 | MatrixXd v_Q(3,1),QT(3,3); |
Kotttaro | 0:4a5272e014d8 | 596 | |
Kotttaro | 0:4a5272e014d8 | 597 | QT = Q.transpose(); |
Kotttaro | 0:4a5272e014d8 | 598 | v_Q = QT * vP[leg]*sampling; |
Kotttaro | 4:8a50c7822dac | 599 | |
Kotttaro | 2:57237f0a4a34 | 600 | dth[3] = w3 ; |
Kotttaro | 0:4a5272e014d8 | 601 | dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2)); |
Kotttaro | 0:4a5272e014d8 | 602 | dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1)); |
Kotttaro | 0:4a5272e014d8 | 603 | dth[0] = (double)((v_Q(0, 0) - R(0, 1) * dth[1] - R(0, 2)*dth[2] - R(0, 3) * dth[3])/R(0,0)); |
Kotttaro | 0:4a5272e014d8 | 604 | if (det == 1) { |
Kotttaro | 0:4a5272e014d8 | 605 | for (int i=0; i < 4; i++) { |
Kotttaro | 4:8a50c7822dac | 606 | th[leg][i] = th[leg][i] + dth[i]; |
Kotttaro | 0:4a5272e014d8 | 607 | } |
Kotttaro | 0:4a5272e014d8 | 608 | } |
Kotttaro | 4:8a50c7822dac | 609 | else if (det == 2) { |
Kotttaro | 4:8a50c7822dac | 610 | for (int u=0; u < 4; u++) { |
Kotttaro | 4:8a50c7822dac | 611 | th0[leg][u] = dth[u]; |
Kotttaro | 4:8a50c7822dac | 612 | } |
Kotttaro | 4:8a50c7822dac | 613 | } |
Kotttaro | 4:8a50c7822dac | 614 | |
Kotttaro | 0:4a5272e014d8 | 615 | } |
Kotttaro | 0:4a5272e014d8 | 616 | |
Kotttaro | 4:8a50c7822dac | 617 | //サーボ関係 |
Kotttaro | 4:8a50c7822dac | 618 | void setup_servo() { |
Kotttaro | 4:8a50c7822dac | 619 | pwm.begin(); |
Kotttaro | 4:8a50c7822dac | 620 | pwm.setPWMFreq(200); |
Kotttaro | 4:8a50c7822dac | 621 | } |
Kotttaro | 4:8a50c7822dac | 622 | void servo_write(int ch,double ang)//引数は[°] |
Kotttaro | 4:8a50c7822dac | 623 | { |
Kotttaro | 4:8a50c7822dac | 624 | if(ch==0) ang=ang-135*PI/180; |
Kotttaro | 4:8a50c7822dac | 625 | if(ch==4) ang=ang-45*PI/180; |
Kotttaro | 4:8a50c7822dac | 626 | if(ch==8) ang=ang+45*PI/180; |
Kotttaro | 4:8a50c7822dac | 627 | if(ch==12) ang=ang+135*PI/180; |
Kotttaro | 4:8a50c7822dac | 628 | if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang; |
Kotttaro | 4:8a50c7822dac | 629 | ang=servo0[ch]+ang*8000/270*180/PI; |
Kotttaro | 4:8a50c7822dac | 630 | servo_write7(ch,ang); |
Kotttaro | 4:8a50c7822dac | 631 | } |
Kotttaro | 4:8a50c7822dac | 632 | |
Kotttaro | 4:8a50c7822dac | 633 | void servo_write7(int ch, double ang){ |
Kotttaro | 4:8a50c7822dac | 634 | ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階 |
Kotttaro | 4:8a50c7822dac | 635 | //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ; //初期状態を設定するときこの値を参考に設定したためそのまま利用 |
Kotttaro | 4:8a50c7822dac | 636 | pwm.setPWM(ch, 0, ang); |
Kotttaro | 4:8a50c7822dac | 637 | } |
Kotttaro | 4:8a50c7822dac | 638 | void servo_calib() |
Kotttaro | 4:8a50c7822dac | 639 | { |
Kotttaro | 4:8a50c7822dac | 640 | for(int u=0; u<4; u++) |
Kotttaro | 4:8a50c7822dac | 641 | { |
Kotttaro | 4:8a50c7822dac | 642 | for(int i=0; i<4; i++) |
Kotttaro | 4:8a50c7822dac | 643 | { |
Kotttaro | 4:8a50c7822dac | 644 | servo_write7(ch[u][i],servo0[ch[u][i]]); |
Kotttaro | 4:8a50c7822dac | 645 | } |
Kotttaro | 4:8a50c7822dac | 646 | } |
Kotttaro | 4:8a50c7822dac | 647 | } |
Kotttaro | 5:f225e0c61cfc | 648 | void servo_ready() |
Kotttaro | 5:f225e0c61cfc | 649 | { |
Kotttaro | 5:f225e0c61cfc | 650 | for(int u=0; u<4; u++) |
Kotttaro | 5:f225e0c61cfc | 651 | { |
Kotttaro | 5:f225e0c61cfc | 652 | for(int i=0; i<4; i++) |
Kotttaro | 5:f225e0c61cfc | 653 | { |
Kotttaro | 5:f225e0c61cfc | 654 | servo_write(ch[u][i],th_ready[u][i]); |
Kotttaro | 5:f225e0c61cfc | 655 | } |
Kotttaro | 5:f225e0c61cfc | 656 | } |
Kotttaro | 5:f225e0c61cfc | 657 | } |