Diff: main.cpp

Revision:

2:57237f0a4a34

Parent:

1:5c2562adca7d

Child:

3:f824e4d8eef7

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