Kotaro Irisawa / Mbed 2 deprecated 4dof_screw

2021.12.22.16:06

Dependencies:   mbed pca9685_2021_12_22 Eigen

main.cpp@2:57237f0a4a34, 2021-12-22 (annotated)

Committer:
Kotttaro
Date:
Wed Dec 22 01:16:37 2021 +0000
Revision:
2:57237f0a4a34
Parent:
1:5c2562adca7d
Child:
3:f824e4d8eef7
brent 2021.12.22

Who changed what in which revision?

UserRevisionLine numberNew 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 2:57237f0a4a34 16 int times= 1000;//実行回数:実行時間は??秒
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 2:57237f0a4a34 64 double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム discrimination 0:谷側, 1:山側
Kotttaro 2:57237f0a4a34 65 double SIGN(double x,double y);//xにyの符号をつけたものを返す
Kotttaro 2:57237f0a4a34 66
Kotttaro 0:4a5272e014d8 67
Kotttaro 0:4a5272e014d8 68 int main()
Kotttaro 0:4a5272e014d8 69 {
Kotttaro 0:4a5272e014d8 70 double t;
Kotttaro 0:4a5272e014d8 71 pc2.baud(921600);
Kotttaro 0:4a5272e014d8 72 int count = 0;
Kotttaro 0:4a5272e014d8 73 //入力したねじ運動を換算する
Kotttaro 0:4a5272e014d8 74 Lin[0] = 0.0; //ねじ軸x
Kotttaro 2:57237f0a4a34 75 Lin[1] = 1.0; //ねじ軸y
Kotttaro 0:4a5272e014d8 76 Lin[2] = 1.0;//ねじ軸z
Kotttaro 0:4a5272e014d8 77 L0[0] = 0.0;//ねじ軸原点座標
Kotttaro 0:4a5272e014d8 78 L0[1] = 0.0;
Kotttaro 0:4a5272e014d8 79 L0[2] = 0.0;
Kotttaro 2:57237f0a4a34 80 vin = 5.0;
Kotttaro 2:57237f0a4a34 81 win = 0.0;
Kotttaro 2:57237f0a4a34 82 printf("\r\n\r\n");
Kotttaro 0:4a5272e014d8 83 nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
Kotttaro 0:4a5272e014d8 84 for (int i = 0; i < 3; i++)
Kotttaro 0:4a5272e014d8 85 {
Kotttaro 0:4a5272e014d8 86 wg[i] = Lin[i] * win / nol;
Kotttaro 0:4a5272e014d8 87 v[i] = Lin[i] * vin / nol;
Kotttaro 0:4a5272e014d8 88 }
Kotttaro 0:4a5272e014d8 89
Kotttaro 0:4a5272e014d8 90 for (int i=0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 91 fwd(i);
Kotttaro 0:4a5272e014d8 92 vp(i);
Kotttaro 0:4a5272e014d8 93 }
Kotttaro 0:4a5272e014d8 94 //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0));
Kotttaro 0:4a5272e014d8 95
Kotttaro 0:4a5272e014d8 96 //times*δtの時間だけサーボを動かす
Kotttaro 0:4a5272e014d8 97 tim.start();
Kotttaro 0:4a5272e014d8 98 for (int i = 0; i < times;i++){
Kotttaro 0:4a5272e014d8 99
Kotttaro 0:4a5272e014d8 100 count = count + 1;
Kotttaro 0:4a5272e014d8 101 double dth;
Kotttaro 1:5c2562adca7d 102 //printf("%d \n", count);
Kotttaro 0:4a5272e014d8 103 fwd(0);
Kotttaro 0:4a5272e014d8 104 vp(0);
Kotttaro 0:4a5272e014d8 105 Jac(0);
Kotttaro 0:4a5272e014d8 106 QR(0);
Kotttaro 0:4a5272e014d8 107 deff(0);
Kotttaro 2:57237f0a4a34 108 dth=brent(0,-5.0,1.90985,5.0,0.001,1);
Kotttaro 0:4a5272e014d8 109 solve(dth, 0, 1);
Kotttaro 0:4a5272e014d8 110
Kotttaro 0:4a5272e014d8 111 t=tim.read();
Kotttaro 2:57237f0a4a34 112 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 113 //pc2.printf("%d %5.8lf\r\n",count ,dth);
Kotttaro 0:4a5272e014d8 114 }
Kotttaro 1:5c2562adca7d 115 t=tim.read();
Kotttaro 2:57237f0a4a34 116 //pc2.printf("%2.4lf\r\n",t);
Kotttaro 0:4a5272e014d8 117 return 0; // ソフトの終了
Kotttaro 0:4a5272e014d8 118 }
Kotttaro 0:4a5272e014d8 119
Kotttaro 0:4a5272e014d8 120 void QR(int leg) {
Kotttaro 0:4a5272e014d8 121 //printf("QR start");
Kotttaro 0:4a5272e014d8 122
Kotttaro 0:4a5272e014d8 123 double s, t;//要素計算用
Kotttaro 0:4a5272e014d8 124 MatrixXd ma(3, 4), ma1(3, 4);
Kotttaro 0:4a5272e014d8 125
Kotttaro 0:4a5272e014d8 126 ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
Kotttaro 0:4a5272e014d8 127 Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
Kotttaro 0:4a5272e014d8 128 Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
Kotttaro 0:4a5272e014d8 129 /*printf("Jac :%lf %lf %lf %lf\n", ma(0, 0), ma(0, 1), ma(0, 2), ma(0, 3));
Kotttaro 0:4a5272e014d8 130 printf(" %lf %lf %lf %lf\n", ma(1, 0), ma(1, 1), ma(1, 2), ma(1, 3));
Kotttaro 0:4a5272e014d8 131 printf(" %lf %lf %lf %lf\n", ma(2, 0), ma(2, 1), ma(2, 2), ma(2, 3));*/
Kotttaro 0:4a5272e014d8 132 //printf("ma was made\n");
Kotttaro 0:4a5272e014d8 133 //ハウスホルダー変換1回目
Kotttaro 0:4a5272e014d8 134 MatrixXd A1(3, 3);
Kotttaro 0:4a5272e014d8 135 A1 << 1.0, 0.0, 0.0,
Kotttaro 0:4a5272e014d8 136 0.0, 1.0, 0.0,
Kotttaro 0:4a5272e014d8 137 0.0, 0.0, 1.0;
Kotttaro 0:4a5272e014d8 138 //printf("A1 was made\n");
Kotttaro 0:4a5272e014d8 139 s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ
Kotttaro 0:4a5272e014d8 140 //printf("%f\n", s);
Kotttaro 0:4a5272e014d8 141 MatrixXd H1(3, 3);//1回目の行列
Kotttaro 0:4a5272e014d8 142 MatrixXd X11(3, 1), X12(1, 3);
Kotttaro 0:4a5272e014d8 143 Vector3d a11, a12;//a11が変換前,a12が変換後
Kotttaro 0:4a5272e014d8 144 // printf("H1,X11,X12,a11,a12 was made\n");
Kotttaro 0:4a5272e014d8 145 a11 << ma(0, 0), ma(1, 0), ma(2, 0);
Kotttaro 0:4a5272e014d8 146 a12 << s, 0.0, 0.0;
Kotttaro 0:4a5272e014d8 147 X11 = a11 - a12;
Kotttaro 0:4a5272e014d8 148 X12 = X11.transpose();
Kotttaro 0:4a5272e014d8 149 //printf("H1,X11,X12,a11,a12 was calculated\n");
Kotttaro 0:4a5272e014d8 150 t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
Kotttaro 0:4a5272e014d8 151 //printf("%f\n", t);//ok
Kotttaro 0:4a5272e014d8 152 H1 = A1 - 2.0 * (X11 * X12) / (t * t);
Kotttaro 0:4a5272e014d8 153 ma1 = H1 * ma;
Kotttaro 0:4a5272e014d8 154 //2回目
Kotttaro 0:4a5272e014d8 155 MatrixXd H2(3, 3), A2(2, 2), h2(2, 2);
Kotttaro 0:4a5272e014d8 156 A2 << 1.0, 0.0,
Kotttaro 0:4a5272e014d8 157 0.0, 1.0;
Kotttaro 0:4a5272e014d8 158 Vector2d a21, a22;
Kotttaro 0:4a5272e014d8 159 MatrixXd X21(2, 1), X22(1, 2);
Kotttaro 0:4a5272e014d8 160 a21 << ma1(1, 1), ma1(2, 1);
Kotttaro 0:4a5272e014d8 161 s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
Kotttaro 0:4a5272e014d8 162 //printf("%f\n", s);//ok
Kotttaro 0:4a5272e014d8 163 a22 << s, 0;
Kotttaro 0:4a5272e014d8 164 X21 = a21 - a22;
Kotttaro 0:4a5272e014d8 165 X22 = X21.transpose();
Kotttaro 0:4a5272e014d8 166 t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0));
Kotttaro 0:4a5272e014d8 167 h2 = A2 - 2 * (X21 * X22) / (t * t);
Kotttaro 0:4a5272e014d8 168 H2 << 1.0, 0.0, 0.0,
Kotttaro 0:4a5272e014d8 169 0.0, h2(0, 0), h2(0, 1),
Kotttaro 0:4a5272e014d8 170 0.0, h2(1, 0), h2(1, 1);
Kotttaro 0:4a5272e014d8 171 R = H2 * ma1;
Kotttaro 0:4a5272e014d8 172 //printf("%lf %lf %lf \n,", R0(0,2), R0(1,2), R0(2,2));//r22が0 ok
Kotttaro 0:4a5272e014d8 173 //printf("\n");
Kotttaro 0:4a5272e014d8 174 MatrixXd H1T(3, 3), H2T(3, 3);
Kotttaro 0:4a5272e014d8 175 H1T = H1.transpose();
Kotttaro 0:4a5272e014d8 176 H2T = H2.transpose();
Kotttaro 0:4a5272e014d8 177 Q = H1T * H2T;
Kotttaro 0:4a5272e014d8 178 //printf("%lf %lf %lf \n,", R0(0, 0), R0(1, 1), R0(2, 2));
Kotttaro 0:4a5272e014d8 179 // printf("\n");
Kotttaro 0:4a5272e014d8 180 //printf("%lf %lf %lf \n,", R0(0, 2), R0(1, 2), R0(2, 2));//
Kotttaro 0:4a5272e014d8 181 /* R << R0(0, 0), R0(0, 1), R0(0, 2), Jacbi[leg][0][3],
Kotttaro 0:4a5272e014d8 182 R0(1, 0), R0(1, 1), R0(1, 2), Jacbi[leg][1][3],
Kotttaro 0:4a5272e014d8 183 R0(2, 0), R0(2, 1), R0(2, 2), Jacbi[leg][2][3];
Kotttaro 0:4a5272e014d8 184 //printf("%lf\n\n", Jacbi[leg][2][3]);
Kotttaro 0:4a5272e014d8 185 printf("R :%lf %lf %lf %lf\n", R(0, 0), R(0, 1), R(0, 2), R(0, 3));
Kotttaro 0:4a5272e014d8 186 printf(" %lf %lf %lf %lf\n", R(1, 0), R(1, 1), R(1, 2), R(1, 3));
Kotttaro 0:4a5272e014d8 187 printf( "%lf %lf %lf %lf\n\n", R(2, 0), R(2, 1), R(2, 2), R(2, 3));
Kotttaro 0:4a5272e014d8 188 printf("Q :%lf %lf %lf \n", Q(0, 0), Q(0, 1), Q(0, 2));
Kotttaro 0:4a5272e014d8 189 printf(" %lf %lf %lf \n", Q(1, 0), Q(1, 1), Q(1, 2));
Kotttaro 0:4a5272e014d8 190 printf(" %lf %lf %lf \n\n", Q(2, 0), Q(2, 1), Q(2, 2));
Kotttaro 0:4a5272e014d8 191
Kotttaro 0:4a5272e014d8 192 MatrixXd check(3, 4);
Kotttaro 0:4a5272e014d8 193 check = Q * R;
Kotttaro 0:4a5272e014d8 194 printf("Jac :%lf %lf %lf %lf\n", check(0, 0), check(0, 1), check(0, 2), check(0, 3));
Kotttaro 0:4a5272e014d8 195 printf(" %lf %lf %lf %lf\n", check(1, 0), check(1, 1), check(1, 2), check(1, 3));
Kotttaro 0:4a5272e014d8 196 printf(" %lf %lf %lf %lf\n\n", check(2, 0), check(2, 1), check(2, 2), check(2, 3));
Kotttaro 0:4a5272e014d8 197
Kotttaro 0:4a5272e014d8 198
Kotttaro 0:4a5272e014d8 199
Kotttaro 0:4a5272e014d8 200 // printf("\n");
Kotttaro 0:4a5272e014d8 201 //printf("QR finishued\n");
Kotttaro 0:4a5272e014d8 202 //double R2[4];
Kotttaro 0:4a5272e014d8 203
Kotttaro 0:4a5272e014d8 204 //R2[0] = R(0, 0);
Kotttaro 0:4a5272e014d8 205 //R2[0] = R(1, 1);
Kotttaro 0:4a5272e014d8 206 //R2[0] = R(2, 2);
Kotttaro 0:4a5272e014d8 207 //R2[0] = R(2, 3);
Kotttaro 0:4a5272e014d8 208 //printf("Rの値 %lf %lf %lf %lf\n,", R2[0], R2[1], R2[2], R2[3]);*/
Kotttaro 0:4a5272e014d8 209
Kotttaro 0:4a5272e014d8 210 }
Kotttaro 0:4a5272e014d8 211
Kotttaro 0:4a5272e014d8 212 void vp(int leg) {//5年生の時に作成したもの
Kotttaro 0:4a5272e014d8 213 double crosx, crosy, crosz;
Kotttaro 0:4a5272e014d8 214 double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) };
Kotttaro 0:4a5272e014d8 215 double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) };
Kotttaro 0:4a5272e014d8 216 double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] };
Kotttaro 0:4a5272e014d8 217 if (Lin[2] != 0.0)
Kotttaro 0:4a5272e014d8 218 {
Kotttaro 0:4a5272e014d8 219 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 220 for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; }
Kotttaro 0:4a5272e014d8 221 AP[2] = 0.0;
Kotttaro 0:4a5272e014d8 222 }
Kotttaro 0:4a5272e014d8 223 crosx = AP[1] * wA[2] + (-AP[2]) * wA[1];
Kotttaro 0:4a5272e014d8 224 crosy = AP[2] * wA[0] + (-AP[0]) * wA[2];
Kotttaro 0:4a5272e014d8 225 crosz = AP[0] * wA[1] + (-AP[1]) * wA[0];
Kotttaro 0:4a5272e014d8 226 vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2];
Kotttaro 0:4a5272e014d8 227 //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]);
Kotttaro 0:4a5272e014d8 228 //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 229 //printf("vp finish\n");
Kotttaro 0:4a5272e014d8 230 }
Kotttaro 0:4a5272e014d8 231 void fwd(int leg) {
Kotttaro 0:4a5272e014d8 232 //printf("fwd start\n");
Kotttaro 0:4a5272e014d8 233 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 234 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 235 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:4a5272e014d8 236 tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x
Kotttaro 0:4a5272e014d8 237 tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y
Kotttaro 0:4a5272e014d8 238 tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z
Kotttaro 0:4a5272e014d8 239 //printf("fwd finish\n");
Kotttaro 0:4a5272e014d8 240 }
Kotttaro 0:4a5272e014d8 241 void Jac(int leg) {
Kotttaro 0:4a5272e014d8 242 //printf("Jac start\n");
Kotttaro 0:4a5272e014d8 243 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 244 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 245 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:4a5272e014d8 246 Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:4a5272e014d8 247 Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 248 Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 249 Jacbi[leg][0][3] = (-L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 250
Kotttaro 0:4a5272e014d8 251 Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:4a5272e014d8 252 Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 253 Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 254 Jacbi[leg][1][3] = (-L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 255
Kotttaro 0:4a5272e014d8 256 Jacbi[leg][2][0] = 0.0;
Kotttaro 0:4a5272e014d8 257 Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 258 Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 259 Jacbi[leg][2][3] = L[2] * c123;
Kotttaro 0:4a5272e014d8 260
Kotttaro 0:4a5272e014d8 261
Kotttaro 0:4a5272e014d8 262 //printf("Jac finish\n");
Kotttaro 0:4a5272e014d8 263 }//ok
Kotttaro 0:4a5272e014d8 264 void deff(int leg) {
Kotttaro 0:4a5272e014d8 265 //printf(" 評価関数定義\n");
Kotttaro 0:4a5272e014d8 266 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 267 a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2]));
Kotttaro 0:4a5272e014d8 268 a = a0 * (double)cos(fi);
Kotttaro 0:4a5272e014d8 269 h = a * (1 / (double)cos(fi) - tan(fi));
Kotttaro 0:4a5272e014d8 270 X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身
Kotttaro 0:4a5272e014d8 271 //tan-1の分母分子
Kotttaro 0:4a5272e014d8 272 tan_u = tip[leg][2];
Kotttaro 0:4a5272e014d8 273 tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]));
Kotttaro 0:4a5272e014d8 274 //printf("評価関数計算完了\n");
Kotttaro 0:4a5272e014d8 275 }
Kotttaro 0:4a5272e014d8 276 void dfd(int leg) {
Kotttaro 0:4a5272e014d8 277 //printf("評価関数微分\n");
Kotttaro 0:4a5272e014d8 278 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 279 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 280 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 281 double cfi=cos(fi),sfi=sin(fi);
Kotttaro 1:5c2562adca7d 282 double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
Kotttaro 1:5c2562adca7d 283
Kotttaro 1:5c2562adca7d 284 double df_da=1/cfi-tan(fi);
Kotttaro 1:5c2562adca7d 285 double df_dfi=a*(-sfi-1)/(cfi*cfi);
Kotttaro 1:5c2562adca7d 286 double da_dx=x*cfi/sqrt(x*x+y*y);
Kotttaro 1:5c2562adca7d 287 double da_dy=y*cfi/sqrt(x*x+y*y);
Kotttaro 1:5c2562adca7d 288 double da_dfi=-sqrt(x*x+y*y)*sfi;
Kotttaro 1:5c2562adca7d 289 double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 1:5c2562adca7d 290 double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 1:5c2562adca7d 291 double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
Kotttaro 1:5c2562adca7d 292
Kotttaro 1:5c2562adca7d 293 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 294 +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]);
Kotttaro 1:5c2562adca7d 295
Kotttaro 1:5c2562adca7d 296 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 297 +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
Kotttaro 1:5c2562adca7d 298
Kotttaro 1:5c2562adca7d 299 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 300 +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]);
Kotttaro 1:5c2562adca7d 301
Kotttaro 1:5c2562adca7d 302 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 303 +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);
Kotttaro 1:5c2562adca7d 304
Kotttaro 1:5c2562adca7d 305
Kotttaro 1:5c2562adca7d 306 /////////////////////////////以下専攻科1年/////////////////////////////////////////////////////////////////////////////////////////////
Kotttaro 1:5c2562adca7d 307 /*double dadth[4];
Kotttaro 0:4a5272e014d8 308 double daindth[4];
Kotttaro 0:4a5272e014d8 309
Kotttaro 0:4a5272e014d8 310
Kotttaro 0:4a5272e014d8 311 double cfi = (double)cos(fi), sfi = (double)sin(fi);
Kotttaro 0:4a5272e014d8 312
Kotttaro 0:4a5272e014d8 313 //aの中身だけの微分
Kotttaro 0:4a5272e014d8 314 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 315 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 316 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 317 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 318
Kotttaro 0:4a5272e014d8 319
Kotttaro 0:4a5272e014d8 320 //fiの微分
Kotttaro 0:4a5272e014d8 321 //tan-1の分母分子それぞれの微分に分ける
Kotttaro 0:4a5272e014d8 322 //分母
Kotttaro 0:4a5272e014d8 323 double dtandth_d[4];
Kotttaro 0:4a5272e014d8 324 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 325 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 326 - 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 327 + 2 * L[1] * L[2] * sin(2*th[leg][1]+2*th[leg][2]+th[leg][3])
Kotttaro 0:4a5272e014d8 328 - 2 * L[3] * L[0] * s1 - 2 * L[3] * L[1] * s12 - 2 * L[3] * L[2] * s123
Kotttaro 0:4a5272e014d8 329 + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[0] * s1 - L[1] * s12 - L[2] * s123))/(2*tan_d);
Kotttaro 0:4a5272e014d8 330 dtandth_d[2] =( 2 * L[1] * L[1] * c12 * (-s12) + 2 * L[2] * L[2] * c123 * (-s123)
Kotttaro 0:4a5272e014d8 331 - 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 332 - 2 * L[3] * L[1] * s12 - 2 * L[3] * L[2] * s123
Kotttaro 0:4a5272e014d8 333 + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[1] * s12 - L[2] * s123)) / (2 * tan_d);
Kotttaro 0:4a5272e014d8 334 dtandth_d[3] = (2 * L[2]* L[2] * c123 * (-s123) - 2 * L[0] * L[2] * c1 * s123
Kotttaro 0:4a5272e014d8 335 - 2 * L[1] * L[2] * c12 * s123 - 2 * L[3] * L[2] * s123
Kotttaro 0:4a5272e014d8 336 + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[2]*s123)) / (2 * tan_d);
Kotttaro 0:4a5272e014d8 337
Kotttaro 0:4a5272e014d8 338 //分子
Kotttaro 0:4a5272e014d8 339 double dtandth_u[4];
Kotttaro 0:4a5272e014d8 340 dtandth_u[0] = 0;//ok 2.21
Kotttaro 0:4a5272e014d8 341 dtandth_u[1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 342 dtandth_u[2] = L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 343 dtandth_u[3] = L[2] * c123;
Kotttaro 0:4a5272e014d8 344
Kotttaro 0:4a5272e014d8 345
Kotttaro 0:4a5272e014d8 346 //tan-1の商の微分=Φの微分
Kotttaro 0:4a5272e014d8 347 double dfidth[4];
Kotttaro 0:4a5272e014d8 348 dfidth[0] = (1 / (X * X + 1)) * (dtandth_u[0] * tan_d - dtandth_d[0] * tan_u) / (tan_d * tan_d);
Kotttaro 0:4a5272e014d8 349 dfidth[1] = (1 / (X * X + 1)) * (dtandth_u[1] * tan_d - dtandth_d[1] * tan_u) / (tan_d * tan_d);
Kotttaro 0:4a5272e014d8 350 dfidth[2] = (1 / (X * X + 1)) * (dtandth_u[2] * tan_d - dtandth_d[2] * tan_u) / (tan_d * tan_d);
Kotttaro 0:4a5272e014d8 351 dfidth[3] = (1 / (X * X + 1)) * (dtandth_u[3] * tan_d - dtandth_d[3] * tan_u) / (tan_d * tan_d);
Kotttaro 0:4a5272e014d8 352
Kotttaro 0:4a5272e014d8 353 //総合したaの微分
Kotttaro 0:4a5272e014d8 354 dadth[0] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[0] + (double)sqrt(a0) * (-sfi) * dfidth[0];
Kotttaro 0:4a5272e014d8 355 dadth[1] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[1] + (double)sqrt(a0) * (-sfi) * dfidth[1];
Kotttaro 0:4a5272e014d8 356 dadth[2] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[2] + (double)sqrt(a0) * (-sfi) * dfidth[2];
Kotttaro 0:4a5272e014d8 357 dadth[3] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[3] + (double)sqrt(a0) * (-sfi) * dfidth[3];
Kotttaro 0:4a5272e014d8 358 //aの微分終わり
Kotttaro 0:4a5272e014d8 359
Kotttaro 0:4a5272e014d8 360 //fの微分
Kotttaro 0:4a5272e014d8 361 dfdth[0] = dadth[0] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[0] + a / (cfi * cfi) * dfidth[0]);
Kotttaro 0:4a5272e014d8 362 dfdth[1] = dadth[1] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[1] + a / (cfi * cfi) * dfidth[1]);
Kotttaro 0:4a5272e014d8 363 dfdth[2] = dadth[2] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[2] + a / (cfi * cfi) * dfidth[2]);
Kotttaro 0:4a5272e014d8 364 dfdth[3] = dadth[3] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[3] + a / (cfi * cfi) * dfidth[3]);
Kotttaro 1:5c2562adca7d 365 */
Kotttaro 1:5c2562adca7d 366 //pc2.printf("%lf, %lf, %lf, %lf\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]);
Kotttaro 0:4a5272e014d8 367 //printf("評価関数微分完了\n");
Kotttaro 0:4a5272e014d8 368 }
Kotttaro 0:4a5272e014d8 369 double search(int leg) {
Kotttaro 0:4a5272e014d8 370 //printf("探索関数開始\n");
Kotttaro 0:4a5272e014d8 371 //th3の角速度のみを変更し, ∇hが最大となるところを探索する
Kotttaro 0:4a5272e014d8 372 double dth=-500.0*PI/180;
Kotttaro 0:4a5272e014d8 373 //double comp_dfdth[4] = {0.0,0.0 ,0.0 ,0.0 };
Kotttaro 0:4a5272e014d8 374 //double dfdth_sum=0.0, dfdth_sum_before=0.0;
Kotttaro 0:4a5272e014d8 375 double dth_return=0.0;
Kotttaro 0:4a5272e014d8 376 double e=500.0;
Kotttaro 1:5c2562adca7d 377 double e_min = 100000000000000000000000000.0;
Kotttaro 0:4a5272e014d8 378 double dfd_nolm = 0.0;
Kotttaro 0:4a5272e014d8 379 dfd(leg);
Kotttaro 0:4a5272e014d8 380 dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
Kotttaro 0:4a5272e014d8 381 //正規化
Kotttaro 0:4a5272e014d8 382 for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 383 dfdth[i]=dfdth[i]/dfd_nolm;
Kotttaro 0:4a5272e014d8 384 }
Kotttaro 0:4a5272e014d8 385 //printf("%lf, %lf, %lf, %lf\n", dfdth[0], dfdth[1], dfdth[2], dfdth[3]);
Kotttaro 0:4a5272e014d8 386
Kotttaro 0:4a5272e014d8 387
Kotttaro 0:4a5272e014d8 388 //以下総当たりの探索for文
Kotttaro 0:4a5272e014d8 389 //0.5度ずつでdthをずらしながら2000回の探索を行う
Kotttaro 1:5c2562adca7d 390 for (int i = 0; i < 1000; i++) {
Kotttaro 0:4a5272e014d8 391 double th0_nolm = 0.0;
Kotttaro 1:5c2562adca7d 392 dth = dth + (double)(1.0 * PI / 180);
Kotttaro 0:4a5272e014d8 393 solve(dth, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 0:4a5272e014d8 394 e = 0.0;
Kotttaro 0:4a5272e014d8 395 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 396 //dthベクトルを正規化
Kotttaro 0:4a5272e014d8 397 for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 398 th0[leg][i] = th0[leg][i] / th0_nolm;
Kotttaro 0:4a5272e014d8 399 }
Kotttaro 0:4a5272e014d8 400 //printf("%lf, %lf, %lf, %lf\n", th0[leg][0], th0[leg][1], th0[leg][2], th0[leg][3]);
Kotttaro 0:4a5272e014d8 401 //評価関数の勾配を単位化したベクトルとの差分のベクトルのノルムの2乗を計算
Kotttaro 0:4a5272e014d8 402 for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 403 e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
Kotttaro 0:4a5272e014d8 404 }
Kotttaro 0:4a5272e014d8 405 //評価関数の勾配方向に最も近いもの、つまり上のfor文の計算結果の値の一番小さいものを採用する
Kotttaro 0:4a5272e014d8 406 //printf("%d, %lf\n", i, e);
Kotttaro 0:4a5272e014d8 407 if (e_min > e) {
Kotttaro 0:4a5272e014d8 408 e_min = e;
Kotttaro 0:4a5272e014d8 409
Kotttaro 0:4a5272e014d8 410 dth_return = dth;
Kotttaro 0:4a5272e014d8 411
Kotttaro 0:4a5272e014d8 412 }
Kotttaro 0:4a5272e014d8 413
Kotttaro 0:4a5272e014d8 414
Kotttaro 0:4a5272e014d8 415 }
Kotttaro 0:4a5272e014d8 416
Kotttaro 0:4a5272e014d8 417 //}
Kotttaro 0:4a5272e014d8 418 //else{
Kotttaro 0:4a5272e014d8 419 // for (int i = 0; i < 500; i++) {
Kotttaro 0:4a5272e014d8 420 //printf("探索ループ内");
Kotttaro 0:4a5272e014d8 421 // dth = dth + 0.5 * PI / 180;
Kotttaro 0:4a5272e014d8 422 //solve(dth, leg, 2);
Kotttaro 0:4a5272e014d8 423 //for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 424 //th0_nolm += th0[leg][i] * th0[leg][i];
Kotttaro 0:4a5272e014d8 425 //}
Kotttaro 0:4a5272e014d8 426 //正規化
Kotttaro 0:4a5272e014d8 427 //for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 428 //th0[leg][i] = th0[leg][i] / th0_nolm;
Kotttaro 0:4a5272e014d8 429 //}
Kotttaro 0:4a5272e014d8 430 //for (int i = 0; i < 4; i++) {
Kotttaro 0:4a5272e014d8 431 //e += (th[leg][i] - dfdth[i]) * (th[leg][i] - dfdth[i]);
Kotttaro 0:4a5272e014d8 432 //}
Kotttaro 0:4a5272e014d8 433 //if (e_min > e) {
Kotttaro 0:4a5272e014d8 434 //e_min = e;
Kotttaro 0:4a5272e014d8 435 //printf("%f", e_min);
Kotttaro 0:4a5272e014d8 436 //dth_return = dth;
Kotttaro 0:4a5272e014d8 437 //printf("%f\n", dth);
Kotttaro 0:4a5272e014d8 438 //}
Kotttaro 0:4a5272e014d8 439 //e = 0.0;
Kotttaro 0:4a5272e014d8 440
Kotttaro 0:4a5272e014d8 441 //}
Kotttaro 0:4a5272e014d8 442 //}
Kotttaro 0:4a5272e014d8 443 //printf("探索関数終了");
Kotttaro 0:4a5272e014d8 444 //printf("%lf\n", e_min);
Kotttaro 1:5c2562adca7d 445 //
Kotttaro 0:4a5272e014d8 446 //pc2.printf("%lf\n\r", dth_return);
Kotttaro 0:4a5272e014d8 447 return dth_return;
Kotttaro 0:4a5272e014d8 448
Kotttaro 0:4a5272e014d8 449 }
Kotttaro 2:57237f0a4a34 450 double fe(int leg,double dth3) {
Kotttaro 2:57237f0a4a34 451 //brent法のための関数, 事前にdfdを実行してから使う
Kotttaro 2:57237f0a4a34 452 double dfd_nolm,th0_nolm,e;
Kotttaro 2:57237f0a4a34 453
Kotttaro 2:57237f0a4a34 454 //∇hを正規化する
Kotttaro 2:57237f0a4a34 455 dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
Kotttaro 2:57237f0a4a34 456 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 457 dfdth[i]=dfdth[i]/dfd_nolm;
Kotttaro 2:57237f0a4a34 458 }
Kotttaro 2:57237f0a4a34 459 //正規化完了
Kotttaro 2:57237f0a4a34 460
Kotttaro 2:57237f0a4a34 461 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 2:57237f0a4a34 462
Kotttaro 2:57237f0a4a34 463 //dthベクトルを正規化
Kotttaro 2:57237f0a4a34 464 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 465 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 466 th0[leg][i] = th0[leg][i] / th0_nolm;
Kotttaro 2:57237f0a4a34 467 }
Kotttaro 2:57237f0a4a34 468 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 469 e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
Kotttaro 2:57237f0a4a34 470 }
Kotttaro 2:57237f0a4a34 471 return e;//eベクトルのノルムの2乗を返す
Kotttaro 0:4a5272e014d8 472
Kotttaro 2:57237f0a4a34 473 }
Kotttaro 2:57237f0a4a34 474 double brent(int leg,double min,double mid,double max,double tol,int discrimination)
Kotttaro 2:57237f0a4a34 475 {
Kotttaro 2:57237f0a4a34 476 int iter;
Kotttaro 2:57237f0a4a34 477 double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
Kotttaro 2:57237f0a4a34 478 double e=0.0;
Kotttaro 2:57237f0a4a34 479 dfd(leg);
Kotttaro 2:57237f0a4a34 480 a=(min < max ? min : max);
Kotttaro 2:57237f0a4a34 481 b=(min > max ? min : max);
Kotttaro 2:57237f0a4a34 482 x=w=v=mid;
Kotttaro 2:57237f0a4a34 483 if(discrimination==0)fw=fv=fu=fe(leg,x);
Kotttaro 2:57237f0a4a34 484 else if(discrimination==1)fw=fv=fu=num_nolm(leg,x);
Kotttaro 2:57237f0a4a34 485 for(iter=1;iter<=ITMAX;iter++)
Kotttaro 2:57237f0a4a34 486 {
Kotttaro 2:57237f0a4a34 487 xm=0.5*(a+b);
Kotttaro 2:57237f0a4a34 488 tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
Kotttaro 2:57237f0a4a34 489 if(fabs(x-xm)<=(tol2-0.5*(b-a)))
Kotttaro 2:57237f0a4a34 490 {
Kotttaro 2:57237f0a4a34 491 xmin=x;
Kotttaro 2:57237f0a4a34 492 return xmin;
Kotttaro 2:57237f0a4a34 493 }
Kotttaro 2:57237f0a4a34 494 if(fabs(e)>tol1)
Kotttaro 2:57237f0a4a34 495 {
Kotttaro 2:57237f0a4a34 496 r=(x-w)*(fx-fv);
Kotttaro 2:57237f0a4a34 497 q=(x-v)*(fx-fw);
Kotttaro 2:57237f0a4a34 498 p=(x-v)*q-(x-w)*r;
Kotttaro 2:57237f0a4a34 499 q=2.0*(q-r);
Kotttaro 2:57237f0a4a34 500 if(q>0.0)p=-p;
Kotttaro 2:57237f0a4a34 501 q=fabs(q);
Kotttaro 2:57237f0a4a34 502 etemp=e;
Kotttaro 2:57237f0a4a34 503 e=d;
Kotttaro 2:57237f0a4a34 504 if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x))
Kotttaro 2:57237f0a4a34 505 { d=CGOLD*(e= (x>=xm ? a-x : b-x));}
Kotttaro 2:57237f0a4a34 506 else
Kotttaro 2:57237f0a4a34 507 {
Kotttaro 2:57237f0a4a34 508 d=p/q;
Kotttaro 2:57237f0a4a34 509 u=x+d;
Kotttaro 2:57237f0a4a34 510 if(u-a < tol2 || b-u < tol2)
Kotttaro 2:57237f0a4a34 511 {d=SIGN(tol1,xm-x);}
Kotttaro 2:57237f0a4a34 512 }
Kotttaro 2:57237f0a4a34 513 }
Kotttaro 2:57237f0a4a34 514 else
Kotttaro 2:57237f0a4a34 515 {
Kotttaro 2:57237f0a4a34 516 d=CGOLD*(e= (x>=xm ? a-x : b-x));
Kotttaro 2:57237f0a4a34 517 }
Kotttaro 2:57237f0a4a34 518 u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
Kotttaro 2:57237f0a4a34 519 if(discrimination==0)fu=fe(leg,x);
Kotttaro 2:57237f0a4a34 520 else if(discrimination==1)fu=num_nolm(leg,x);
Kotttaro 2:57237f0a4a34 521 if(fu <= fx)
Kotttaro 2:57237f0a4a34 522 {
Kotttaro 2:57237f0a4a34 523 if(u >= x)a=x; else b=x;
Kotttaro 2:57237f0a4a34 524 SHIFT(v,w,x,u);
Kotttaro 2:57237f0a4a34 525 SHIFT(fv,fw,fx,fu);
Kotttaro 2:57237f0a4a34 526 }
Kotttaro 2:57237f0a4a34 527 else{
Kotttaro 2:57237f0a4a34 528 if(u < x){a=u;}
Kotttaro 2:57237f0a4a34 529 else {b=u;}
Kotttaro 2:57237f0a4a34 530 if(fu <= fw || w==x)
Kotttaro 2:57237f0a4a34 531 {
Kotttaro 2:57237f0a4a34 532 v=w;
Kotttaro 2:57237f0a4a34 533 w=u;
Kotttaro 2:57237f0a4a34 534 fv=fw;
Kotttaro 2:57237f0a4a34 535 fw=fu;
Kotttaro 2:57237f0a4a34 536 }
Kotttaro 2:57237f0a4a34 537 else if (fu <= fv || v==x || v==w)
Kotttaro 2:57237f0a4a34 538 {
Kotttaro 2:57237f0a4a34 539 v=u;
Kotttaro 2:57237f0a4a34 540 fv=fu;
Kotttaro 2:57237f0a4a34 541 }
Kotttaro 2:57237f0a4a34 542 }
Kotttaro 2:57237f0a4a34 543
Kotttaro 2:57237f0a4a34 544 }
Kotttaro 2:57237f0a4a34 545 return xmin;
Kotttaro 2:57237f0a4a34 546 }
Kotttaro 2:57237f0a4a34 547 double SIGN(double x,double y)
Kotttaro 2:57237f0a4a34 548 {
Kotttaro 2:57237f0a4a34 549 double x_return;
Kotttaro 2:57237f0a4a34 550 x_return=abs(x);
Kotttaro 2:57237f0a4a34 551 if(y<0.0)x_return=-x_return;
Kotttaro 2:57237f0a4a34 552
Kotttaro 2:57237f0a4a34 553 return x_return;
Kotttaro 2:57237f0a4a34 554 }
Kotttaro 2:57237f0a4a34 555 double num_nolm(int leg,double dth3)
Kotttaro 2:57237f0a4a34 556 {
Kotttaro 2:57237f0a4a34 557 double nolm_return=0.0;
Kotttaro 2:57237f0a4a34 558 solve(leg,dth3,2);
Kotttaro 2:57237f0a4a34 559 for(int i=0; i<4; i++ )
Kotttaro 2:57237f0a4a34 560 {
Kotttaro 2:57237f0a4a34 561 nolm_return+=th0[leg][i]*th0[leg][i];
Kotttaro 2:57237f0a4a34 562 }
Kotttaro 2:57237f0a4a34 563 //pc2.printf("%lf\r\n",nolm_return);
Kotttaro 2:57237f0a4a34 564 //nolm_return=sqrt(nolm_return);
Kotttaro 2:57237f0a4a34 565 return nolm_return;
Kotttaro 2:57237f0a4a34 566 }
Kotttaro 0:4a5272e014d8 567 void solve(double w3, int leg,int det) {
Kotttaro 0:4a5272e014d8 568 //printf("後退代入関数開始\n");
Kotttaro 0:4a5272e014d8 569 double dth[4];
Kotttaro 0:4a5272e014d8 570 MatrixXd v_Q(3,1),QT(3,3);
Kotttaro 0:4a5272e014d8 571
Kotttaro 0:4a5272e014d8 572 QT = Q.transpose();
Kotttaro 0:4a5272e014d8 573 //printf("Q転地完了\n");
Kotttaro 0:4a5272e014d8 574 v_Q = QT * vP[leg]*sampling;
Kotttaro 0:4a5272e014d8 575 //printf("v_Q(%lf,%lf,%lf)\n", v_Q(0.0), v_Q(1.0), v_Q(2.0));
Kotttaro 0:4a5272e014d8 576 //printf("v_Q計算完了\n");
Kotttaro 2:57237f0a4a34 577 dth[3] = w3 ;
Kotttaro 0:4a5272e014d8 578 //printf("dth3計算終了\n");
Kotttaro 0:4a5272e014d8 579 dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
Kotttaro 0:4a5272e014d8 580 //printf("dth2計算終了\n");
Kotttaro 0:4a5272e014d8 581 dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
Kotttaro 0:4a5272e014d8 582 //printf("dth1計算終了\n");
Kotttaro 0:4a5272e014d8 583 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 584 //printf("dth0計算終了\n");
Kotttaro 0:4a5272e014d8 585 //printf("dthすべて計算終了\n");
Kotttaro 0:4a5272e014d8 586 if (det == 1) {
Kotttaro 0:4a5272e014d8 587 for (int i=0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 588 th[leg][i] = th[leg][i] + dth[i]*sampling;
Kotttaro 0:4a5272e014d8 589
Kotttaro 0:4a5272e014d8 590 }
Kotttaro 0:4a5272e014d8 591 }
Kotttaro 0:4a5272e014d8 592 if (det == 2) {
Kotttaro 0:4a5272e014d8 593 for (int u=0; u < 4; u++) {
Kotttaro 0:4a5272e014d8 594 th0[leg][u] = dth[u];
Kotttaro 0:4a5272e014d8 595 }
Kotttaro 0:4a5272e014d8 596 }
Kotttaro 0:4a5272e014d8 597 //printf("後退代入終了\n");
Kotttaro 0:4a5272e014d8 598 }
Kotttaro 0:4a5272e014d8 599