Kotaro Irisawa
2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 3:f824e4d8eef7
- Parent:
- 2:57237f0a4a34
- Child:
- 4:8a50c7822dac
--- a/main.cpp Wed Dec 22 01:16:37 2021 +0000
+++ b/main.cpp Wed Dec 22 07:06:14 2021 +0000
@@ -13,7 +13,7 @@
//#pragma warning(disable: 4996)
Serial pc2(USBTX,USBRX);
Timer tim;
-int times= 1000;//実行回数:実行時間は??秒
+int times= 1;//実行回数:実行時間は??秒
double PI =3.14159265358979323846264338327950288;
using namespace Eigen;
@@ -61,7 +61,9 @@
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 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の符号をつけたものを返す
@@ -105,11 +107,26 @@
Jac(0);
QR(0);
deff(0);
- dth=brent(0,-5.0,1.90985,5.0,0.001,1);
- solve(dth, 0, 1);
+ 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("%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();
@@ -175,37 +192,7 @@
H1T = H1.transpose();
H2T = H2.transpose();
Q = H1T * H2T;
- //printf("%lf %lf %lf \n,", R0(0, 0), R0(1, 1), R0(2, 2));
- // printf("\n");
- //printf("%lf %lf %lf \n,", R0(0, 2), R0(1, 2), R0(2, 2));//
- /* R << R0(0, 0), R0(0, 1), R0(0, 2), Jacbi[leg][0][3],
- R0(1, 0), R0(1, 1), R0(1, 2), Jacbi[leg][1][3],
- R0(2, 0), R0(2, 1), R0(2, 2), Jacbi[leg][2][3];
- //printf("%lf\n\n", Jacbi[leg][2][3]);
- printf("R :%lf %lf %lf %lf\n", R(0, 0), R(0, 1), R(0, 2), R(0, 3));
- printf(" %lf %lf %lf %lf\n", R(1, 0), R(1, 1), R(1, 2), R(1, 3));
- printf( "%lf %lf %lf %lf\n\n", R(2, 0), R(2, 1), R(2, 2), R(2, 3));
- printf("Q :%lf %lf %lf \n", Q(0, 0), Q(0, 1), Q(0, 2));
- printf(" %lf %lf %lf \n", Q(1, 0), Q(1, 1), Q(1, 2));
- printf(" %lf %lf %lf \n\n", Q(2, 0), Q(2, 1), Q(2, 2));
-
- MatrixXd check(3, 4);
- check = Q * R;
- printf("Jac :%lf %lf %lf %lf\n", check(0, 0), check(0, 1), check(0, 2), check(0, 3));
- printf(" %lf %lf %lf %lf\n", check(1, 0), check(1, 1), check(1, 2), check(1, 3));
- printf(" %lf %lf %lf %lf\n\n", check(2, 0), check(2, 1), check(2, 2), check(2, 3));
-
-
-
- // printf("\n");
- //printf("QR finishued\n");
- //double R2[4];
-
- //R2[0] = R(0, 0);
- //R2[0] = R(1, 1);
- //R2[0] = R(2, 2);
- //R2[0] = R(2, 3);
- //printf("Rの値 %lf %lf %lf %lf\n,", R2[0], R2[1], R2[2], R2[3]);*/
+
}
@@ -302,160 +289,22 @@
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]);
-
- /////////////////////////////以下専攻科1年/////////////////////////////////////////////////////////////////////////////////////////////
- /*double dadth[4];
- double daindth[4];
-
-
- double cfi = (double)cos(fi), sfi = (double)sin(fi);
-
- //aの中身だけの微分
- daindth[0] = 2 * (-con[leg][0] * s0 + con[leg][1] * c0) * (L[3] + L[0] * c1 + L[1] * c12 + L[2] * c123);//ok 2.21たぶんcon[leg][1]の前に-
- daindth[1] = 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[0] * s1 - L[1] * s12 - L[2] * s123) - 2 * L[3] * L[0] * s1 - 2*L[3] * L[1] * s12 - 2*L[3]*L[2]*s123;
- daindth[2] = -2 * L[0] * L[1] * s2 - 2 * L[0] * L[0]*s23 +2*(-L[1]*c12-L[2]*c123)*(con[leg][0]*c0+con[leg][1]*s0) - 2 * L[3] * L[1] * s12 - 2 * L[3] * L[2] * s123;//L0*L0ではない？
- daindth[3] = -2 * L[0] * L[2] * s23 - L[1] * L[2] * s3 + 2 * (con[leg][0] * c0 + con[leg][1]*s0) * (-L[2]*s123) - 2 * L[3] * L[2] * s123;
-
-
- //fiの微分
- //tan-1の分母分子それぞれの微分に分ける
- //分母
- double dtandth_d[4];
- dtandth_d[0] = (2 * (-con[leg][0] * s0 + con[leg][1] * c0) * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123)) / (2 * tan_d);//ok 2.21
- dtandth_d[1] = (2 * L[0] * L[0] * c1 * (-s1) + 2 * L[1] * L[1] * c12 * (-s12) + 2 * L[2] * L[2] * c123 * (-s123)
- - 2 * L[0] * L[1] * sin(2*th[leg][1]+th[leg][2]) - L[0] * L[2] * sin(2*th[leg][1]+th[leg][2]+ th[leg][3])
- + 2 * L[1] * L[2] * sin(2*th[leg][1]+2*th[leg][2]+th[leg][3])
- - 2 * L[3] * L[0] * s1 - 2 * L[3] * L[1] * s12 - 2 * L[3] * L[2] * s123
- + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[0] * s1 - L[1] * s12 - L[2] * s123))/(2*tan_d);
- dtandth_d[2] =( 2 * L[1] * L[1] * c12 * (-s12) + 2 * L[2] * L[2] * c123 * (-s123)
- - 2 * L[0] * L[1] * c1 * s12 - 2 * L[0] * L[2] * c1 * s123 - 2 * L[1] * L[2] * sin(th[leg][1]+2* th[leg][2]+ th[leg][3])
- - 2 * L[3] * L[1] * s12 - 2 * L[3] * L[2] * s123
- + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[1] * s12 - L[2] * s123)) / (2 * tan_d);
- dtandth_d[3] = (2 * L[2]* L[2] * c123 * (-s123) - 2 * L[0] * L[2] * c1 * s123
- - 2 * L[1] * L[2] * c12 * s123 - 2 * L[3] * L[2] * s123
- + 2 * (con[leg][0] * c0 + con[leg][1] * s0) * (-L[2]*s123)) / (2 * tan_d);
-
- //分子
- double dtandth_u[4];
- dtandth_u[0] = 0;//ok 2.21
- dtandth_u[1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
- dtandth_u[2] = L[1] * c12 + L[2] * c123;
- dtandth_u[3] = L[2] * c123;
-
-
- //tan-1の商の微分=Φの微分
- double dfidth[4];
- dfidth[0] = (1 / (X * X + 1)) * (dtandth_u[0] * tan_d - dtandth_d[0] * tan_u) / (tan_d * tan_d);
- dfidth[1] = (1 / (X * X + 1)) * (dtandth_u[1] * tan_d - dtandth_d[1] * tan_u) / (tan_d * tan_d);
- dfidth[2] = (1 / (X * X + 1)) * (dtandth_u[2] * tan_d - dtandth_d[2] * tan_u) / (tan_d * tan_d);
- dfidth[3] = (1 / (X * X + 1)) * (dtandth_u[3] * tan_d - dtandth_d[3] * tan_u) / (tan_d * tan_d);
-
- //総合したaの微分
- dadth[0] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[0] + (double)sqrt(a0) * (-sfi) * dfidth[0];
- dadth[1] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[1] + (double)sqrt(a0) * (-sfi) * dfidth[1];
- dadth[2] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[2] + (double)sqrt(a0) * (-sfi) * dfidth[2];
- dadth[3] = 1 / (2 * (double)sqrt(a0)) * cfi * daindth[3] + (double)sqrt(a0) * (-sfi) * dfidth[3];
- //aの微分終わり
-
- //fの微分
- dfdth[0] = dadth[0] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[0] + a / (cfi * cfi) * dfidth[0]);
- dfdth[1] = dadth[1] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[1] + a / (cfi * cfi) * dfidth[1]);
- dfdth[2] = dadth[2] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[2] + a / (cfi * cfi) * dfidth[2]);
- dfdth[3] = dadth[3] * (1 / (double)cos(fi) - tan(fi)) + a * (sfi / (cfi * cfi) * dfidth[3] + a / (cfi * cfi) * dfidth[3]);
- */
- //pc2.printf("%lf, %lf, %lf, %lf\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]);
- //printf("評価関数微分完了\n");
+ //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 search(int leg) {
- //printf("探索関数開始\n");
- //th3の角速度のみを変更し, ∇hが最大となるところを探索する
- double dth=-500.0*PI/180;
- //double comp_dfdth[4] = {0.0,0.0 ,0.0 ,0.0 };
- //double dfdth_sum=0.0, dfdth_sum_before=0.0;
- double dth_return=0.0;
- double e=500.0;
- double e_min = 100000000000000000000000000.0;
- double dfd_nolm = 0.0;
- 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;
- }
- //printf("%lf, %lf, %lf, %lf\n", dfdth[0], dfdth[1], dfdth[2], dfdth[3]);
-
- //以下総当たりの探索for文
- //0.5度ずつでdthをずらしながら2000回の探索を行う
- for (int i = 0; i < 1000; i++) {
- double th0_nolm = 0.0;
- dth = dth + (double)(1.0 * PI / 180);
- solve(dth, leg, 2);//後退代入でほかの3つのパラメータを導出
- e = 0.0;
- 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]);
- //dthベクトルを正規化
- for (int i = 0; i < 4; i++) {
- th0[leg][i] = th0[leg][i] / th0_nolm;
- }
- //printf("%lf, %lf, %lf, %lf\n", th0[leg][0], th0[leg][1], th0[leg][2], th0[leg][3]);
- //評価関数の勾配を単位化したベクトルとの差分のベクトルのノルムの2乗を計算
- for (int i = 0; i < 4; i++) {
- e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
- }
- //評価関数の勾配方向に最も近いもの、つまり上のfor文の計算結果の値の一番小さいものを採用する
- //printf("%d, %lf\n", i, e);
- if (e_min > e) {
- e_min = e;
-
- dth_return = dth;
-
- }
-
-
- }
-
- //}
- //else{
- // for (int i = 0; i < 500; i++) {
- //printf("探索ループ内");
- // dth = dth + 0.5 * PI / 180;
- //solve(dth, leg, 2);
- //for (int i = 0; i < 4; i++) {
- //th0_nolm += th0[leg][i] * th0[leg][i];
- //}
- //正規化
- //for (int i = 0; i < 4; i++) {
- //th0[leg][i] = th0[leg][i] / th0_nolm;
- //}
- //for (int i = 0; i < 4; i++) {
- //e += (th[leg][i] - dfdth[i]) * (th[leg][i] - dfdth[i]);
- //}
- //if (e_min > e) {
- //e_min = e;
- //printf("%f", e_min);
- //dth_return = dth;
- //printf("%f\n", dth);
- //}
- //e = 0.0;
-
- //}
- //}
- //printf("探索関数終了");
- //printf("%lf\n", e_min);
- //
- //pc2.printf("%lf\n\r", dth_return);
- return dth_return;
-
- }
double fe(int leg,double dth3) {
//brent法のための関数, 事前にdfdを実行してから使う
- double dfd_nolm,th0_nolm,e;
-
+ 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つのパラメータを導出
@@ -465,23 +314,40 @@
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);
+ //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);
@@ -518,6 +384,7 @@
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;
@@ -583,11 +450,14 @@
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++) {