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