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