2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
main.cpp
- Committer:
- Kotttaro
- Date:
- 2021-12-22
- Revision:
- 3:f824e4d8eef7
- Parent:
- 2:57237f0a4a34
- Child:
- 4:8a50c7822dac
File content as of revision 3:f824e4d8eef7:
#include "mbed.h" //特別研究Ⅰで用いたプログラム //ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する //brent法の部分は『numerical recipes in c』 参照 #include "Eigen/Geometry.h" #include "Eigen/Dense.h" #include <math.h> #define ITMAX 100 #define CGOLD 0.3819660 #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d); #define ZEPS 1.0e-10 //#pragma warning(disable: 4996) Serial pc2(USBTX,USBRX); Timer tim; int times= 1;//実行回数:実行時間は??秒 double PI =3.14159265358979323846264338327950288; using namespace Eigen; //以下変数定義 double r=50*PI/180;//斜面の傾き[°] double sampling=0.01;//δtの時間[s] double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる double tip[4][3];//足先座標 double con[4][3] = { 50.0, 50.0,0, -50.0, 50.0,0, -50.0,-50.0,0, 50.0,-50.0,0};//脚のコーナー座標,zは必ず0 double th[4][4] = { 45 * PI / 180,30 * PI / 180,-30 * PI / 180,-30 * PI / 180, 135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180, -135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180, -45 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180 }; double th0[4][4]= { 0.0,0.0,0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0, 0.0,0.0 }; //計算用の関節角度 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元 double a,a0, h,fi;//評価関数内の変数 fi=φ double X,tan_u, tan_d;//計算用 //ねじ軸 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ double Lin[3], L0[3], vin,v[3],wg[3],win,nol;//ねじ軸条件 double dfdth[4];//評価関数のナブラ //以下行列定義 MatrixXd Q(3, 3);//Q行列 MatrixXd R(3, 4);//R行列 Vector3d vP[4];//各脚の速度ベクトル void QR(int leg);//QR分解用関数,引数は脚番号 void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する void fwd(int leg);//順運動学より脚先の座標を導出する void Jac(int leg);//指定した脚のヤコビアンを計算 void deff(int leg);//評価関数計算, legは距離と傾きから指定する void dfd( int leg);//評価関数の勾配をとる double search(int leg);//最大のthetaを探索するための関数 void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数 double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用 double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f) double SIGN(double x,double y);//xにyの符号をつけたものを返す int main() { double t; pc2.baud(921600); int count = 0; //入力したねじ運動を換算する Lin[0] = 0.0; //ねじ軸x Lin[1] = 1.0; //ねじ軸y Lin[2] = 1.0;//ねじ軸z L0[0] = 0.0;//ねじ軸原点座標 L0[1] = 0.0; L0[2] = 0.0; vin = 5.0; win = 0.0; printf("\r\n\r\n"); nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]); for (int i = 0; i < 3; i++) { wg[i] = Lin[i] * win / nol; v[i] = Lin[i] * vin / nol; } for (int i=0; i < 4; i++) { fwd(i); vp(i); } //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0)); //times*δtの時間だけサーボを動かす tim.start(); for (int i = 0; i < times;i++){ count = count + 1; double dth; //printf("%d \n", count); fwd(0); vp(0); Jac(0); QR(0); deff(0); pc2.printf("////////////////\r\n"); f(0,3000); pc2.printf("////////////////\r\n"); f(0,-3000); pc2.printf("////////////////\r\n"); f(0,-40.0); pc2.printf("////////////////\r\n"); f(0,-500.0985); pc2.printf("////////////////\r\n"); f(0,0.0); pc2.printf("////////////////\r\n"); f(0,500.0985); pc2.printf("////////////////\r\n"); f(0,40.0); pc2.printf("////////////////\r\n"); //dth=brent(0,-500.0985,0.0,500.0985,0.001,0); //solve(dth, 0, 1); t=tim.read(); //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 ); //pc2.printf("%d %5.8lf\r\n",count ,dth); } t=tim.read(); //pc2.printf("%2.4lf\r\n",t); return 0; // ソフトの終了 } void QR(int leg) { //printf("QR start"); double s, t;//要素計算用 MatrixXd ma(3, 4), ma1(3, 4); ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3], Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3], Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3]; /*printf("Jac :%lf %lf %lf %lf\n", ma(0, 0), ma(0, 1), ma(0, 2), ma(0, 3)); printf(" %lf %lf %lf %lf\n", ma(1, 0), ma(1, 1), ma(1, 2), ma(1, 3)); printf(" %lf %lf %lf %lf\n", ma(2, 0), ma(2, 1), ma(2, 2), ma(2, 3));*/ //printf("ma was made\n"); //ハウスホルダー変換1回目 MatrixXd A1(3, 3); A1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; //printf("A1 was made\n"); s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ //printf("%f\n", s); MatrixXd H1(3, 3);//1回目の行列 MatrixXd X11(3, 1), X12(1, 3); Vector3d a11, a12;//a11が変換前,a12が変換後 // printf("H1,X11,X12,a11,a12 was made\n"); a11 << ma(0, 0), ma(1, 0), ma(2, 0); a12 << s, 0.0, 0.0; X11 = a11 - a12; X12 = X11.transpose(); //printf("H1,X11,X12,a11,a12 was calculated\n"); t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0)); //printf("%f\n", t);//ok H1 = A1 - 2.0 * (X11 * X12) / (t * t); ma1 = H1 * ma; //2回目 MatrixXd H2(3, 3), A2(2, 2), h2(2, 2); A2 << 1.0, 0.0, 0.0, 1.0; Vector2d a21, a22; MatrixXd X21(2, 1), X22(1, 2); a21 << ma1(1, 1), ma1(2, 1); s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1)); //printf("%f\n", s);//ok a22 << s, 0; X21 = a21 - a22; X22 = X21.transpose(); t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0)); h2 = A2 - 2 * (X21 * X22) / (t * t); H2 << 1.0, 0.0, 0.0, 0.0, h2(0, 0), h2(0, 1), 0.0, h2(1, 0), h2(1, 1); R = H2 * ma1; //printf("%lf %lf %lf \n,", R0(0,2), R0(1,2), R0(2,2));//r22が0 ok //printf("\n"); MatrixXd H1T(3, 3), H2T(3, 3); H1T = H1.transpose(); H2T = H2.transpose(); Q = H1T * H2T; } void vp(int leg) {//5年生の時に作成したもの double crosx, crosy, crosz; double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) }; double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) }; double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] }; if (Lin[2] != 0.0) { double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 }; for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; } AP[2] = 0.0; } crosx = AP[1] * wA[2] + (-AP[2]) * wA[1]; crosy = AP[2] * wA[0] + (-AP[0]) * wA[2]; crosz = AP[0] * wA[1] + (-AP[1]) * wA[0]; vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2]; //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]); //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0)); //printf("vp finish\n"); } void fwd(int leg) { //printf("fwd start\n"); 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]), 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]), s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z //printf("fwd finish\n"); } void Jac(int leg) { //printf("Jac start\n"); 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]), 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]), s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]); Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0; Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0; Jacbi[leg][0][3] = (-L[2] * s123) * c0; Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123); Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0; Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0; Jacbi[leg][1][3] = (-L[2] * s123) * s0; Jacbi[leg][2][0] = 0.0; Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123; Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123; Jacbi[leg][2][3] = L[2] * c123; //printf("Jac finish\n"); }//ok void deff(int leg) { //printf(" 評価関数定義\n"); fi = r + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順 a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2])); a = a0 * (double)cos(fi); h = a * (1 / (double)cos(fi) - tan(fi)); X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身 //tan-1の分母分子 tan_u = tip[leg][2]; tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])); //printf("評価関数計算完了\n"); } void dfd(int leg) { //printf("評価関数微分\n"); 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]); 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]); 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]); double cfi=cos(fi),sfi=sin(fi); double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2]; double df_da=1/cfi-tan(fi); double df_dfi=a*(-sfi-1)/(cfi*cfi); double da_dx=x*cfi/sqrt(x*x+y*y); double da_dy=y*cfi/sqrt(x*x+y*y); double da_dfi=-sqrt(x*x+y*y)*sfi; double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y)); double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z); 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])) +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); 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])) +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]); 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])) +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); 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])) +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]); //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); } double fe(int leg,double dth3) { //brent法のための関数, 事前にdfdを実行してから使う double dfd_nolm,th0_nolm,e=0.0; //∇hを正規化する dfd(leg); dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]); for (int i = 0; i < 4; i++) { dfdth[i]=dfdth[i]/dfd_nolm; } //double nolm; //nolm=dfdth[0]*dfdth[0]+dfdth[1]*dfdth[1]+dfdth[2]*dfdth[2]+dfdth[3]*dfdth[3]; //pc2.printf("nolm=%lf\r\n",nolm); //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); //正規化完了 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 //dthベクトルを正規化 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]); for (int i = 0; i < 4; i++) { th0[leg][i] = th0[leg][i] / th0_nolm; } //double nolm; //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]; //pc2.printf("th0 nolm=%lf\r\n",nolm); for (int i = 0; i < 4; i++) { e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]); } //pc2.printf("th0=%lf,th1=%lf,th2=%lf,th3=%lf\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]); pc2.printf("(dth,e) = (%lf,%2.10lf)\r\n",dth3,e); return e;//eベクトルのノルムの2乗を返す } double f(int leg,double dth3) { double f_return=0.0; dfd(leg); solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出 pc2.printf("dfdth=(%lf,%lf,%lf,%lf)\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]); pc2.printf("th0=(%lf,%lf,%lf,%lf)\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]); f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3]; pc2.printf("(dth,-f)=(%lf,%lf)\r\n",dth3,f_return); return f_return;//eベクトルのノルムの2乗を返す } double brent(int leg,double min,double mid,double max,double tol,int discrimination) { int iter; double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin; double e=0.0; //dfd(leg); a=(min < max ? min : max); b=(min > max ? min : max); x=w=v=mid; if(discrimination==0)fw=fv=fu=fe(leg,x); else if(discrimination==1)fw=fv=fu=num_nolm(leg,x); else if(discrimination==2)fw=fv=fu=-f(leg,x); for(iter=1;iter<=ITMAX;iter++) { xm=0.5*(a+b); tol2=2.0*(tol1=tol*fabs(x)+ZEPS); if(fabs(x-xm)<=(tol2-0.5*(b-a))) { xmin=x; return xmin; } if(fabs(e)>tol1) { r=(x-w)*(fx-fv); q=(x-v)*(fx-fw); p=(x-v)*q-(x-w)*r; q=2.0*(q-r); if(q>0.0)p=-p; q=fabs(q); etemp=e; e=d; if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x)) { d=CGOLD*(e= (x>=xm ? a-x : b-x));} else { d=p/q; u=x+d; if(u-a < tol2 || b-u < tol2) {d=SIGN(tol1,xm-x);} } } else { d=CGOLD*(e= (x>=xm ? a-x : b-x)); } u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d)); if(discrimination==0)fu=fe(leg,x); else if(discrimination==1)fu=num_nolm(leg,x); else if(discrimination==2)fu=-f(leg,x);//最大方向にしたいが,brent法は極小なので符号を変える if(fu <= fx) { if(u >= x)a=x; else b=x; SHIFT(v,w,x,u); SHIFT(fv,fw,fx,fu); } else{ if(u < x){a=u;} else {b=u;} if(fu <= fw || w==x) { v=w; w=u; fv=fw; fw=fu; } else if (fu <= fv || v==x || v==w) { v=u; fv=fu; } } } return xmin; } double SIGN(double x,double y) { double x_return; x_return=abs(x); if(y<0.0)x_return=-x_return; return x_return; } double num_nolm(int leg,double dth3) { double nolm_return=0.0; solve(leg,dth3,2); for(int i=0; i<4; i++ ) { nolm_return+=th0[leg][i]*th0[leg][i]; } //pc2.printf("%lf\r\n",nolm_return); //nolm_return=sqrt(nolm_return); return nolm_return; } void solve(double w3, int leg,int det) { //printf("後退代入関数開始\n"); double dth[4]; MatrixXd v_Q(3,1),QT(3,3); QT = Q.transpose(); //printf("Q転地完了\n"); v_Q = QT * vP[leg]*sampling; //printf("v_Q(%lf,%lf,%lf)\n", v_Q(0.0), v_Q(1.0), v_Q(2.0)); //printf("v_Q計算完了\n"); dth[3] = w3 ; //printf("dth3計算終了\n"); dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2)); //printf("dth2計算終了\n"); dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1)); //printf("dth1計算終了\n"); 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)); //printf("dth0計算終了\n"); //printf("dthすべて計算終了\n"); pc2.printf("1:%lf 2:%lf 3:%lf 4:%lf \r\n",dth[0],dth[1],dth[2],dth[3]); if (det == 1) { for (int i=0; i < 4; i++) { th[leg][i] = th[leg][i] + dth[i]*sampling; //pc2.printf("%d:%lf/ ",i,dth[i]); } //pc2.printf("///\r\n"); } if (det == 2) { for (int u=0; u < 4; u++) { th0[leg][u] = dth[u]; } } //printf("後退代入終了\n"); }