2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
main.cpp@3:f824e4d8eef7, 2021-12-22 (annotated)
- Committer:
- Kotttaro
- Date:
- Wed Dec 22 07:06:14 2021 +0000
- Revision:
- 3:f824e4d8eef7
- Parent:
- 2:57237f0a4a34
- Child:
- 4:8a50c7822dac
2021.12.22.16:06
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Kotttaro | 0:4a5272e014d8 | 1 | #include "mbed.h" |
Kotttaro | 0:4a5272e014d8 | 2 | //特別研究Ⅰで用いたプログラム |
Kotttaro | 0:4a5272e014d8 | 3 | //ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する |
Kotttaro | 2:57237f0a4a34 | 4 | //brent法の部分は『numerical recipes in c』 参照 |
Kotttaro | 0:4a5272e014d8 | 5 | #include "Eigen/Geometry.h" |
Kotttaro | 0:4a5272e014d8 | 6 | #include "Eigen/Dense.h" |
Kotttaro | 0:4a5272e014d8 | 7 | #include <math.h> |
Kotttaro | 2:57237f0a4a34 | 8 | #define ITMAX 100 |
Kotttaro | 2:57237f0a4a34 | 9 | #define CGOLD 0.3819660 |
Kotttaro | 2:57237f0a4a34 | 10 | #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); |
Kotttaro | 2:57237f0a4a34 | 11 | #define ZEPS 1.0e-10 |
Kotttaro | 2:57237f0a4a34 | 12 | |
Kotttaro | 0:4a5272e014d8 | 13 | //#pragma warning(disable: 4996) |
Kotttaro | 0:4a5272e014d8 | 14 | Serial pc2(USBTX,USBRX); |
Kotttaro | 0:4a5272e014d8 | 15 | Timer tim; |
Kotttaro | 3:f824e4d8eef7 | 16 | int times= 1;//実行回数:実行時間は??秒 |
Kotttaro | 0:4a5272e014d8 | 17 | double PI =3.14159265358979323846264338327950288; |
Kotttaro | 0:4a5272e014d8 | 18 | |
Kotttaro | 0:4a5272e014d8 | 19 | using namespace Eigen; |
Kotttaro | 0:4a5272e014d8 | 20 | |
Kotttaro | 0:4a5272e014d8 | 21 | //以下変数定義 |
Kotttaro | 0:4a5272e014d8 | 22 | double r=50*PI/180;//斜面の傾き[°] |
Kotttaro | 1:5c2562adca7d | 23 | double sampling=0.01;//δtの時間[s] |
Kotttaro | 0:4a5272e014d8 | 24 | double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる |
Kotttaro | 0:4a5272e014d8 | 25 | double tip[4][3];//足先座標 |
Kotttaro | 0:4a5272e014d8 | 26 | double con[4][3] = { 50.0, 50.0,0, |
Kotttaro | 0:4a5272e014d8 | 27 | -50.0, 50.0,0, |
Kotttaro | 0:4a5272e014d8 | 28 | -50.0,-50.0,0, |
Kotttaro | 0:4a5272e014d8 | 29 | 50.0,-50.0,0};//脚のコーナー座標,zは必ず0 |
Kotttaro | 2:57237f0a4a34 | 30 | double th[4][4] = { 45 * PI / 180,30 * PI / 180,-30 * PI / 180,-30 * PI / 180, |
Kotttaro | 0:4a5272e014d8 | 31 | 135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180, |
Kotttaro | 0:4a5272e014d8 | 32 | -135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180, |
Kotttaro | 0:4a5272e014d8 | 33 | -45 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180 }; |
Kotttaro | 0:4a5272e014d8 | 34 | double th0[4][4]= { 0.0,0.0,0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 35 | 0.0,0.0, 0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 36 | 0.0,0.0, 0.0,0.0, |
Kotttaro | 0:4a5272e014d8 | 37 | 0.0,0.0, 0.0,0.0 }; //計算用の関節角度 |
Kotttaro | 0:4a5272e014d8 | 38 | double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元 |
Kotttaro | 0:4a5272e014d8 | 39 | double a,a0, h,fi;//評価関数内の変数 fi=φ |
Kotttaro | 0:4a5272e014d8 | 40 | double X,tan_u, tan_d;//計算用 |
Kotttaro | 0:4a5272e014d8 | 41 | |
Kotttaro | 0:4a5272e014d8 | 42 | //ねじ軸 |
Kotttaro | 0:4a5272e014d8 | 43 | //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ |
Kotttaro | 0:4a5272e014d8 | 44 | double Lin[3], L0[3], vin,v[3],wg[3],win,nol;//ねじ軸条件 |
Kotttaro | 0:4a5272e014d8 | 45 | double dfdth[4];//評価関数のナブラ |
Kotttaro | 0:4a5272e014d8 | 46 | |
Kotttaro | 0:4a5272e014d8 | 47 | |
Kotttaro | 0:4a5272e014d8 | 48 | //以下行列定義 |
Kotttaro | 0:4a5272e014d8 | 49 | MatrixXd Q(3, 3);//Q行列 |
Kotttaro | 0:4a5272e014d8 | 50 | MatrixXd R(3, 4);//R行列 |
Kotttaro | 0:4a5272e014d8 | 51 | Vector3d vP[4];//各脚の速度ベクトル |
Kotttaro | 0:4a5272e014d8 | 52 | |
Kotttaro | 0:4a5272e014d8 | 53 | |
Kotttaro | 0:4a5272e014d8 | 54 | void QR(int leg);//QR分解用関数,引数は脚番号 |
Kotttaro | 0:4a5272e014d8 | 55 | void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する |
Kotttaro | 0:4a5272e014d8 | 56 | void fwd(int leg);//順運動学より脚先の座標を導出する |
Kotttaro | 0:4a5272e014d8 | 57 | void Jac(int leg);//指定した脚のヤコビアンを計算 |
Kotttaro | 0:4a5272e014d8 | 58 | void deff(int leg);//評価関数計算, legは距離と傾きから指定する |
Kotttaro | 0:4a5272e014d8 | 59 | void dfd( int leg);//評価関数の勾配をとる |
Kotttaro | 0:4a5272e014d8 | 60 | double search(int leg);//最大のthetaを探索するための関数 |
Kotttaro | 0:4a5272e014d8 | 61 | void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す |
Kotttaro | 2:57237f0a4a34 | 62 | double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの |
Kotttaro | 2:57237f0a4a34 | 63 | double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数 |
Kotttaro | 3:f824e4d8eef7 | 64 | double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用 |
Kotttaro | 3:f824e4d8eef7 | 65 | double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム |
Kotttaro | 3:f824e4d8eef7 | 66 | //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f) |
Kotttaro | 2:57237f0a4a34 | 67 | double SIGN(double x,double y);//xにyの符号をつけたものを返す |
Kotttaro | 2:57237f0a4a34 | 68 | |
Kotttaro | 0:4a5272e014d8 | 69 | |
Kotttaro | 0:4a5272e014d8 | 70 | int main() |
Kotttaro | 0:4a5272e014d8 | 71 | { |
Kotttaro | 0:4a5272e014d8 | 72 | double t; |
Kotttaro | 0:4a5272e014d8 | 73 | pc2.baud(921600); |
Kotttaro | 0:4a5272e014d8 | 74 | int count = 0; |
Kotttaro | 0:4a5272e014d8 | 75 | //入力したねじ運動を換算する |
Kotttaro | 0:4a5272e014d8 | 76 | Lin[0] = 0.0; //ねじ軸x |
Kotttaro | 2:57237f0a4a34 | 77 | Lin[1] = 1.0; //ねじ軸y |
Kotttaro | 0:4a5272e014d8 | 78 | Lin[2] = 1.0;//ねじ軸z |
Kotttaro | 0:4a5272e014d8 | 79 | L0[0] = 0.0;//ねじ軸原点座標 |
Kotttaro | 0:4a5272e014d8 | 80 | L0[1] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 81 | L0[2] = 0.0; |
Kotttaro | 2:57237f0a4a34 | 82 | vin = 5.0; |
Kotttaro | 2:57237f0a4a34 | 83 | win = 0.0; |
Kotttaro | 2:57237f0a4a34 | 84 | printf("\r\n\r\n"); |
Kotttaro | 0:4a5272e014d8 | 85 | nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]); |
Kotttaro | 0:4a5272e014d8 | 86 | for (int i = 0; i < 3; i++) |
Kotttaro | 0:4a5272e014d8 | 87 | { |
Kotttaro | 0:4a5272e014d8 | 88 | wg[i] = Lin[i] * win / nol; |
Kotttaro | 0:4a5272e014d8 | 89 | v[i] = Lin[i] * vin / nol; |
Kotttaro | 0:4a5272e014d8 | 90 | } |
Kotttaro | 0:4a5272e014d8 | 91 | |
Kotttaro | 0:4a5272e014d8 | 92 | for (int i=0; i < 4; i++) { |
Kotttaro | 0:4a5272e014d8 | 93 | fwd(i); |
Kotttaro | 0:4a5272e014d8 | 94 | vp(i); |
Kotttaro | 0:4a5272e014d8 | 95 | } |
Kotttaro | 0:4a5272e014d8 | 96 | //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0)); |
Kotttaro | 0:4a5272e014d8 | 97 | |
Kotttaro | 0:4a5272e014d8 | 98 | //times*δtの時間だけサーボを動かす |
Kotttaro | 0:4a5272e014d8 | 99 | tim.start(); |
Kotttaro | 0:4a5272e014d8 | 100 | for (int i = 0; i < times;i++){ |
Kotttaro | 0:4a5272e014d8 | 101 | |
Kotttaro | 0:4a5272e014d8 | 102 | count = count + 1; |
Kotttaro | 0:4a5272e014d8 | 103 | double dth; |
Kotttaro | 1:5c2562adca7d | 104 | //printf("%d \n", count); |
Kotttaro | 0:4a5272e014d8 | 105 | fwd(0); |
Kotttaro | 0:4a5272e014d8 | 106 | vp(0); |
Kotttaro | 0:4a5272e014d8 | 107 | Jac(0); |
Kotttaro | 0:4a5272e014d8 | 108 | QR(0); |
Kotttaro | 0:4a5272e014d8 | 109 | deff(0); |
Kotttaro | 3:f824e4d8eef7 | 110 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 111 | f(0,3000); |
Kotttaro | 3:f824e4d8eef7 | 112 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 113 | f(0,-3000); |
Kotttaro | 3:f824e4d8eef7 | 114 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 115 | f(0,-40.0); |
Kotttaro | 3:f824e4d8eef7 | 116 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 117 | f(0,-500.0985); |
Kotttaro | 3:f824e4d8eef7 | 118 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 119 | f(0,0.0); |
Kotttaro | 3:f824e4d8eef7 | 120 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 121 | f(0,500.0985); |
Kotttaro | 3:f824e4d8eef7 | 122 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 123 | f(0,40.0); |
Kotttaro | 3:f824e4d8eef7 | 124 | pc2.printf("////////////////\r\n"); |
Kotttaro | 3:f824e4d8eef7 | 125 | //dth=brent(0,-500.0985,0.0,500.0985,0.001,0); |
Kotttaro | 3:f824e4d8eef7 | 126 | //solve(dth, 0, 1); |
Kotttaro | 0:4a5272e014d8 | 127 | |
Kotttaro | 0:4a5272e014d8 | 128 | t=tim.read(); |
Kotttaro | 3:f824e4d8eef7 | 129 | //pc2.printf("%2.4lf:(%3.3lf, %3.3lf, %3.3lf, %3.3lf)\n\r",t,th[0][0]*180/PI, th[0][1]*180/PI , th[0][2]*180/PI , th[0][3]*180/PI ); |
Kotttaro | 2:57237f0a4a34 | 130 | //pc2.printf("%d %5.8lf\r\n",count ,dth); |
Kotttaro | 0:4a5272e014d8 | 131 | } |
Kotttaro | 1:5c2562adca7d | 132 | t=tim.read(); |
Kotttaro | 2:57237f0a4a34 | 133 | //pc2.printf("%2.4lf\r\n",t); |
Kotttaro | 0:4a5272e014d8 | 134 | return 0; // ソフトの終了 |
Kotttaro | 0:4a5272e014d8 | 135 | } |
Kotttaro | 0:4a5272e014d8 | 136 | |
Kotttaro | 0:4a5272e014d8 | 137 | void QR(int leg) { |
Kotttaro | 0:4a5272e014d8 | 138 | //printf("QR start"); |
Kotttaro | 0:4a5272e014d8 | 139 | |
Kotttaro | 0:4a5272e014d8 | 140 | double s, t;//要素計算用 |
Kotttaro | 0:4a5272e014d8 | 141 | MatrixXd ma(3, 4), ma1(3, 4); |
Kotttaro | 0:4a5272e014d8 | 142 | |
Kotttaro | 0:4a5272e014d8 | 143 | ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3], |
Kotttaro | 0:4a5272e014d8 | 144 | Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3], |
Kotttaro | 0:4a5272e014d8 | 145 | Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3]; |
Kotttaro | 0:4a5272e014d8 | 146 | /*printf("Jac :%lf %lf %lf %lf\n", ma(0, 0), ma(0, 1), ma(0, 2), ma(0, 3)); |
Kotttaro | 0:4a5272e014d8 | 147 | printf(" %lf %lf %lf %lf\n", ma(1, 0), ma(1, 1), ma(1, 2), ma(1, 3)); |
Kotttaro | 0:4a5272e014d8 | 148 | printf(" %lf %lf %lf %lf\n", ma(2, 0), ma(2, 1), ma(2, 2), ma(2, 3));*/ |
Kotttaro | 0:4a5272e014d8 | 149 | //printf("ma was made\n"); |
Kotttaro | 0:4a5272e014d8 | 150 | //ハウスホルダー変換1回目 |
Kotttaro | 0:4a5272e014d8 | 151 | MatrixXd A1(3, 3); |
Kotttaro | 0:4a5272e014d8 | 152 | A1 << 1.0, 0.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 153 | 0.0, 1.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 154 | 0.0, 0.0, 1.0; |
Kotttaro | 0:4a5272e014d8 | 155 | //printf("A1 was made\n"); |
Kotttaro | 0:4a5272e014d8 | 156 | s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ |
Kotttaro | 0:4a5272e014d8 | 157 | //printf("%f\n", s); |
Kotttaro | 0:4a5272e014d8 | 158 | MatrixXd H1(3, 3);//1回目の行列 |
Kotttaro | 0:4a5272e014d8 | 159 | MatrixXd X11(3, 1), X12(1, 3); |
Kotttaro | 0:4a5272e014d8 | 160 | Vector3d a11, a12;//a11が変換前,a12が変換後 |
Kotttaro | 0:4a5272e014d8 | 161 | // printf("H1,X11,X12,a11,a12 was made\n"); |
Kotttaro | 0:4a5272e014d8 | 162 | a11 << ma(0, 0), ma(1, 0), ma(2, 0); |
Kotttaro | 0:4a5272e014d8 | 163 | a12 << s, 0.0, 0.0; |
Kotttaro | 0:4a5272e014d8 | 164 | X11 = a11 - a12; |
Kotttaro | 0:4a5272e014d8 | 165 | X12 = X11.transpose(); |
Kotttaro | 0:4a5272e014d8 | 166 | //printf("H1,X11,X12,a11,a12 was calculated\n"); |
Kotttaro | 0:4a5272e014d8 | 167 | t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0)); |
Kotttaro | 0:4a5272e014d8 | 168 | //printf("%f\n", t);//ok |
Kotttaro | 0:4a5272e014d8 | 169 | H1 = A1 - 2.0 * (X11 * X12) / (t * t); |
Kotttaro | 0:4a5272e014d8 | 170 | ma1 = H1 * ma; |
Kotttaro | 0:4a5272e014d8 | 171 | //2回目 |
Kotttaro | 0:4a5272e014d8 | 172 | MatrixXd H2(3, 3), A2(2, 2), h2(2, 2); |
Kotttaro | 0:4a5272e014d8 | 173 | A2 << 1.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 174 | 0.0, 1.0; |
Kotttaro | 0:4a5272e014d8 | 175 | Vector2d a21, a22; |
Kotttaro | 0:4a5272e014d8 | 176 | MatrixXd X21(2, 1), X22(1, 2); |
Kotttaro | 0:4a5272e014d8 | 177 | a21 << ma1(1, 1), ma1(2, 1); |
Kotttaro | 0:4a5272e014d8 | 178 | s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1)); |
Kotttaro | 0:4a5272e014d8 | 179 | //printf("%f\n", s);//ok |
Kotttaro | 0:4a5272e014d8 | 180 | a22 << s, 0; |
Kotttaro | 0:4a5272e014d8 | 181 | X21 = a21 - a22; |
Kotttaro | 0:4a5272e014d8 | 182 | X22 = X21.transpose(); |
Kotttaro | 0:4a5272e014d8 | 183 | t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0)); |
Kotttaro | 0:4a5272e014d8 | 184 | h2 = A2 - 2 * (X21 * X22) / (t * t); |
Kotttaro | 0:4a5272e014d8 | 185 | H2 << 1.0, 0.0, 0.0, |
Kotttaro | 0:4a5272e014d8 | 186 | 0.0, h2(0, 0), h2(0, 1), |
Kotttaro | 0:4a5272e014d8 | 187 | 0.0, h2(1, 0), h2(1, 1); |
Kotttaro | 0:4a5272e014d8 | 188 | R = H2 * ma1; |
Kotttaro | 0:4a5272e014d8 | 189 | //printf("%lf %lf %lf \n,", R0(0,2), R0(1,2), R0(2,2));//r22が0 ok |
Kotttaro | 0:4a5272e014d8 | 190 | //printf("\n"); |
Kotttaro | 0:4a5272e014d8 | 191 | MatrixXd H1T(3, 3), H2T(3, 3); |
Kotttaro | 0:4a5272e014d8 | 192 | H1T = H1.transpose(); |
Kotttaro | 0:4a5272e014d8 | 193 | H2T = H2.transpose(); |
Kotttaro | 0:4a5272e014d8 | 194 | Q = H1T * H2T; |
Kotttaro | 3:f824e4d8eef7 | 195 | |
Kotttaro | 0:4a5272e014d8 | 196 | |
Kotttaro | 0:4a5272e014d8 | 197 | } |
Kotttaro | 0:4a5272e014d8 | 198 | |
Kotttaro | 0:4a5272e014d8 | 199 | void vp(int leg) {//5年生の時に作成したもの |
Kotttaro | 0:4a5272e014d8 | 200 | double crosx, crosy, crosz; |
Kotttaro | 0:4a5272e014d8 | 201 | double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) }; |
Kotttaro | 0:4a5272e014d8 | 202 | double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) }; |
Kotttaro | 0:4a5272e014d8 | 203 | double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] }; |
Kotttaro | 0:4a5272e014d8 | 204 | if (Lin[2] != 0.0) |
Kotttaro | 0:4a5272e014d8 | 205 | { |
Kotttaro | 0:4a5272e014d8 | 206 | 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 | 207 | for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; } |
Kotttaro | 0:4a5272e014d8 | 208 | AP[2] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 209 | } |
Kotttaro | 0:4a5272e014d8 | 210 | crosx = AP[1] * wA[2] + (-AP[2]) * wA[1]; |
Kotttaro | 0:4a5272e014d8 | 211 | crosy = AP[2] * wA[0] + (-AP[0]) * wA[2]; |
Kotttaro | 0:4a5272e014d8 | 212 | crosz = AP[0] * wA[1] + (-AP[1]) * wA[0]; |
Kotttaro | 0:4a5272e014d8 | 213 | vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2]; |
Kotttaro | 0:4a5272e014d8 | 214 | //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]); |
Kotttaro | 0:4a5272e014d8 | 215 | //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 | 216 | //printf("vp finish\n"); |
Kotttaro | 0:4a5272e014d8 | 217 | } |
Kotttaro | 0:4a5272e014d8 | 218 | void fwd(int leg) { |
Kotttaro | 0:4a5272e014d8 | 219 | //printf("fwd start\n"); |
Kotttaro | 0:4a5272e014d8 | 220 | 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 | 221 | 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 | 222 | s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); |
Kotttaro | 0:4a5272e014d8 | 223 | tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x |
Kotttaro | 0:4a5272e014d8 | 224 | tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y |
Kotttaro | 0:4a5272e014d8 | 225 | tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z |
Kotttaro | 0:4a5272e014d8 | 226 | //printf("fwd finish\n"); |
Kotttaro | 0:4a5272e014d8 | 227 | } |
Kotttaro | 0:4a5272e014d8 | 228 | void Jac(int leg) { |
Kotttaro | 0:4a5272e014d8 | 229 | //printf("Jac start\n"); |
Kotttaro | 0:4a5272e014d8 | 230 | 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 | 231 | 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 | 232 | s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); |
Kotttaro | 0:4a5272e014d8 | 233 | Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); |
Kotttaro | 0:4a5272e014d8 | 234 | Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 235 | Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 236 | Jacbi[leg][0][3] = (-L[2] * s123) * c0; |
Kotttaro | 0:4a5272e014d8 | 237 | |
Kotttaro | 0:4a5272e014d8 | 238 | Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); |
Kotttaro | 0:4a5272e014d8 | 239 | Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 240 | Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 241 | Jacbi[leg][1][3] = (-L[2] * s123) * s0; |
Kotttaro | 0:4a5272e014d8 | 242 | |
Kotttaro | 0:4a5272e014d8 | 243 | Jacbi[leg][2][0] = 0.0; |
Kotttaro | 0:4a5272e014d8 | 244 | Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 245 | Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 246 | Jacbi[leg][2][3] = L[2] * c123; |
Kotttaro | 0:4a5272e014d8 | 247 | |
Kotttaro | 0:4a5272e014d8 | 248 | |
Kotttaro | 0:4a5272e014d8 | 249 | //printf("Jac finish\n"); |
Kotttaro | 0:4a5272e014d8 | 250 | }//ok |
Kotttaro | 0:4a5272e014d8 | 251 | void deff(int leg) { |
Kotttaro | 0:4a5272e014d8 | 252 | //printf(" 評価関数定義\n"); |
Kotttaro | 0:4a5272e014d8 | 253 | 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 | 254 | a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2])); |
Kotttaro | 0:4a5272e014d8 | 255 | a = a0 * (double)cos(fi); |
Kotttaro | 0:4a5272e014d8 | 256 | h = a * (1 / (double)cos(fi) - tan(fi)); |
Kotttaro | 0:4a5272e014d8 | 257 | X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身 |
Kotttaro | 0:4a5272e014d8 | 258 | //tan-1の分母分子 |
Kotttaro | 0:4a5272e014d8 | 259 | tan_u = tip[leg][2]; |
Kotttaro | 0:4a5272e014d8 | 260 | tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])); |
Kotttaro | 0:4a5272e014d8 | 261 | //printf("評価関数計算完了\n"); |
Kotttaro | 0:4a5272e014d8 | 262 | } |
Kotttaro | 0:4a5272e014d8 | 263 | void dfd(int leg) { |
Kotttaro | 0:4a5272e014d8 | 264 | //printf("評価関数微分\n"); |
Kotttaro | 0:4a5272e014d8 | 265 | 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 | 266 | 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 | 267 | 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 | 268 | double cfi=cos(fi),sfi=sin(fi); |
Kotttaro | 1:5c2562adca7d | 269 | double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2]; |
Kotttaro | 1:5c2562adca7d | 270 | |
Kotttaro | 1:5c2562adca7d | 271 | double df_da=1/cfi-tan(fi); |
Kotttaro | 1:5c2562adca7d | 272 | double df_dfi=a*(-sfi-1)/(cfi*cfi); |
Kotttaro | 1:5c2562adca7d | 273 | double da_dx=x*cfi/sqrt(x*x+y*y); |
Kotttaro | 1:5c2562adca7d | 274 | double da_dy=y*cfi/sqrt(x*x+y*y); |
Kotttaro | 1:5c2562adca7d | 275 | double da_dfi=-sqrt(x*x+y*y)*sfi; |
Kotttaro | 1:5c2562adca7d | 276 | double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); |
Kotttaro | 1:5c2562adca7d | 277 | double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); |
Kotttaro | 1:5c2562adca7d | 278 | double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z); |
Kotttaro | 1:5c2562adca7d | 279 | |
Kotttaro | 1:5c2562adca7d | 280 | 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 | 281 | +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); |
Kotttaro | 1:5c2562adca7d | 282 | |
Kotttaro | 1:5c2562adca7d | 283 | 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 | 284 | +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]); |
Kotttaro | 1:5c2562adca7d | 285 | |
Kotttaro | 1:5c2562adca7d | 286 | 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 | 287 | +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); |
Kotttaro | 1:5c2562adca7d | 288 | |
Kotttaro | 1:5c2562adca7d | 289 | 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 | 290 | +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]); |
Kotttaro | 1:5c2562adca7d | 291 | |
Kotttaro | 3:f824e4d8eef7 | 292 | //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 | 293 | } |
Kotttaro | 0:4a5272e014d8 | 294 | |
Kotttaro | 2:57237f0a4a34 | 295 | double fe(int leg,double dth3) { |
Kotttaro | 2:57237f0a4a34 | 296 | //brent法のための関数, 事前にdfdを実行してから使う |
Kotttaro | 3:f824e4d8eef7 | 297 | double dfd_nolm,th0_nolm,e=0.0; |
Kotttaro | 2:57237f0a4a34 | 298 | //∇hを正規化する |
Kotttaro | 3:f824e4d8eef7 | 299 | dfd(leg); |
Kotttaro | 2:57237f0a4a34 | 300 | dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]); |
Kotttaro | 2:57237f0a4a34 | 301 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 302 | dfdth[i]=dfdth[i]/dfd_nolm; |
Kotttaro | 2:57237f0a4a34 | 303 | } |
Kotttaro | 3:f824e4d8eef7 | 304 | //double nolm; |
Kotttaro | 3:f824e4d8eef7 | 305 | //nolm=dfdth[0]*dfdth[0]+dfdth[1]*dfdth[1]+dfdth[2]*dfdth[2]+dfdth[3]*dfdth[3]; |
Kotttaro | 3:f824e4d8eef7 | 306 | //pc2.printf("nolm=%lf\r\n",nolm); |
Kotttaro | 3:f824e4d8eef7 | 307 | //pc2.printf("(dfdth[0],dfdth[1],dfdth[2],dfdth[3],dfd_nolm) = (%lf,%lf,%lf,%lf,%lf)\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3],dfd_nolm); |
Kotttaro | 2:57237f0a4a34 | 308 | //正規化完了 |
Kotttaro | 2:57237f0a4a34 | 309 | |
Kotttaro | 2:57237f0a4a34 | 310 | solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 |
Kotttaro | 2:57237f0a4a34 | 311 | |
Kotttaro | 2:57237f0a4a34 | 312 | //dthベクトルを正規化 |
Kotttaro | 2:57237f0a4a34 | 313 | 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 | 314 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 315 | th0[leg][i] = th0[leg][i] / th0_nolm; |
Kotttaro | 2:57237f0a4a34 | 316 | } |
Kotttaro | 3:f824e4d8eef7 | 317 | //double nolm; |
Kotttaro | 3:f824e4d8eef7 | 318 | //nolm=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 | 3:f824e4d8eef7 | 319 | //pc2.printf("th0 nolm=%lf\r\n",nolm); |
Kotttaro | 2:57237f0a4a34 | 320 | for (int i = 0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 321 | e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]); |
Kotttaro | 3:f824e4d8eef7 | 322 | } |
Kotttaro | 3:f824e4d8eef7 | 323 | //pc2.printf("th0=%lf,th1=%lf,th2=%lf,th3=%lf\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]); |
Kotttaro | 3:f824e4d8eef7 | 324 | pc2.printf("(dth,e) = (%lf,%2.10lf)\r\n",dth3,e); |
Kotttaro | 2:57237f0a4a34 | 325 | return e;//eベクトルのノルムの2乗を返す |
Kotttaro | 0:4a5272e014d8 | 326 | |
Kotttaro | 2:57237f0a4a34 | 327 | } |
Kotttaro | 3:f824e4d8eef7 | 328 | double f(int leg,double dth3) { |
Kotttaro | 3:f824e4d8eef7 | 329 | double f_return=0.0; |
Kotttaro | 3:f824e4d8eef7 | 330 | dfd(leg); |
Kotttaro | 3:f824e4d8eef7 | 331 | solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 |
Kotttaro | 3:f824e4d8eef7 | 332 | pc2.printf("dfdth=(%lf,%lf,%lf,%lf)\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]); |
Kotttaro | 3:f824e4d8eef7 | 333 | pc2.printf("th0=(%lf,%lf,%lf,%lf)\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]); |
Kotttaro | 3:f824e4d8eef7 | 334 | f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3]; |
Kotttaro | 3:f824e4d8eef7 | 335 | pc2.printf("(dth,-f)=(%lf,%lf)\r\n",dth3,f_return); |
Kotttaro | 3:f824e4d8eef7 | 336 | return f_return;//eベクトルのノルムの2乗を返す |
Kotttaro | 3:f824e4d8eef7 | 337 | |
Kotttaro | 3:f824e4d8eef7 | 338 | } |
Kotttaro | 2:57237f0a4a34 | 339 | double brent(int leg,double min,double mid,double max,double tol,int discrimination) |
Kotttaro | 2:57237f0a4a34 | 340 | { |
Kotttaro | 2:57237f0a4a34 | 341 | int iter; |
Kotttaro | 2:57237f0a4a34 | 342 | double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin; |
Kotttaro | 2:57237f0a4a34 | 343 | double e=0.0; |
Kotttaro | 3:f824e4d8eef7 | 344 | //dfd(leg); |
Kotttaro | 2:57237f0a4a34 | 345 | a=(min < max ? min : max); |
Kotttaro | 2:57237f0a4a34 | 346 | b=(min > max ? min : max); |
Kotttaro | 2:57237f0a4a34 | 347 | x=w=v=mid; |
Kotttaro | 2:57237f0a4a34 | 348 | if(discrimination==0)fw=fv=fu=fe(leg,x); |
Kotttaro | 2:57237f0a4a34 | 349 | else if(discrimination==1)fw=fv=fu=num_nolm(leg,x); |
Kotttaro | 3:f824e4d8eef7 | 350 | else if(discrimination==2)fw=fv=fu=-f(leg,x); |
Kotttaro | 2:57237f0a4a34 | 351 | for(iter=1;iter<=ITMAX;iter++) |
Kotttaro | 2:57237f0a4a34 | 352 | { |
Kotttaro | 2:57237f0a4a34 | 353 | xm=0.5*(a+b); |
Kotttaro | 2:57237f0a4a34 | 354 | tol2=2.0*(tol1=tol*fabs(x)+ZEPS); |
Kotttaro | 2:57237f0a4a34 | 355 | if(fabs(x-xm)<=(tol2-0.5*(b-a))) |
Kotttaro | 2:57237f0a4a34 | 356 | { |
Kotttaro | 2:57237f0a4a34 | 357 | xmin=x; |
Kotttaro | 2:57237f0a4a34 | 358 | return xmin; |
Kotttaro | 2:57237f0a4a34 | 359 | } |
Kotttaro | 2:57237f0a4a34 | 360 | if(fabs(e)>tol1) |
Kotttaro | 2:57237f0a4a34 | 361 | { |
Kotttaro | 2:57237f0a4a34 | 362 | r=(x-w)*(fx-fv); |
Kotttaro | 2:57237f0a4a34 | 363 | q=(x-v)*(fx-fw); |
Kotttaro | 2:57237f0a4a34 | 364 | p=(x-v)*q-(x-w)*r; |
Kotttaro | 2:57237f0a4a34 | 365 | q=2.0*(q-r); |
Kotttaro | 2:57237f0a4a34 | 366 | if(q>0.0)p=-p; |
Kotttaro | 2:57237f0a4a34 | 367 | q=fabs(q); |
Kotttaro | 2:57237f0a4a34 | 368 | etemp=e; |
Kotttaro | 2:57237f0a4a34 | 369 | e=d; |
Kotttaro | 2:57237f0a4a34 | 370 | if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x)) |
Kotttaro | 2:57237f0a4a34 | 371 | { d=CGOLD*(e= (x>=xm ? a-x : b-x));} |
Kotttaro | 2:57237f0a4a34 | 372 | else |
Kotttaro | 2:57237f0a4a34 | 373 | { |
Kotttaro | 2:57237f0a4a34 | 374 | d=p/q; |
Kotttaro | 2:57237f0a4a34 | 375 | u=x+d; |
Kotttaro | 2:57237f0a4a34 | 376 | if(u-a < tol2 || b-u < tol2) |
Kotttaro | 2:57237f0a4a34 | 377 | {d=SIGN(tol1,xm-x);} |
Kotttaro | 2:57237f0a4a34 | 378 | } |
Kotttaro | 2:57237f0a4a34 | 379 | } |
Kotttaro | 2:57237f0a4a34 | 380 | else |
Kotttaro | 2:57237f0a4a34 | 381 | { |
Kotttaro | 2:57237f0a4a34 | 382 | d=CGOLD*(e= (x>=xm ? a-x : b-x)); |
Kotttaro | 2:57237f0a4a34 | 383 | } |
Kotttaro | 2:57237f0a4a34 | 384 | u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d)); |
Kotttaro | 2:57237f0a4a34 | 385 | if(discrimination==0)fu=fe(leg,x); |
Kotttaro | 2:57237f0a4a34 | 386 | else if(discrimination==1)fu=num_nolm(leg,x); |
Kotttaro | 3:f824e4d8eef7 | 387 | else if(discrimination==2)fu=-f(leg,x);//最大方向にしたいが,brent法は極小なので符号を変える |
Kotttaro | 2:57237f0a4a34 | 388 | if(fu <= fx) |
Kotttaro | 2:57237f0a4a34 | 389 | { |
Kotttaro | 2:57237f0a4a34 | 390 | if(u >= x)a=x; else b=x; |
Kotttaro | 2:57237f0a4a34 | 391 | SHIFT(v,w,x,u); |
Kotttaro | 2:57237f0a4a34 | 392 | SHIFT(fv,fw,fx,fu); |
Kotttaro | 2:57237f0a4a34 | 393 | } |
Kotttaro | 2:57237f0a4a34 | 394 | else{ |
Kotttaro | 2:57237f0a4a34 | 395 | if(u < x){a=u;} |
Kotttaro | 2:57237f0a4a34 | 396 | else {b=u;} |
Kotttaro | 2:57237f0a4a34 | 397 | if(fu <= fw || w==x) |
Kotttaro | 2:57237f0a4a34 | 398 | { |
Kotttaro | 2:57237f0a4a34 | 399 | v=w; |
Kotttaro | 2:57237f0a4a34 | 400 | w=u; |
Kotttaro | 2:57237f0a4a34 | 401 | fv=fw; |
Kotttaro | 2:57237f0a4a34 | 402 | fw=fu; |
Kotttaro | 2:57237f0a4a34 | 403 | } |
Kotttaro | 2:57237f0a4a34 | 404 | else if (fu <= fv || v==x || v==w) |
Kotttaro | 2:57237f0a4a34 | 405 | { |
Kotttaro | 2:57237f0a4a34 | 406 | v=u; |
Kotttaro | 2:57237f0a4a34 | 407 | fv=fu; |
Kotttaro | 2:57237f0a4a34 | 408 | } |
Kotttaro | 2:57237f0a4a34 | 409 | } |
Kotttaro | 2:57237f0a4a34 | 410 | |
Kotttaro | 2:57237f0a4a34 | 411 | } |
Kotttaro | 2:57237f0a4a34 | 412 | return xmin; |
Kotttaro | 2:57237f0a4a34 | 413 | } |
Kotttaro | 2:57237f0a4a34 | 414 | double SIGN(double x,double y) |
Kotttaro | 2:57237f0a4a34 | 415 | { |
Kotttaro | 2:57237f0a4a34 | 416 | double x_return; |
Kotttaro | 2:57237f0a4a34 | 417 | x_return=abs(x); |
Kotttaro | 2:57237f0a4a34 | 418 | if(y<0.0)x_return=-x_return; |
Kotttaro | 2:57237f0a4a34 | 419 | |
Kotttaro | 2:57237f0a4a34 | 420 | return x_return; |
Kotttaro | 2:57237f0a4a34 | 421 | } |
Kotttaro | 2:57237f0a4a34 | 422 | double num_nolm(int leg,double dth3) |
Kotttaro | 2:57237f0a4a34 | 423 | { |
Kotttaro | 2:57237f0a4a34 | 424 | double nolm_return=0.0; |
Kotttaro | 2:57237f0a4a34 | 425 | solve(leg,dth3,2); |
Kotttaro | 2:57237f0a4a34 | 426 | for(int i=0; i<4; i++ ) |
Kotttaro | 2:57237f0a4a34 | 427 | { |
Kotttaro | 2:57237f0a4a34 | 428 | nolm_return+=th0[leg][i]*th0[leg][i]; |
Kotttaro | 2:57237f0a4a34 | 429 | } |
Kotttaro | 2:57237f0a4a34 | 430 | //pc2.printf("%lf\r\n",nolm_return); |
Kotttaro | 2:57237f0a4a34 | 431 | //nolm_return=sqrt(nolm_return); |
Kotttaro | 2:57237f0a4a34 | 432 | return nolm_return; |
Kotttaro | 2:57237f0a4a34 | 433 | } |
Kotttaro | 0:4a5272e014d8 | 434 | void solve(double w3, int leg,int det) { |
Kotttaro | 0:4a5272e014d8 | 435 | //printf("後退代入関数開始\n"); |
Kotttaro | 0:4a5272e014d8 | 436 | double dth[4]; |
Kotttaro | 0:4a5272e014d8 | 437 | MatrixXd v_Q(3,1),QT(3,3); |
Kotttaro | 0:4a5272e014d8 | 438 | |
Kotttaro | 0:4a5272e014d8 | 439 | QT = Q.transpose(); |
Kotttaro | 0:4a5272e014d8 | 440 | //printf("Q転地完了\n"); |
Kotttaro | 0:4a5272e014d8 | 441 | v_Q = QT * vP[leg]*sampling; |
Kotttaro | 0:4a5272e014d8 | 442 | //printf("v_Q(%lf,%lf,%lf)\n", v_Q(0.0), v_Q(1.0), v_Q(2.0)); |
Kotttaro | 0:4a5272e014d8 | 443 | //printf("v_Q計算完了\n"); |
Kotttaro | 2:57237f0a4a34 | 444 | dth[3] = w3 ; |
Kotttaro | 0:4a5272e014d8 | 445 | //printf("dth3計算終了\n"); |
Kotttaro | 0:4a5272e014d8 | 446 | dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2)); |
Kotttaro | 0:4a5272e014d8 | 447 | //printf("dth2計算終了\n"); |
Kotttaro | 0:4a5272e014d8 | 448 | dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1)); |
Kotttaro | 0:4a5272e014d8 | 449 | //printf("dth1計算終了\n"); |
Kotttaro | 0:4a5272e014d8 | 450 | 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 | 451 | //printf("dth0計算終了\n"); |
Kotttaro | 0:4a5272e014d8 | 452 | //printf("dthすべて計算終了\n"); |
Kotttaro | 3:f824e4d8eef7 | 453 | pc2.printf("1:%lf 2:%lf 3:%lf 4:%lf \r\n",dth[0],dth[1],dth[2],dth[3]); |
Kotttaro | 0:4a5272e014d8 | 454 | if (det == 1) { |
Kotttaro | 0:4a5272e014d8 | 455 | for (int i=0; i < 4; i++) { |
Kotttaro | 2:57237f0a4a34 | 456 | th[leg][i] = th[leg][i] + dth[i]*sampling; |
Kotttaro | 3:f824e4d8eef7 | 457 | //pc2.printf("%d:%lf/ ",i,dth[i]); |
Kotttaro | 0:4a5272e014d8 | 458 | |
Kotttaro | 0:4a5272e014d8 | 459 | } |
Kotttaro | 3:f824e4d8eef7 | 460 | //pc2.printf("///\r\n"); |
Kotttaro | 0:4a5272e014d8 | 461 | } |
Kotttaro | 0:4a5272e014d8 | 462 | if (det == 2) { |
Kotttaro | 0:4a5272e014d8 | 463 | for (int u=0; u < 4; u++) { |
Kotttaro | 0:4a5272e014d8 | 464 | th0[leg][u] = dth[u]; |
Kotttaro | 0:4a5272e014d8 | 465 | } |
Kotttaro | 0:4a5272e014d8 | 466 | } |
Kotttaro | 0:4a5272e014d8 | 467 | //printf("後退代入終了\n"); |
Kotttaro | 0:4a5272e014d8 | 468 | } |
Kotttaro | 0:4a5272e014d8 | 469 |