Kotaro Irisawa / Mbed 2 deprecated 4dof_screw

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?

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