Kotaro Irisawa / Mbed 2 deprecated 4dof_screw

2021.12.22.16:06

Dependencies:   mbed pca9685_2021_12_22 Eigen

main.cpp@5:f225e0c61cfc, 2022-02-24 (annotated)

Committer:
Kotttaro
Date:
Thu Feb 24 06:12:53 2022 +0000
Revision:
5:f225e0c61cfc
Parent:
4:8a50c7822dac 
2022.02.24

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kotttaro 4:8a50c7822dac 1 //特別研究Ⅰで用いたプログラムを改良したもの
Kotttaro 0:4a5272e014d8 2 //ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する
Kotttaro 2:57237f0a4a34 3 //brent法の部分は『numerical recipes in c』 参照
Kotttaro 4:8a50c7822dac 4 #include "mbed.h"
Kotttaro 4:8a50c7822dac 5 #include <PCA9685.h>
Kotttaro 0:4a5272e014d8 6 #include "Eigen/Geometry.h"
Kotttaro 0:4a5272e014d8 7 #include "Eigen/Dense.h"
Kotttaro 0:4a5272e014d8 8 #include <math.h>
Kotttaro 4:8a50c7822dac 9
Kotttaro 2:57237f0a4a34 10 #define ITMAX 100
Kotttaro 2:57237f0a4a34 11 #define CGOLD 0.3819660
Kotttaro 2:57237f0a4a34 12 #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
Kotttaro 2:57237f0a4a34 13 #define ZEPS 1.0e-10
Kotttaro 4:8a50c7822dac 14 #define GOLD 1.618034
Kotttaro 4:8a50c7822dac 15 #define TINY 1.0e-20
Kotttaro 4:8a50c7822dac 16 #define GLIMIT 100.0
Kotttaro 4:8a50c7822dac 17 #define SERVOMIN 700
Kotttaro 4:8a50c7822dac 18 #define SERVOMAX 2300
Kotttaro 4:8a50c7822dac 19 #define SERVOGAIN 29.6296300
Kotttaro 4:8a50c7822dac 20 #define PI 3.14159265358979323846264338327950288
Kotttaro 2:57237f0a4a34 21
Kotttaro 4:8a50c7822dac 22 PCA9685 pwm;//クラス宣言
Kotttaro 0:4a5272e014d8 23 Serial pc2(USBTX,USBRX);
Kotttaro 0:4a5272e014d8 24 Timer tim;
Kotttaro 4:8a50c7822dac 25 Timer manage;
Kotttaro 5:f225e0c61cfc 26 int times= 200;//実行回数:実行時間は (sampling)*(times)秒
Kotttaro 0:4a5272e014d8 27
Kotttaro 0:4a5272e014d8 28 using namespace Eigen;
Kotttaro 0:4a5272e014d8 29
Kotttaro 0:4a5272e014d8 30 //以下変数定義
Kotttaro 4:8a50c7822dac 31
Kotttaro 4:8a50c7822dac 32 //brent法に必要な変数
Kotttaro 5:f225e0c61cfc 33 double ax=-0.1*PI/180.0,bx=0.1*PI/180.0,cx=0.0;
Kotttaro 4:8a50c7822dac 34 double fa,fb,fc;
Kotttaro 4:8a50c7822dac 35 //サーボの書き込みに必要な変数
Kotttaro 5:f225e0c61cfc 36 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の初期値
Kotttaro 4:8a50c7822dac 37 int ch[4][4]={{0 ,1 ,2 ,3} ,
Kotttaro 4:8a50c7822dac 38 {4 ,5 ,6 ,7} ,
Kotttaro 4:8a50c7822dac 39 {8 ,9 ,10,11},
Kotttaro 4:8a50c7822dac 40 {12,13,14,15} };
Kotttaro 5:f225e0c61cfc 41 double r=0.0*PI/180;//斜面の傾き[°]
Kotttaro 5:f225e0c61cfc 42 double sampling=0.01;//δtの時間[s]
Kotttaro 5:f225e0c61cfc 43 double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
Kotttaro 0:4a5272e014d8 44 double tip[4][3];//足先座標
Kotttaro 5:f225e0c61cfc 45 double con[4][3] = { {-55.0, 55.0,0.0},
Kotttaro 5:f225e0c61cfc 46 { 55.0, 55.0,0.0},
Kotttaro 5:f225e0c61cfc 47 { 55.0,-55.0,0.0},
Kotttaro 5:f225e0c61cfc 48 {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0
Kotttaro 5:f225e0c61cfc 49 double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 5:f225e0c61cfc 50 {45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 5:f225e0c61cfc 51 {-45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 5:f225e0c61cfc 52 {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態
Kotttaro 0:4a5272e014d8 53 double th0[4][4]= { 0.0,0.0,0.0,0.0,
Kotttaro 0:4a5272e014d8 54 0.0,0.0, 0.0,0.0,
Kotttaro 0:4a5272e014d8 55 0.0,0.0, 0.0,0.0,
Kotttaro 0:4a5272e014d8 56 0.0,0.0, 0.0,0.0 }; //計算用の関節角度
Kotttaro 5:f225e0c61cfc 57 double th[4][4];//サーボに入力する角度
Kotttaro 0:4a5272e014d8 58 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元
Kotttaro 0:4a5272e014d8 59 double a,a0, h,fi;//評価関数内の変数 fi=φ
Kotttaro 0:4a5272e014d8 60 double X,tan_u, tan_d;//計算用
Kotttaro 0:4a5272e014d8 61
Kotttaro 0:4a5272e014d8 62 //ねじ軸
Kotttaro 0:4a5272e014d8 63 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ
Kotttaro 5:f225e0c61cfc 64 double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件
Kotttaro 0:4a5272e014d8 65 double dfdth[4];//評価関数のナブラ
Kotttaro 0:4a5272e014d8 66
Kotttaro 0:4a5272e014d8 67
Kotttaro 0:4a5272e014d8 68 //以下行列定義
Kotttaro 0:4a5272e014d8 69 MatrixXd Q(3, 3);//Q行列
Kotttaro 0:4a5272e014d8 70 MatrixXd R(3, 4);//R行列
Kotttaro 0:4a5272e014d8 71 Vector3d vP[4];//各脚の速度ベクトル
Kotttaro 0:4a5272e014d8 72
Kotttaro 0:4a5272e014d8 73
Kotttaro 0:4a5272e014d8 74 void QR(int leg);//QR分解用関数,引数は脚番号
Kotttaro 0:4a5272e014d8 75 void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する
Kotttaro 0:4a5272e014d8 76 void fwd(int leg);//順運動学より脚先の座標を導出する
Kotttaro 0:4a5272e014d8 77 void Jac(int leg);//指定した脚のヤコビアンを計算
Kotttaro 0:4a5272e014d8 78 void deff(int leg);//評価関数計算, legは距離と傾きから指定する
Kotttaro 0:4a5272e014d8 79 void dfd( int leg);//評価関数の勾配をとる
Kotttaro 0:4a5272e014d8 80 double search(int leg);//最大のthetaを探索するための関数
Kotttaro 0:4a5272e014d8 81 void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す
Kotttaro 2:57237f0a4a34 82 double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの
Kotttaro 2:57237f0a4a34 83 double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数
Kotttaro 3:f824e4d8eef7 84 double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用
Kotttaro 4:8a50c7822dac 85 void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数
Kotttaro 3:f824e4d8eef7 86 double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム
Kotttaro 4:8a50c7822dac 87 //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test)
Kotttaro 2:57237f0a4a34 88 double SIGN(double x,double y);//xにyの符号をつけたものを返す
Kotttaro 4:8a50c7822dac 89 double FMAX(double x,double y);//大きいほうの値が返される
Kotttaro 4:8a50c7822dac 90 double f_test(double x);//テスト用の関数
Kotttaro 4:8a50c7822dac 91
Kotttaro 4:8a50c7822dac 92 //以下サーボ関係
Kotttaro 4:8a50c7822dac 93 void setup_servo();//サーボセットアップ用関数
Kotttaro 4:8a50c7822dac 94 void servo_write(int ch,double ang);//angに
Kotttaro 4:8a50c7822dac 95 void servo_write7(int ch, double ang);
Kotttaro 4:8a50c7822dac 96 void servo_calib();//全ての角度を0度にする
Kotttaro 5:f225e0c61cfc 97 void servo_ready();//初期状態
Kotttaro 0:4a5272e014d8 98
Kotttaro 0:4a5272e014d8 99 int main()
Kotttaro 0:4a5272e014d8 100 {
Kotttaro 0:4a5272e014d8 101 double t;
Kotttaro 0:4a5272e014d8 102 pc2.baud(921600);
Kotttaro 5:f225e0c61cfc 103 //setup_servo();
Kotttaro 5:f225e0c61cfc 104 for(int u=0; u<4; u++)
Kotttaro 5:f225e0c61cfc 105 {
Kotttaro 5:f225e0c61cfc 106 for(int i=0; i<4; i++)
Kotttaro 5:f225e0c61cfc 107 {
Kotttaro 5:f225e0c61cfc 108 th[u][i]=th_ready[u][i];
Kotttaro 5:f225e0c61cfc 109 }
Kotttaro 5:f225e0c61cfc 110 }
Kotttaro 5:f225e0c61cfc 111 for(int i=0; i<4; i++)
Kotttaro 5:f225e0c61cfc 112 {
Kotttaro 5:f225e0c61cfc 113 fwd(i);
Kotttaro 5:f225e0c61cfc 114 }
Kotttaro 5:f225e0c61cfc 115 //servo_ready();
Kotttaro 4:8a50c7822dac 116 wait(2);
Kotttaro 5:f225e0c61cfc 117 //while(1);
Kotttaro 5:f225e0c61cfc 118 /*for(int u=0; u<4; u++)
Kotttaro 4:8a50c7822dac 119 {
Kotttaro 4:8a50c7822dac 120 for(int i=0; i<4; i++)
Kotttaro 4:8a50c7822dac 121 {
Kotttaro 4:8a50c7822dac 122 servo_write(ch[u][i],th[u][i]);
Kotttaro 4:8a50c7822dac 123 }
Kotttaro 5:f225e0c61cfc 124 }*/
Kotttaro 5:f225e0c61cfc 125 //while(1);
Kotttaro 4:8a50c7822dac 126 wait(2);
Kotttaro 0:4a5272e014d8 127 int count = 0;
Kotttaro 0:4a5272e014d8 128 //入力したねじ運動を換算する
Kotttaro 5:f225e0c61cfc 129 Lin[0] = cos(30.0*PI/180); //ねじ軸x
Kotttaro 5:f225e0c61cfc 130 Lin[1] = sin(30.0*PI/180); //ねじ軸y
Kotttaro 5:f225e0c61cfc 131 Lin[2] = 0.0;//ねじ軸z
Kotttaro 0:4a5272e014d8 132 L0[0] = 0.0;//ねじ軸原点座標
Kotttaro 0:4a5272e014d8 133 L0[1] = 0.0;
Kotttaro 0:4a5272e014d8 134 L0[2] = 0.0;
Kotttaro 4:8a50c7822dac 135 vin = 0.0;
Kotttaro 5:f225e0c61cfc 136 win = 2.0;
Kotttaro 2:57237f0a4a34 137 printf("\r\n\r\n");
Kotttaro 0:4a5272e014d8 138 nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
Kotttaro 0:4a5272e014d8 139 for (int i = 0; i < 3; i++)
Kotttaro 0:4a5272e014d8 140 {
Kotttaro 0:4a5272e014d8 141 wg[i] = Lin[i] * win / nol;
Kotttaro 0:4a5272e014d8 142 v[i] = Lin[i] * vin / nol;
Kotttaro 0:4a5272e014d8 143 }
Kotttaro 0:4a5272e014d8 144 //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0));
Kotttaro 0:4a5272e014d8 145
Kotttaro 0:4a5272e014d8 146 //times*δtの時間だけサーボを動かす
Kotttaro 0:4a5272e014d8 147 tim.start();
Kotttaro 5:f225e0c61cfc 148
Kotttaro 4:8a50c7822dac 149 manage.start();
Kotttaro 5:f225e0c61cfc 150 //for (int i = 0; i < times;i++){
Kotttaro 5:f225e0c61cfc 151 while(tim.read()<=5.0) {
Kotttaro 5:f225e0c61cfc 152 int l=2;
Kotttaro 5:f225e0c61cfc 153 //count = count + 1;
Kotttaro 0:4a5272e014d8 154 double dth;
Kotttaro 5:f225e0c61cfc 155 //for(int l=0;l<4;l++)
Kotttaro 5:f225e0c61cfc 156 //{
Kotttaro 4:8a50c7822dac 157 //////////計算部/////////////////
Kotttaro 5:f225e0c61cfc 158 t=tim.read();
Kotttaro 5:f225e0c61cfc 159 fwd(l);
Kotttaro 5:f225e0c61cfc 160 pc2.printf("%3.3lf %3.4lf %3.4lf %3.4lf\r\n",t,tip[l][0],tip[l][1],tip[l][2]);
Kotttaro 5:f225e0c61cfc 161 vp(l);
Kotttaro 5:f225e0c61cfc 162 Jac(l);
Kotttaro 5:f225e0c61cfc 163 QR(l);
Kotttaro 5:f225e0c61cfc 164 deff(l);
Kotttaro 5:f225e0c61cfc 165 dfd(l);
Kotttaro 5:f225e0c61cfc 166 ax=-0.15*PI/180.0;bx=0.15*PI/180.0;cx=0.0;
Kotttaro 5:f225e0c61cfc 167 //2:谷,1:山
Kotttaro 5:f225e0c61cfc 168 mnbrak(l,2);
Kotttaro 5:f225e0c61cfc 169 dth=brent(l,ax,bx,cx,0.01,2);
Kotttaro 5:f225e0c61cfc 170 solve(dth, l, 1);
Kotttaro 4:8a50c7822dac 171 ////////////////////////////////
Kotttaro 5:f225e0c61cfc 172 ///*
Kotttaro 5:f225e0c61cfc 173 //}
Kotttaro 5:f225e0c61cfc 174 /*for(int u=0; u<4; u++)
Kotttaro 4:8a50c7822dac 175 {
Kotttaro 4:8a50c7822dac 176 for(int i=0; i<4; i++)
Kotttaro 4:8a50c7822dac 177 {
Kotttaro 4:8a50c7822dac 178 servo_write(ch[u][i],th[u][i]);
Kotttaro 4:8a50c7822dac 179 }
Kotttaro 5:f225e0c61cfc 180 }//*/
Kotttaro 5:f225e0c61cfc 181 //t=tim.read();
Kotttaro 5:f225e0c61cfc 182 //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 );
Kotttaro 5:f225e0c61cfc 183 //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 );
Kotttaro 5:f225e0c61cfc 184 //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 );
Kotttaro 5:f225e0c61cfc 185 //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 );
Kotttaro 5:f225e0c61cfc 186 //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",
Kotttaro 5:f225e0c61cfc 187 //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]);
Kotttaro 5:f225e0c61cfc 188 //pc2.printf("\r\n");
Kotttaro 4:8a50c7822dac 189 while(1)
Kotttaro 4:8a50c7822dac 190 {
Kotttaro 4:8a50c7822dac 191 if(manage.read()>sampling)break;
Kotttaro 4:8a50c7822dac 192 }
Kotttaro 5:f225e0c61cfc 193 manage.reset();
Kotttaro 4:8a50c7822dac 194
Kotttaro 0:4a5272e014d8 195 }
Kotttaro 1:5c2562adca7d 196 t=tim.read();
Kotttaro 4:8a50c7822dac 197 wait(3);
Kotttaro 5:f225e0c61cfc 198 servo_ready();
Kotttaro 0:4a5272e014d8 199 return 0; // ソフトの終了
Kotttaro 0:4a5272e014d8 200 }
Kotttaro 0:4a5272e014d8 201
Kotttaro 0:4a5272e014d8 202 void QR(int leg) {
Kotttaro 0:4a5272e014d8 203 double s, t;//要素計算用
Kotttaro 0:4a5272e014d8 204 MatrixXd ma(3, 4), ma1(3, 4);
Kotttaro 0:4a5272e014d8 205
Kotttaro 0:4a5272e014d8 206 ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
Kotttaro 0:4a5272e014d8 207 Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
Kotttaro 0:4a5272e014d8 208 Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
Kotttaro 4:8a50c7822dac 209
Kotttaro 0:4a5272e014d8 210 //ハウスホルダー変換1回目
Kotttaro 0:4a5272e014d8 211 MatrixXd A1(3, 3);
Kotttaro 0:4a5272e014d8 212 A1 << 1.0, 0.0, 0.0,
Kotttaro 0:4a5272e014d8 213 0.0, 1.0, 0.0,
Kotttaro 0:4a5272e014d8 214 0.0, 0.0, 1.0;
Kotttaro 4:8a50c7822dac 215
Kotttaro 0:4a5272e014d8 216 s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ
Kotttaro 4:8a50c7822dac 217
Kotttaro 0:4a5272e014d8 218 MatrixXd H1(3, 3);//1回目の行列
Kotttaro 0:4a5272e014d8 219 MatrixXd X11(3, 1), X12(1, 3);
Kotttaro 0:4a5272e014d8 220 Vector3d a11, a12;//a11が変換前,a12が変換後
Kotttaro 0:4a5272e014d8 221 a11 << ma(0, 0), ma(1, 0), ma(2, 0);
Kotttaro 0:4a5272e014d8 222 a12 << s, 0.0, 0.0;
Kotttaro 0:4a5272e014d8 223 X11 = a11 - a12;
Kotttaro 0:4a5272e014d8 224 X12 = X11.transpose();
Kotttaro 0:4a5272e014d8 225 t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
Kotttaro 0:4a5272e014d8 226 H1 = A1 - 2.0 * (X11 * X12) / (t * t);
Kotttaro 0:4a5272e014d8 227 ma1 = H1 * ma;
Kotttaro 0:4a5272e014d8 228 //2回目
Kotttaro 0:4a5272e014d8 229 MatrixXd H2(3, 3), A2(2, 2), h2(2, 2);
Kotttaro 0:4a5272e014d8 230 A2 << 1.0, 0.0,
Kotttaro 0:4a5272e014d8 231 0.0, 1.0;
Kotttaro 0:4a5272e014d8 232 Vector2d a21, a22;
Kotttaro 0:4a5272e014d8 233 MatrixXd X21(2, 1), X22(1, 2);
Kotttaro 0:4a5272e014d8 234 a21 << ma1(1, 1), ma1(2, 1);
Kotttaro 0:4a5272e014d8 235 s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
Kotttaro 0:4a5272e014d8 236 a22 << s, 0;
Kotttaro 0:4a5272e014d8 237 X21 = a21 - a22;
Kotttaro 0:4a5272e014d8 238 X22 = X21.transpose();
Kotttaro 0:4a5272e014d8 239 t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0));
Kotttaro 0:4a5272e014d8 240 h2 = A2 - 2 * (X21 * X22) / (t * t);
Kotttaro 0:4a5272e014d8 241 H2 << 1.0, 0.0, 0.0,
Kotttaro 0:4a5272e014d8 242 0.0, h2(0, 0), h2(0, 1),
Kotttaro 0:4a5272e014d8 243 0.0, h2(1, 0), h2(1, 1);
Kotttaro 0:4a5272e014d8 244 R = H2 * ma1;
Kotttaro 0:4a5272e014d8 245 MatrixXd H1T(3, 3), H2T(3, 3);
Kotttaro 0:4a5272e014d8 246 H1T = H1.transpose();
Kotttaro 0:4a5272e014d8 247 H2T = H2.transpose();
Kotttaro 4:8a50c7822dac 248 Q = H1T * H2T;
Kotttaro 0:4a5272e014d8 249 }
Kotttaro 0:4a5272e014d8 250
Kotttaro 0:4a5272e014d8 251 void vp(int leg) {//5年生の時に作成したもの
Kotttaro 0:4a5272e014d8 252 double crosx, crosy, crosz;
Kotttaro 5:f225e0c61cfc 253 double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) };
Kotttaro 0:4a5272e014d8 254 double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) };
Kotttaro 0:4a5272e014d8 255 double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] };
Kotttaro 0:4a5272e014d8 256 if (Lin[2] != 0.0)
Kotttaro 0:4a5272e014d8 257 {
Kotttaro 0:4a5272e014d8 258 double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 };
Kotttaro 0:4a5272e014d8 259 for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; }
Kotttaro 0:4a5272e014d8 260 AP[2] = 0.0;
Kotttaro 0:4a5272e014d8 261 }
Kotttaro 0:4a5272e014d8 262 crosx = AP[1] * wA[2] + (-AP[2]) * wA[1];
Kotttaro 0:4a5272e014d8 263 crosy = AP[2] * wA[0] + (-AP[0]) * wA[2];
Kotttaro 0:4a5272e014d8 264 crosz = AP[0] * wA[1] + (-AP[1]) * wA[0];
Kotttaro 0:4a5272e014d8 265 vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2];
Kotttaro 0:4a5272e014d8 266 //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]);
Kotttaro 0:4a5272e014d8 267 //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0));
Kotttaro 0:4a5272e014d8 268 //printf("vp finish\n");
Kotttaro 0:4a5272e014d8 269 }
Kotttaro 0:4a5272e014d8 270 void fwd(int leg) {
Kotttaro 0:4a5272e014d8 271 //printf("fwd start\n");
Kotttaro 0:4a5272e014d8 272 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]),
Kotttaro 0:4a5272e014d8 273 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]),
Kotttaro 0:4a5272e014d8 274 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:4a5272e014d8 275 tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x
Kotttaro 0:4a5272e014d8 276 tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y
Kotttaro 0:4a5272e014d8 277 tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z
Kotttaro 5:f225e0c61cfc 278 //pc2.printf("leg=%d,( %3.3lf, %3.3lf, %3.3lf)) ",leg,tip[leg][0],tip[leg][1],tip[leg][2]);
Kotttaro 0:4a5272e014d8 279 }
Kotttaro 0:4a5272e014d8 280 void Jac(int leg) {
Kotttaro 0:4a5272e014d8 281 //printf("Jac start\n");
Kotttaro 0:4a5272e014d8 282 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]),
Kotttaro 0:4a5272e014d8 283 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]),
Kotttaro 0:4a5272e014d8 284 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:4a5272e014d8 285 Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:4a5272e014d8 286 Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 287 Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 288 Jacbi[leg][0][3] = (-L[2] * s123) * c0;
Kotttaro 0:4a5272e014d8 289
Kotttaro 0:4a5272e014d8 290 Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:4a5272e014d8 291 Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 292 Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 293 Jacbi[leg][1][3] = (-L[2] * s123) * s0;
Kotttaro 0:4a5272e014d8 294
Kotttaro 0:4a5272e014d8 295 Jacbi[leg][2][0] = 0.0;
Kotttaro 0:4a5272e014d8 296 Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 297 Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123;
Kotttaro 0:4a5272e014d8 298 Jacbi[leg][2][3] = L[2] * c123;
Kotttaro 0:4a5272e014d8 299
Kotttaro 0:4a5272e014d8 300
Kotttaro 0:4a5272e014d8 301 //printf("Jac finish\n");
Kotttaro 0:4a5272e014d8 302 }//ok
Kotttaro 0:4a5272e014d8 303 void deff(int leg) {
Kotttaro 0:4a5272e014d8 304 //printf(" 評価関数定義\n");
Kotttaro 0:4a5272e014d8 305 fi = r + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順
Kotttaro 0:4a5272e014d8 306 a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2]));
Kotttaro 0:4a5272e014d8 307 a = a0 * (double)cos(fi);
Kotttaro 0:4a5272e014d8 308 h = a * (1 / (double)cos(fi) - tan(fi));
Kotttaro 0:4a5272e014d8 309 X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身
Kotttaro 0:4a5272e014d8 310 //tan-1の分母分子
Kotttaro 0:4a5272e014d8 311 tan_u = tip[leg][2];
Kotttaro 0:4a5272e014d8 312 tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]));
Kotttaro 0:4a5272e014d8 313 //printf("評価関数計算完了\n");
Kotttaro 0:4a5272e014d8 314 }
Kotttaro 0:4a5272e014d8 315 void dfd(int leg) {
Kotttaro 0:4a5272e014d8 316 //printf("評価関数微分\n");
Kotttaro 0:4a5272e014d8 317 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]);
Kotttaro 0:4a5272e014d8 318 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]);
Kotttaro 0:4a5272e014d8 319 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]);
Kotttaro 1:5c2562adca7d 320 double cfi=cos(fi),sfi=sin(fi);
Kotttaro 1:5c2562adca7d 321 double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
Kotttaro 1:5c2562adca7d 322
Kotttaro 1:5c2562adca7d 323 double df_da=1/cfi-tan(fi);
Kotttaro 1:5c2562adca7d 324 double df_dfi=a*(-sfi-1)/(cfi*cfi);
Kotttaro 1:5c2562adca7d 325 double da_dx=x*cfi/sqrt(x*x+y*y);
Kotttaro 1:5c2562adca7d 326 double da_dy=y*cfi/sqrt(x*x+y*y);
Kotttaro 1:5c2562adca7d 327 double da_dfi=-sqrt(x*x+y*y)*sfi;
Kotttaro 1:5c2562adca7d 328 double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 1:5c2562adca7d 329 double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 1:5c2562adca7d 330 double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
Kotttaro 1:5c2562adca7d 331
Kotttaro 1:5c2562adca7d 332 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]))
Kotttaro 1:5c2562adca7d 333 +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]);
Kotttaro 1:5c2562adca7d 334
Kotttaro 1:5c2562adca7d 335 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]))
Kotttaro 1:5c2562adca7d 336 +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
Kotttaro 1:5c2562adca7d 337
Kotttaro 1:5c2562adca7d 338 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]))
Kotttaro 1:5c2562adca7d 339 +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]);
Kotttaro 1:5c2562adca7d 340
Kotttaro 1:5c2562adca7d 341 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]))
Kotttaro 1:5c2562adca7d 342 +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);
Kotttaro 1:5c2562adca7d 343
Kotttaro 3:f824e4d8eef7 344 //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);
Kotttaro 0:4a5272e014d8 345 }
Kotttaro 4:8a50c7822dac 346 //使わない
Kotttaro 2:57237f0a4a34 347 double fe(int leg,double dth3) {
Kotttaro 2:57237f0a4a34 348 //brent法のための関数, 事前にdfdを実行してから使う
Kotttaro 3:f824e4d8eef7 349 double dfd_nolm,th0_nolm,e=0.0;
Kotttaro 2:57237f0a4a34 350 //∇hを正規化する
Kotttaro 3:f824e4d8eef7 351 dfd(leg);
Kotttaro 2:57237f0a4a34 352 dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
Kotttaro 2:57237f0a4a34 353 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 354 dfdth[i]=dfdth[i]/dfd_nolm;
Kotttaro 2:57237f0a4a34 355 }
Kotttaro 2:57237f0a4a34 356
Kotttaro 2:57237f0a4a34 357 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 2:57237f0a4a34 358
Kotttaro 2:57237f0a4a34 359 //dthベクトルを正規化
Kotttaro 2:57237f0a4a34 360 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]);
Kotttaro 2:57237f0a4a34 361 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 362 th0[leg][i] = th0[leg][i] / th0_nolm;
Kotttaro 2:57237f0a4a34 363 }
Kotttaro 2:57237f0a4a34 364 for (int i = 0; i < 4; i++) {
Kotttaro 2:57237f0a4a34 365 e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
Kotttaro 4:8a50c7822dac 366 }
Kotttaro 2:57237f0a4a34 367 return e;//eベクトルのノルムの2乗を返す
Kotttaro 0:4a5272e014d8 368
Kotttaro 2:57237f0a4a34 369 }
Kotttaro 3:f824e4d8eef7 370 double f(int leg,double dth3) {
Kotttaro 3:f824e4d8eef7 371 double f_return=0.0;
Kotttaro 3:f824e4d8eef7 372 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 3:f824e4d8eef7 373 f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3];
Kotttaro 4:8a50c7822dac 374
Kotttaro 4:8a50c7822dac 375 return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる
Kotttaro 3:f824e4d8eef7 376
Kotttaro 4:8a50c7822dac 377 }
Kotttaro 4:8a50c7822dac 378 void mnbrak(int leg,int discrimination)
Kotttaro 2:57237f0a4a34 379 {
Kotttaro 4:8a50c7822dac 380 double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc;
Kotttaro 4:8a50c7822dac 381 //fa=f(ax);
Kotttaro 4:8a50c7822dac 382 //fb=f(bx);
Kotttaro 4:8a50c7822dac 383 if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);}
Kotttaro 4:8a50c7822dac 384 if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);}
Kotttaro 4:8a50c7822dac 385 if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);}
Kotttaro 4:8a50c7822dac 386 if(discrimination==3){fa=f_test(ax);fb=f_test(bx);}
Kotttaro 4:8a50c7822dac 387
Kotttaro 4:8a50c7822dac 388 if(fb>fa)
Kotttaro 4:8a50c7822dac 389 {
Kotttaro 4:8a50c7822dac 390 SHIFT(dum,ax,bx,dum);
Kotttaro 4:8a50c7822dac 391 SHIFT(dum,fb,fa,dum);
Kotttaro 4:8a50c7822dac 392 }
Kotttaro 4:8a50c7822dac 393 cx=bx+GOLD*(bx-ax);
Kotttaro 4:8a50c7822dac 394 //fc=f(cx);
Kotttaro 4:8a50c7822dac 395 if(discrimination==0){fc=fe(leg,cx);}
Kotttaro 4:8a50c7822dac 396 if(discrimination==1){fc=num_nolm(leg,cx);}
Kotttaro 4:8a50c7822dac 397 if(discrimination==2){fc=f(leg,cx);}
Kotttaro 4:8a50c7822dac 398 if(discrimination==3){fc=f_test(cx);}
Kotttaro 4:8a50c7822dac 399 while (fb>fc)
Kotttaro 4:8a50c7822dac 400 {
Kotttaro 4:8a50c7822dac 401 r=(bx-ax)*(fb-fc);
Kotttaro 4:8a50c7822dac 402 q=(bx-cx)*(fb-fa);
Kotttaro 4:8a50c7822dac 403 u=bx-((bx-cx)*q-(bx-cx)*r)/
Kotttaro 4:8a50c7822dac 404 (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));
Kotttaro 4:8a50c7822dac 405 ulim=bx+GLIMIT*(cx-bx);
Kotttaro 4:8a50c7822dac 406
Kotttaro 4:8a50c7822dac 407 if((bx-u)*(u-cx)>0.0)
Kotttaro 4:8a50c7822dac 408 {
Kotttaro 4:8a50c7822dac 409 //fu=f(u);
Kotttaro 4:8a50c7822dac 410 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 411 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 412 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 413 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 414 if(fu<fc)
Kotttaro 4:8a50c7822dac 415 {
Kotttaro 4:8a50c7822dac 416 ax=bx;
Kotttaro 4:8a50c7822dac 417 bx=u;
Kotttaro 4:8a50c7822dac 418 fa=fb;
Kotttaro 4:8a50c7822dac 419 fb=fu;
Kotttaro 4:8a50c7822dac 420 return;
Kotttaro 4:8a50c7822dac 421 }
Kotttaro 4:8a50c7822dac 422 else if(fu>fb)
Kotttaro 4:8a50c7822dac 423 {
Kotttaro 4:8a50c7822dac 424 cx=u;
Kotttaro 4:8a50c7822dac 425 fc=fu;
Kotttaro 4:8a50c7822dac 426 return;
Kotttaro 4:8a50c7822dac 427 }
Kotttaro 4:8a50c7822dac 428 u=cx*+GOLD*(cx-bx);
Kotttaro 4:8a50c7822dac 429 //fu=f(u);
Kotttaro 4:8a50c7822dac 430 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 431 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 432 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 433 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 434 }
Kotttaro 4:8a50c7822dac 435 else if((cx-u)*(u-ulim)>0.0)
Kotttaro 4:8a50c7822dac 436 {
Kotttaro 4:8a50c7822dac 437 //fu=f(u);
Kotttaro 4:8a50c7822dac 438 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 439 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 440 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 441 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 442 if(fu<fc)
Kotttaro 4:8a50c7822dac 443 {
Kotttaro 4:8a50c7822dac 444 SHIFT(bx,cx,u,cx+GOLD*(cx-bx));
Kotttaro 4:8a50c7822dac 445 if(discrimination==0){fu_2=fe(leg,u);}
Kotttaro 4:8a50c7822dac 446 if(discrimination==1){fu_2=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 447 if(discrimination==2){fu_2=f(leg,u);}
Kotttaro 4:8a50c7822dac 448 if(discrimination==3){fu_2=f_test(u);}
Kotttaro 4:8a50c7822dac 449 //SHIFT(fb,fc,fu,f(u));
Kotttaro 4:8a50c7822dac 450 SHIFT(fb,fc,fu,fu_2);
Kotttaro 4:8a50c7822dac 451 }
Kotttaro 4:8a50c7822dac 452
Kotttaro 4:8a50c7822dac 453 }
Kotttaro 4:8a50c7822dac 454 else if((u-ulim)*(ulim-cx)>=0.0)
Kotttaro 4:8a50c7822dac 455 {
Kotttaro 4:8a50c7822dac 456 u=ulim;
Kotttaro 4:8a50c7822dac 457 //fu=f(u);
Kotttaro 4:8a50c7822dac 458 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 459 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 460 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 461 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 462 }
Kotttaro 4:8a50c7822dac 463 else
Kotttaro 4:8a50c7822dac 464 {
Kotttaro 4:8a50c7822dac 465 u=cx+GOLD*(cx-bx);
Kotttaro 4:8a50c7822dac 466 //fu=f(u);
Kotttaro 4:8a50c7822dac 467 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 468 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 469 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 470 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 471 }
Kotttaro 4:8a50c7822dac 472 SHIFT(ax,bx,cx,u);
Kotttaro 4:8a50c7822dac 473 SHIFT(fa,fb,fc,fu);
Kotttaro 4:8a50c7822dac 474 }
Kotttaro 4:8a50c7822dac 475 }
Kotttaro 4:8a50c7822dac 476 double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ
Kotttaro 4:8a50c7822dac 477 {
Kotttaro 4:8a50c7822dac 478
Kotttaro 2:57237f0a4a34 479 int iter;
Kotttaro 2:57237f0a4a34 480 double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
Kotttaro 2:57237f0a4a34 481 double e=0.0;
Kotttaro 4:8a50c7822dac 482 a=(ax <cx ? ax : cx);
Kotttaro 4:8a50c7822dac 483 b=(ax >cx ? ax : cx);
Kotttaro 4:8a50c7822dac 484 x=w=v=bx;
Kotttaro 4:8a50c7822dac 485 if(discrimination==0){fw=fv=fx=fe(leg,x);}
Kotttaro 4:8a50c7822dac 486 if(discrimination==1){fw=fv=fx=num_nolm(leg,x);}
Kotttaro 4:8a50c7822dac 487 if(discrimination==2){fw=fv=fx=f(leg,x);}
Kotttaro 4:8a50c7822dac 488 if(discrimination==3){fw=fv=fx=f_test(x);}
Kotttaro 4:8a50c7822dac 489 //fw=fv=fx=f(x);//関数部分
Kotttaro 2:57237f0a4a34 490 for(iter=1;iter<=ITMAX;iter++)
Kotttaro 2:57237f0a4a34 491 {
Kotttaro 2:57237f0a4a34 492 xm=0.5*(a+b);
Kotttaro 2:57237f0a4a34 493 tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
Kotttaro 4:8a50c7822dac 494 //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u);
Kotttaro 2:57237f0a4a34 495 if(fabs(x-xm)<=(tol2-0.5*(b-a)))
Kotttaro 2:57237f0a4a34 496 {
Kotttaro 4:8a50c7822dac 497 //pc.printf("bernt out");
Kotttaro 2:57237f0a4a34 498 xmin=x;
Kotttaro 4:8a50c7822dac 499 //pc2.printf("xmin=%lf\r\n",x);
Kotttaro 4:8a50c7822dac 500 //pc2.printf("fx=%lf\r\n",fx);
Kotttaro 2:57237f0a4a34 501 return xmin;
Kotttaro 2:57237f0a4a34 502 }
Kotttaro 2:57237f0a4a34 503 if(fabs(e)>tol1)
Kotttaro 2:57237f0a4a34 504 {
Kotttaro 2:57237f0a4a34 505 r=(x-w)*(fx-fv);
Kotttaro 2:57237f0a4a34 506 q=(x-v)*(fx-fw);
Kotttaro 2:57237f0a4a34 507 p=(x-v)*q-(x-w)*r;
Kotttaro 2:57237f0a4a34 508 q=2.0*(q-r);
Kotttaro 2:57237f0a4a34 509 if(q>0.0)p=-p;
Kotttaro 2:57237f0a4a34 510 q=fabs(q);
Kotttaro 2:57237f0a4a34 511 etemp=e;
Kotttaro 2:57237f0a4a34 512 e=d;
Kotttaro 2:57237f0a4a34 513 if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x))
Kotttaro 2:57237f0a4a34 514 { d=CGOLD*(e= (x>=xm ? a-x : b-x));}
Kotttaro 2:57237f0a4a34 515 else
Kotttaro 2:57237f0a4a34 516 {
Kotttaro 2:57237f0a4a34 517 d=p/q;
Kotttaro 2:57237f0a4a34 518 u=x+d;
Kotttaro 2:57237f0a4a34 519 if(u-a < tol2 || b-u < tol2)
Kotttaro 2:57237f0a4a34 520 {d=SIGN(tol1,xm-x);}
Kotttaro 2:57237f0a4a34 521 }
Kotttaro 2:57237f0a4a34 522 }
Kotttaro 2:57237f0a4a34 523 else
Kotttaro 2:57237f0a4a34 524 {
Kotttaro 2:57237f0a4a34 525 d=CGOLD*(e= (x>=xm ? a-x : b-x));
Kotttaro 2:57237f0a4a34 526 }
Kotttaro 2:57237f0a4a34 527 u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
Kotttaro 4:8a50c7822dac 528 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 4:8a50c7822dac 529 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 4:8a50c7822dac 530 if(discrimination==2){fu=f(leg,u);}
Kotttaro 4:8a50c7822dac 531 if(discrimination==3){fu=f_test(u);}
Kotttaro 4:8a50c7822dac 532 //fu=f(u);//関数部分
Kotttaro 2:57237f0a4a34 533 if(fu <= fx)
Kotttaro 2:57237f0a4a34 534 {
Kotttaro 2:57237f0a4a34 535 if(u >= x)a=x; else b=x;
Kotttaro 2:57237f0a4a34 536 SHIFT(v,w,x,u);
Kotttaro 2:57237f0a4a34 537 SHIFT(fv,fw,fx,fu);
Kotttaro 2:57237f0a4a34 538 }
Kotttaro 2:57237f0a4a34 539 else{
Kotttaro 2:57237f0a4a34 540 if(u < x){a=u;}
Kotttaro 2:57237f0a4a34 541 else {b=u;}
Kotttaro 2:57237f0a4a34 542 if(fu <= fw || w==x)
Kotttaro 2:57237f0a4a34 543 {
Kotttaro 2:57237f0a4a34 544 v=w;
Kotttaro 2:57237f0a4a34 545 w=u;
Kotttaro 2:57237f0a4a34 546 fv=fw;
Kotttaro 2:57237f0a4a34 547 fw=fu;
Kotttaro 2:57237f0a4a34 548 }
Kotttaro 2:57237f0a4a34 549 else if (fu <= fv || v==x || v==w)
Kotttaro 2:57237f0a4a34 550 {
Kotttaro 2:57237f0a4a34 551 v=u;
Kotttaro 2:57237f0a4a34 552 fv=fu;
Kotttaro 2:57237f0a4a34 553 }
Kotttaro 2:57237f0a4a34 554 }
Kotttaro 2:57237f0a4a34 555
Kotttaro 2:57237f0a4a34 556 }
Kotttaro 4:8a50c7822dac 557 //pc2.printf("xmin=%lf\r\n",x);
Kotttaro 4:8a50c7822dac 558 //pc2.printf("fx=%lf\r\n",fx);
Kotttaro 2:57237f0a4a34 559 return xmin;
Kotttaro 4:8a50c7822dac 560 }
Kotttaro 4:8a50c7822dac 561
Kotttaro 2:57237f0a4a34 562 double SIGN(double x,double y)
Kotttaro 2:57237f0a4a34 563 {
Kotttaro 2:57237f0a4a34 564 double x_return;
Kotttaro 2:57237f0a4a34 565 x_return=abs(x);
Kotttaro 2:57237f0a4a34 566 if(y<0.0)x_return=-x_return;
Kotttaro 2:57237f0a4a34 567
Kotttaro 2:57237f0a4a34 568 return x_return;
Kotttaro 2:57237f0a4a34 569 }
Kotttaro 4:8a50c7822dac 570 double FMAX(double x, double y)
Kotttaro 4:8a50c7822dac 571 {
Kotttaro 4:8a50c7822dac 572 if(x>y){return x;}
Kotttaro 4:8a50c7822dac 573 if(y>x){return y;}
Kotttaro 4:8a50c7822dac 574 return 0;
Kotttaro 4:8a50c7822dac 575 }
Kotttaro 2:57237f0a4a34 576 double num_nolm(int leg,double dth3)
Kotttaro 2:57237f0a4a34 577 {
Kotttaro 4:8a50c7822dac 578
Kotttaro 2:57237f0a4a34 579 double nolm_return=0.0;
Kotttaro 4:8a50c7822dac 580 solve(dth3,leg,2);
Kotttaro 4:8a50c7822dac 581 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];
Kotttaro 4:8a50c7822dac 582
Kotttaro 2:57237f0a4a34 583 return nolm_return;
Kotttaro 4:8a50c7822dac 584 }
Kotttaro 4:8a50c7822dac 585 //brent法のテスト用の関数
Kotttaro 4:8a50c7822dac 586 //極小値を求めたい関数を定義
Kotttaro 4:8a50c7822dac 587 double f_test(double x){
Kotttaro 4:8a50c7822dac 588 double x_return;
Kotttaro 4:8a50c7822dac 589 x_return=x*x-2*x+1;
Kotttaro 4:8a50c7822dac 590 return x_return;
Kotttaro 4:8a50c7822dac 591 }
Kotttaro 0:4a5272e014d8 592 void solve(double w3, int leg,int det) {
Kotttaro 4:8a50c7822dac 593
Kotttaro 0:4a5272e014d8 594 double dth[4];
Kotttaro 0:4a5272e014d8 595 MatrixXd v_Q(3,1),QT(3,3);
Kotttaro 0:4a5272e014d8 596
Kotttaro 0:4a5272e014d8 597 QT = Q.transpose();
Kotttaro 0:4a5272e014d8 598 v_Q = QT * vP[leg]*sampling;
Kotttaro 4:8a50c7822dac 599
Kotttaro 2:57237f0a4a34 600 dth[3] = w3 ;
Kotttaro 0:4a5272e014d8 601 dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
Kotttaro 0:4a5272e014d8 602 dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
Kotttaro 0:4a5272e014d8 603 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));
Kotttaro 0:4a5272e014d8 604 if (det == 1) {
Kotttaro 0:4a5272e014d8 605 for (int i=0; i < 4; i++) {
Kotttaro 4:8a50c7822dac 606 th[leg][i] = th[leg][i] + dth[i];
Kotttaro 0:4a5272e014d8 607 }
Kotttaro 0:4a5272e014d8 608 }
Kotttaro 4:8a50c7822dac 609 else if (det == 2) {
Kotttaro 4:8a50c7822dac 610 for (int u=0; u < 4; u++) {
Kotttaro 4:8a50c7822dac 611 th0[leg][u] = dth[u];
Kotttaro 4:8a50c7822dac 612 }
Kotttaro 4:8a50c7822dac 613 }
Kotttaro 4:8a50c7822dac 614
Kotttaro 0:4a5272e014d8 615 }
Kotttaro 0:4a5272e014d8 616
Kotttaro 4:8a50c7822dac 617 //サーボ関係
Kotttaro 4:8a50c7822dac 618 void setup_servo() {
Kotttaro 4:8a50c7822dac 619 pwm.begin();
Kotttaro 4:8a50c7822dac 620 pwm.setPWMFreq(200);
Kotttaro 4:8a50c7822dac 621 }
Kotttaro 4:8a50c7822dac 622 void servo_write(int ch,double ang)//引数は[°]
Kotttaro 4:8a50c7822dac 623 {
Kotttaro 4:8a50c7822dac 624 if(ch==0) ang=ang-135*PI/180;
Kotttaro 4:8a50c7822dac 625 if(ch==4) ang=ang-45*PI/180;
Kotttaro 4:8a50c7822dac 626 if(ch==8) ang=ang+45*PI/180;
Kotttaro 4:8a50c7822dac 627 if(ch==12) ang=ang+135*PI/180;
Kotttaro 4:8a50c7822dac 628 if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang;
Kotttaro 4:8a50c7822dac 629 ang=servo0[ch]+ang*8000/270*180/PI;
Kotttaro 4:8a50c7822dac 630 servo_write7(ch,ang);
Kotttaro 4:8a50c7822dac 631 }
Kotttaro 4:8a50c7822dac 632
Kotttaro 4:8a50c7822dac 633 void servo_write7(int ch, double ang){
Kotttaro 4:8a50c7822dac 634 ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
Kotttaro 4:8a50c7822dac 635 //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ; //初期状態を設定するときこの値を参考に設定したためそのまま利用　　
Kotttaro 4:8a50c7822dac 636 pwm.setPWM(ch, 0, ang);
Kotttaro 4:8a50c7822dac 637 }
Kotttaro 4:8a50c7822dac 638 void servo_calib()
Kotttaro 4:8a50c7822dac 639 {
Kotttaro 4:8a50c7822dac 640 for(int u=0; u<4; u++)
Kotttaro 4:8a50c7822dac 641 {
Kotttaro 4:8a50c7822dac 642 for(int i=0; i<4; i++)
Kotttaro 4:8a50c7822dac 643 {
Kotttaro 4:8a50c7822dac 644 servo_write7(ch[u][i],servo0[ch[u][i]]);
Kotttaro 4:8a50c7822dac 645 }
Kotttaro 4:8a50c7822dac 646 }
Kotttaro 4:8a50c7822dac 647 }
Kotttaro 5:f225e0c61cfc 648 void servo_ready()
Kotttaro 5:f225e0c61cfc 649 {
Kotttaro 5:f225e0c61cfc 650 for(int u=0; u<4; u++)
Kotttaro 5:f225e0c61cfc 651 {
Kotttaro 5:f225e0c61cfc 652 for(int i=0; i<4; i++)
Kotttaro 5:f225e0c61cfc 653 {
Kotttaro 5:f225e0c61cfc 654 servo_write(ch[u][i],th_ready[u][i]);
Kotttaro 5:f225e0c61cfc 655 }
Kotttaro 5:f225e0c61cfc 656 }
Kotttaro 5:f225e0c61cfc 657 }