main.cpp

00001 //特別研究Ⅰで用いたプログラムを改良したもの
00002 //ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する
00003 //brent法の部分は『numerical recipes in c』 参照
00004 #include "mbed.h"
00005 #include <PCA9685.h> 
00006 #include "Eigen/Geometry.h"
00007 #include "Eigen/Dense.h"
00008 #include <math.h>
00009 
00010 #define ITMAX 100
00011 #define CGOLD 0.3819660
00012 #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
00013 #define ZEPS 1.0e-10
00014 #define GOLD 1.618034
00015 #define TINY 1.0e-20
00016 #define GLIMIT 100.0
00017 #define SERVOMIN 700       
00018 #define SERVOMAX 2300
00019 #define SERVOGAIN 29.6296300
00020 #define PI 3.14159265358979323846264338327950288
00021 
00022 PCA9685 pwm;//クラス宣言
00023 Serial pc2(USBTX,USBRX);
00024 Timer tim;
00025 Timer manage;
00026 int times= 200;//実行回数:実行時間は (sampling)*(times)秒
00027 
00028 using namespace Eigen;
00029 
00030 //以下変数定義
00031 
00032 //brent法に必要な変数
00033 double ax=-0.1*PI/180.0,bx=0.1*PI/180.0,cx=0.0;
00034 double fa,fb,fc;
00035 //サーボの書き込みに必要な変数
00036 double servo0[16]={7700.0, 5850.0, 7050.0, 6850.0, 6000.0, 6300.0, 8400.0, 7200.0, 6700.0, 7000.0, 5650.0, 8400.0, 6000.0, 5100.0, 5600.0, 6570.0};//servoの初期値
00037 int ch[4][4]={{0 ,1 ,2 ,3} ,
00038               {4 ,5 ,6 ,7} ,
00039               {8 ,9 ,10,11},
00040               {12,13,14,15} };
00041 double r=0.0*PI/180;//斜面の傾き[°]
00042 double sampling=0.01;//δtの時間[s]
00043 double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
00044 double tip[4][3];//足先座標
00045 double con[4][3] = {  {-55.0, 55.0,0.0},
00046                       { 55.0, 55.0,0.0},
00047                       { 55.0,-55.0,0.0},
00048                       {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0
00049 double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
00050                           {45  * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
00051                           {-45  * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
00052                           {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態                   
00053 double th0[4][4]= { 0.0,0.0,0.0,0.0, 
00054                     0.0,0.0, 0.0,0.0, 
00055                     0.0,0.0, 0.0,0.0, 
00056                     0.0,0.0, 0.0,0.0 }; //計算用の関節角度
00057 double th[4][4];//サーボに入力する角度                    
00058 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元
00059 double a,a0, h,fi;//評価関数内の変数 fi=φ
00060 double X,tan_u, tan_d;//計算用
00061 
00062 //ねじ軸
00063 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ
00064 double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件 
00065 double dfdth[4];//評価関数のナブラ
00066 
00067 
00068 //以下行列定義
00069 MatrixXd Q(3, 3);//Q行列
00070 MatrixXd R(3, 4);//R行列
00071 Vector3d vP[4];//各脚の速度ベクトル
00072 
00073 
00074 void QR(int leg);//QR分解用関数,引数は脚番号
00075 void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する
00076 void fwd(int leg);//順運動学より脚先の座標を導出する
00077 void Jac(int leg);//指定した脚のヤコビアンを計算
00078 void deff(int leg);//評価関数計算, legは距離と傾きから指定する
00079 void dfd( int leg);//評価関数の勾配をとる
00080 double search(int leg);//最大のthetaを探索するための関数
00081 void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す
00082 double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの
00083 double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数
00084 double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用
00085 void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数
00086 double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム 
00087 //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test)
00088 double SIGN(double x,double y);//xにyの符号をつけたものを返す
00089 double FMAX(double x,double y);//大きいほうの値が返される
00090 double f_test(double x);//テスト用の関数
00091 
00092 //以下サーボ関係
00093 void setup_servo();//サーボセットアップ用関数
00094 void servo_write(int ch,double ang);//angに
00095 void servo_write7(int ch, double ang);
00096 void servo_calib();//全ての角度を0度にする
00097 void servo_ready();//初期状態
00098 
00099 int main()
00100 { 
00101     double t;
00102     pc2.baud(921600);
00103     //setup_servo();
00104     for(int u=0; u<4; u++)
00105     {
00106      for(int i=0; i<4; i++)
00107      {
00108         th[u][i]=th_ready[u][i];
00109         }
00110     }
00111     for(int i=0; i<4; i++)
00112      {
00113         fwd(i);
00114         }
00115     //servo_ready();
00116     wait(2);
00117     //while(1);
00118     /*for(int u=0; u<4; u++)
00119     {
00120      for(int i=0; i<4; i++)
00121      {
00122         servo_write(ch[u][i],th[u][i]);
00123         }
00124         }*/
00125     //while(1);    
00126     wait(2);    
00127     int count = 0;
00128     //入力したねじ運動を換算する
00129     Lin[0] = cos(30.0*PI/180); //ねじ軸x
00130     Lin[1] = sin(30.0*PI/180); //ねじ軸y
00131     Lin[2] = 0.0;//ねじ軸z
00132     L0[0] = 0.0;//ねじ軸原点座標
00133     L0[1] = 0.0;
00134     L0[2] = 0.0;
00135     vin = 0.0;
00136     win = 2.0;
00137     printf("\r\n\r\n");
00138     nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
00139     for (int i = 0; i < 3; i++)
00140     {
00141         wg[i] = Lin[i] * win / nol;
00142         v[i] = Lin[i] * vin / nol;
00143     }
00144     //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0));
00145 
00146     //times*δtの時間だけサーボを動かす
00147     tim.start();
00148     
00149     manage.start();
00150     //for (int i = 0; i < times;i++){
00151     while(tim.read()<=5.0) {   
00152         int l=2;
00153         //count = count + 1;
00154         double dth;
00155         //for(int l=0;l<4;l++)
00156         //{
00157             //////////計算部/////////////////
00158             t=tim.read();
00159             fwd(l);
00160             pc2.printf("%3.3lf %3.4lf %3.4lf %3.4lf\r\n",t,tip[l][0],tip[l][1],tip[l][2]);
00161             vp(l);
00162             Jac(l);
00163             QR(l);
00164             deff(l);
00165             dfd(l);
00166             ax=-0.15*PI/180.0;bx=0.15*PI/180.0;cx=0.0;
00167             //2:谷,1:山
00168             mnbrak(l,2);
00169             dth=brent(l,ax,bx,cx,0.01,2);
00170             solve(dth, l, 1);
00171             ////////////////////////////////
00172             ///*
00173       //}      
00174             /*for(int u=0; u<4; u++)
00175             {
00176               for(int i=0; i<4; i++)
00177               { 
00178                servo_write(ch[u][i],th[u][i]);
00179                }
00180              }//*/    
00181         //t=tim.read();
00182         //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][0]*180/PI,  th[1][0]*180/PI , th[2][0]*180/PI , th[3][0]*180/PI );
00183         //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][1]*180/PI,  th[1][1]*180/PI , th[2][1]*180/PI , th[3][1]*180/PI );
00184         //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][2]*180/PI,  th[1][2]*180/PI , th[2][2]*180/PI , th[3][2]*180/PI );
00185         //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][3]*180/PI,  th[1][3]*180/PI , th[2][3]*180/PI , th[3][3]*180/PI );
00186         //pc2.printf("%2.4lf:x( %3.3lf, %3.3lf, %3.3lf, %3.3lf )y( %3.3lf, %3.3lf, %3.3lf, %3.3lf )z( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",
00187         //t,tip[0][0],tip[1][0],tip[2][0],tip[3][0],tip[0][1],tip[1][1],tip[2][1],tip[3][1],tip[0][2],tip[1][2],tip[2][2],,tip[3][2]);
00188         //pc2.printf("\r\n");
00189         while(1)
00190         {
00191             if(manage.read()>sampling)break;
00192             }
00193         manage.reset();    
00194             
00195         }
00196         t=tim.read();
00197         wait(3);
00198         servo_ready();
00199     return 0;               // ソフトの終了 
00200 }
00201 
00202 void QR(int leg) {
00203     double s, t;//要素計算用
00204     MatrixXd ma(3, 4), ma1(3, 4);
00205     
00206     ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
00207         Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
00208         Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
00209     
00210     //ハウスホルダー変換1回目
00211     MatrixXd A1(3, 3);
00212     A1 << 1.0, 0.0, 0.0,
00213         0.0, 1.0, 0.0,
00214         0.0, 0.0, 1.0;
00215 
00216     s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ
00217 
00218     MatrixXd H1(3, 3);//1回目の行列
00219     MatrixXd X11(3, 1), X12(1, 3);
00220     Vector3d a11, a12;//a11が変換前,a12が変換後
00221     a11 << ma(0, 0), ma(1, 0), ma(2, 0);
00222     a12 << s, 0.0, 0.0;
00223     X11 = a11 - a12;
00224     X12 = X11.transpose();
00225     t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
00226     H1 = A1 - 2.0 * (X11 * X12) / (t * t);
00227     ma1 = H1 * ma;
00228     //2回目
00229     MatrixXd H2(3, 3), A2(2, 2), h2(2, 2);
00230     A2 << 1.0, 0.0,
00231         0.0, 1.0;
00232     Vector2d a21, a22;
00233     MatrixXd X21(2, 1), X22(1, 2);
00234     a21 << ma1(1, 1), ma1(2, 1);
00235     s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
00236     a22 << s, 0;
00237     X21 = a21 - a22;
00238     X22 = X21.transpose();
00239     t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0));
00240     h2 = A2 - 2 * (X21 * X22) / (t * t);
00241     H2 << 1.0, 0.0, 0.0,
00242         0.0, h2(0, 0), h2(0, 1),
00243         0.0, h2(1, 0), h2(1, 1);
00244     R = H2 * ma1;
00245     MatrixXd H1T(3, 3), H2T(3, 3);
00246     H1T = H1.transpose();
00247     H2T = H2.transpose();
00248     Q = H1T * H2T;    
00249 }
00250 
00251 void vp(int leg) {//5年生の時に作成したもの
00252     double crosx, crosy, crosz;
00253     double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) };
00254     double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) };
00255     double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] };
00256     if (Lin[2] != 0.0)
00257     {
00258         double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 };
00259         for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; }
00260         AP[2] = 0.0;
00261     }
00262     crosx = AP[1] * wA[2] + (-AP[2]) * wA[1];
00263     crosy = AP[2] * wA[0] + (-AP[0]) * wA[2];
00264     crosz = AP[0] * wA[1] + (-AP[1]) * wA[0];
00265     vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2];
00266     //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]);
00267     //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0));
00268     //printf("vp finish\n");
00269 }
00270 void fwd(int leg) {
00271     //printf("fwd start\n");
00272     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]),
00273         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]),
00274         s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
00275     tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0];  //x
00276     tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1];  //y
00277     tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z
00278     //pc2.printf("leg=%d,(  %3.3lf,  %3.3lf,  %3.3lf))  ",leg,tip[leg][0],tip[leg][1],tip[leg][2]);
00279 }
00280 void Jac(int leg) {
00281     //printf("Jac start\n");
00282     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]),
00283         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]),
00284         s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
00285     Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
00286     Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0;
00287     Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0;
00288     Jacbi[leg][0][3] = (-L[2] * s123) * c0;
00289 
00290     Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
00291     Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0;
00292     Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0;
00293     Jacbi[leg][1][3] = (-L[2] * s123) * s0;
00294 
00295     Jacbi[leg][2][0] = 0.0;
00296     Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
00297     Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123;
00298     Jacbi[leg][2][3] = L[2] * c123;
00299 
00300 
00301     //printf("Jac finish\n");
00302 }//ok
00303 void deff(int leg) {
00304     //printf(" 評価関数定義\n");
00305     fi = r  + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順
00306     a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2]));
00307     a = a0 * (double)cos(fi);
00308     h = a * (1 / (double)cos(fi) - tan(fi));
00309     X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身
00310     //tan-1の分母分子
00311     tan_u = tip[leg][2];
00312     tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]));
00313     //printf("評価関数計算完了\n");
00314 }
00315 void dfd(int leg) {
00316     //printf("評価関数微分\n");
00317     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]);
00318     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]);
00319     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]);
00320     double cfi=cos(fi),sfi=sin(fi);
00321     double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
00322     
00323     double df_da=1/cfi-tan(fi);
00324     double df_dfi=a*(-sfi-1)/(cfi*cfi);
00325     double da_dx=x*cfi/sqrt(x*x+y*y);
00326     double da_dy=y*cfi/sqrt(x*x+y*y);
00327     double da_dfi=-sqrt(x*x+y*y)*sfi;
00328     double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
00329     double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
00330     double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
00331    
00332     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]))
00333              +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); 
00334              
00335     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]))
00336              +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
00337               
00338     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]))
00339              +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); 
00340     
00341     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]))
00342              +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);                              
00343    
00344    //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);  
00345 }
00346 //使わない
00347 double fe(int leg,double dth3) {
00348     //brent法のための関数, 事前にdfdを実行してから使う
00349     double dfd_nolm,th0_nolm,e=0.0;
00350     //∇hを正規化する
00351     dfd(leg);
00352     dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
00353     for (int i = 0; i < 4; i++) {
00354         dfdth[i]=dfdth[i]/dfd_nolm;
00355     }
00356     
00357     solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
00358     
00359     //dthベクトルを正規化
00360     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]);
00361     for (int i = 0; i < 4; i++) {
00362         th0[leg][i] = th0[leg][i] / th0_nolm;
00363     }
00364     for (int i = 0; i < 4; i++) {
00365          e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
00366     }    
00367     return e;//eベクトルのノルムの2乗を返す
00368 
00369     }
00370 double f(int leg,double dth3) {
00371     double f_return=0.0;
00372     solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
00373     f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3];   
00374     
00375     return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる
00376 
00377     } 
00378 void mnbrak(int leg,int discrimination)
00379 {
00380     double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc;
00381     //fa=f(ax);
00382     //fb=f(bx);
00383     if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);}
00384     if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);}
00385     if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);}
00386     if(discrimination==3){fa=f_test(ax);fb=f_test(bx);}
00387     
00388     if(fb>fa)
00389     {
00390         SHIFT(dum,ax,bx,dum);
00391         SHIFT(dum,fb,fa,dum);
00392         }
00393     cx=bx+GOLD*(bx-ax);
00394     //fc=f(cx);
00395     if(discrimination==0){fc=fe(leg,cx);}
00396     if(discrimination==1){fc=num_nolm(leg,cx);}
00397     if(discrimination==2){fc=f(leg,cx);}
00398     if(discrimination==3){fc=f_test(cx);}
00399     while (fb>fc)
00400     {
00401         r=(bx-ax)*(fb-fc);
00402         q=(bx-cx)*(fb-fa);
00403         u=bx-((bx-cx)*q-(bx-cx)*r)/
00404         (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));
00405         ulim=bx+GLIMIT*(cx-bx);
00406         
00407         if((bx-u)*(u-cx)>0.0)
00408         {
00409             //fu=f(u);
00410             if(discrimination==0){fu=fe(leg,u);}
00411             if(discrimination==1){fu=num_nolm(leg,u);}
00412             if(discrimination==2){fu=f(leg,u);}
00413             if(discrimination==3){fu=f_test(u);}
00414             if(fu<fc)
00415             {
00416                 ax=bx;
00417                 bx=u;
00418                 fa=fb;
00419                 fb=fu;
00420                 return;
00421                 }
00422             else if(fu>fb)
00423             {
00424                 cx=u;
00425                 fc=fu;
00426                 return;
00427                 }
00428              u=cx*+GOLD*(cx-bx);
00429              //fu=f(u);
00430              if(discrimination==0){fu=fe(leg,u);}
00431              if(discrimination==1){fu=num_nolm(leg,u);}
00432              if(discrimination==2){fu=f(leg,u);}
00433              if(discrimination==3){fu=f_test(u);}       
00434             }
00435         else if((cx-u)*(u-ulim)>0.0)
00436         {
00437             //fu=f(u);
00438             if(discrimination==0){fu=fe(leg,u);}
00439             if(discrimination==1){fu=num_nolm(leg,u);}
00440             if(discrimination==2){fu=f(leg,u);}
00441             if(discrimination==3){fu=f_test(u);}
00442             if(fu<fc)
00443             {
00444                 SHIFT(bx,cx,u,cx+GOLD*(cx-bx));
00445                 if(discrimination==0){fu_2=fe(leg,u);}
00446                 if(discrimination==1){fu_2=num_nolm(leg,u);}
00447                 if(discrimination==2){fu_2=f(leg,u);}
00448                 if(discrimination==3){fu_2=f_test(u);}
00449                 //SHIFT(fb,fc,fu,f(u));
00450                 SHIFT(fb,fc,fu,fu_2);
00451                 }
00452             
00453             }
00454         else if((u-ulim)*(ulim-cx)>=0.0)
00455         {
00456             u=ulim;
00457             //fu=f(u);
00458             if(discrimination==0){fu=fe(leg,u);}
00459             if(discrimination==1){fu=num_nolm(leg,u);}
00460             if(discrimination==2){fu=f(leg,u);}
00461             if(discrimination==3){fu=f_test(u);}
00462             }
00463         else
00464         {
00465             u=cx+GOLD*(cx-bx);
00466             //fu=f(u);
00467             if(discrimination==0){fu=fe(leg,u);}
00468             if(discrimination==1){fu=num_nolm(leg,u);}
00469             if(discrimination==2){fu=f(leg,u);}
00470             if(discrimination==3){fu=f_test(u);}
00471             }
00472         SHIFT(ax,bx,cx,u);
00473         SHIFT(fa,fb,fc,fu);
00474         }
00475     }
00476 double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ
00477 {
00478 
00479     int iter;
00480     double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
00481     double e=0.0;
00482     a=(ax <cx ? ax : cx);
00483     b=(ax >cx ? ax : cx);
00484     x=w=v=bx;
00485     if(discrimination==0){fw=fv=fx=fe(leg,x);}
00486     if(discrimination==1){fw=fv=fx=num_nolm(leg,x);}
00487     if(discrimination==2){fw=fv=fx=f(leg,x);}
00488     if(discrimination==3){fw=fv=fx=f_test(x);}
00489     //fw=fv=fx=f(x);//関数部分
00490     for(iter=1;iter<=ITMAX;iter++)
00491     {
00492         xm=0.5*(a+b);
00493         tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
00494         //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u);
00495         if(fabs(x-xm)<=(tol2-0.5*(b-a)))
00496         {
00497             //pc.printf("bernt out");
00498             xmin=x;
00499              //pc2.printf("xmin=%lf\r\n",x);
00500              //pc2.printf("fx=%lf\r\n",fx);
00501             return xmin;
00502         }
00503         if(fabs(e)>tol1)
00504         {
00505             r=(x-w)*(fx-fv);
00506             q=(x-v)*(fx-fw);
00507             p=(x-v)*q-(x-w)*r;
00508             q=2.0*(q-r);
00509             if(q>0.0)p=-p;
00510             q=fabs(q);
00511             etemp=e;
00512             e=d;
00513             if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x))
00514             {  d=CGOLD*(e= (x>=xm ? a-x : b-x));}
00515             else
00516             {
00517                d=p/q;
00518                u=x+d;
00519                if(u-a < tol2 || b-u < tol2)
00520                {d=SIGN(tol1,xm-x);}
00521             }
00522         }
00523         else
00524         {
00525             d=CGOLD*(e= (x>=xm ? a-x : b-x));
00526             }    
00527         u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
00528         if(discrimination==0){fu=fe(leg,u);}
00529         if(discrimination==1){fu=num_nolm(leg,u);}
00530         if(discrimination==2){fu=f(leg,u);}
00531         if(discrimination==3){fu=f_test(u);}
00532         //fu=f(u);//関数部分
00533          if(fu <= fx)
00534          {
00535             if(u >= x)a=x; else b=x;
00536             SHIFT(v,w,x,u);
00537             SHIFT(fv,fw,fx,fu);
00538           }
00539          else{
00540           if(u < x){a=u;} 
00541           else {b=u;}
00542           if(fu <= fw || w==x)
00543           {
00544               v=w;
00545               w=u;
00546               fv=fw;
00547               fw=fu;
00548               }    
00549           else if (fu <= fv || v==x || v==w)
00550           {
00551             v=u;
00552             fv=fu;
00553           }
00554         }
00555         
00556         }
00557         //pc2.printf("xmin=%lf\r\n",x);
00558         //pc2.printf("fx=%lf\r\n",fx);
00559     return xmin;
00560     }           
00561 
00562 double SIGN(double x,double y)
00563 {
00564     double x_return;
00565     x_return=abs(x);
00566     if(y<0.0)x_return=-x_return;
00567     
00568     return x_return; 
00569     }
00570 double FMAX(double x, double y)
00571 {
00572     if(x>y){return x;}
00573     if(y>x){return y;}
00574     return 0;
00575     }      
00576 double num_nolm(int leg,double dth3)
00577 {
00578     
00579     double nolm_return=0.0;
00580     solve(dth3,leg,2);
00581     nolm_return=th0[leg][0]*th0[leg][0]+th0[leg][1]*th0[leg][1]+th0[leg][2]*th0[leg][2]+th0[leg][3]*th0[leg][3];
00582     
00583     return nolm_return;    
00584     }
00585 //brent法のテスト用の関数    
00586 //極小値を求めたい関数を定義    
00587 double f_test(double x){
00588     double x_return;
00589     x_return=x*x-2*x+1;
00590     return x_return;
00591     }              
00592 void solve(double w3, int leg,int det) {
00593    
00594     double dth[4];
00595     MatrixXd v_Q(3,1),QT(3,3);
00596 
00597     QT = Q.transpose();
00598     v_Q = QT * vP[leg]*sampling;
00599    
00600     dth[3] = w3 ;
00601     dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
00602     dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
00603     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));
00604     if (det == 1) {
00605         for (int i=0; i < 4; i++) {
00606             th[leg][i] = th[leg][i] + dth[i];
00607         }
00608     }
00609     else if (det == 2) {
00610         for (int u=0; u < 4; u++) {
00611             th0[leg][u] = dth[u];
00612         }
00613     }
00614             
00615 }
00616 
00617 //サーボ関係
00618 void setup_servo() { 
00619  pwm.begin();               
00620  pwm.setPWMFreq(200);                
00621 }
00622 void servo_write(int ch,double ang)//引数は[°]
00623 {
00624     if(ch==0)  ang=ang-135*PI/180;
00625     if(ch==4)  ang=ang-45*PI/180;
00626     if(ch==8)  ang=ang+45*PI/180;
00627     if(ch==12) ang=ang+135*PI/180;
00628     if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang;
00629     ang=servo0[ch]+ang*8000/270*180/PI;
00630     servo_write7(ch,ang);
00631     }
00632 
00633 void servo_write7(int ch, double ang){ 
00634   ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
00635   //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ;                                               //初期状態を設定するときこの値を参考に設定したためそのまま利用　　 
00636   pwm.setPWM(ch, 0, ang);
00637 }
00638 void servo_calib()
00639 {
00640     for(int u=0; u<4; u++)
00641     {
00642      for(int i=0; i<4; i++)
00643      {
00644         servo_write7(ch[u][i],servo0[ch[u][i]]);
00645         }
00646      }
00647 }
00648 void servo_ready()
00649 {
00650     for(int u=0; u<4; u++)
00651             {
00652               for(int i=0; i<4; i++)
00653               { 
00654                servo_write(ch[u][i],th_ready[u][i]);
00655                }
00656              }
00657     }