Kotaro Irisawa / Mbed 2 deprecated 4dof_screw

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?

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 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