2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 2:57237f0a4a34
- Parent:
- 1:5c2562adca7d
- Child:
- 3:f824e4d8eef7
diff -r 5c2562adca7d -r 57237f0a4a34 main.cpp --- a/main.cpp Tue Dec 21 06:28:22 2021 +0000 +++ b/main.cpp Wed Dec 22 01:16:37 2021 +0000 @@ -1,13 +1,19 @@ #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= 100;//実行回数:実行時間は7秒 +int times= 1000;//実行回数:実行時間は??秒 double PI =3.14159265358979323846264338327950288; using namespace Eigen; @@ -21,7 +27,7 @@ -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,-60 * PI / 180,-60 * PI / 180, +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 }; @@ -53,6 +59,11 @@ 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 brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム discrimination 0:谷側, 1:山側 +double SIGN(double x,double y);//xにyの符号をつけたものを返す + int main() { @@ -61,13 +72,14 @@ int count = 0; //入力したねじ運動を換算する Lin[0] = 0.0; //ねじ軸x - Lin[1] = 0.0; //ねじ軸y + Lin[1] = 1.0; //ねじ軸y Lin[2] = 1.0;//ねじ軸z L0[0] = 0.0;//ねじ軸原点座標 L0[1] = 0.0; L0[2] = 0.0; - vin = 0.0; - win = 5.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++) { @@ -93,16 +105,15 @@ Jac(0); QR(0); deff(0); - dth=search(0); + dth=brent(0,-5.0,1.90985,5.0,0.001,1); 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("%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); - + //pc2.printf("%2.4lf\r\n",t); return 0; // ソフトの終了 } @@ -436,7 +447,123 @@ return dth_return; } +double fe(int leg,double dth3) { + //brent法のための関数, 事前にdfdを実行してから使う + double dfd_nolm,th0_nolm,e; + + //∇hを正規化する + 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; + } + //正規化完了 + + 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; + } + for (int i = 0; i < 4; i++) { + e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]); + } + return e;//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); + 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); + 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]; @@ -447,7 +574,7 @@ 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 * sampling; + dth[3] = w3 ; //printf("dth3計算終了\n"); dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2)); //printf("dth2計算終了\n"); @@ -458,7 +585,7 @@ //printf("dthすべて計算終了\n"); if (det == 1) { for (int i=0; i < 4; i++) { - th[leg][i] = th[leg][i] + dth[i]; + th[leg][i] = th[leg][i] + dth[i]*sampling; } }