Kotaro Irisawa
2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 0:4a5272e014d8
- Child:
- 1:5c2562adca7d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Dec 18 10:02:57 2021 +0000
@@ -0,0 +1,443 @@
+#include "mbed.h"
+//特別研究Ⅰで用いたプログラム
+//ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する
+#include "Eigen/Geometry.h"
+#include "Eigen/Dense.h"
+#include <math.h>
+//#pragma warning(disable: 4996)
+Serial pc2(USBTX,USBRX);
+Timer tim;
+int times= 200;//実行回数:実行時間は7秒
+double PI =3.14159265358979323846264338327950288;
+
+using namespace Eigen;
+
+//以下変数定義
+double r=50*PI/180;//斜面の傾き[°]
+double sampling=0.1;//δ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,-60 * PI / 180,-60 * 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の角速度から全関節の関節角度を導き出す
+
+int main()
+{
+ double t;
+ pc2.baud(921600);
+ int count = 0;
+ //入力したねじ運動を換算する
+ Lin[0] = 0.0; //ねじ軸x
+ Lin[1] = 0.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;
+ 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);
+ dth=search(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 );
+
+ }
+
+
+ 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;
+ //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]);*/
+
+}
+
+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 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");
+}
+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 = 0.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 < 2000; i++) {
+ double th0_nolm = 0.0;
+ dth = dth + (double)(0.5 * 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;
+
+ }
+
+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 * sampling;
+ //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");
+ if (det == 1) {
+ for (int i=0; i < 4; i++) {
+ th[leg][i] = th[leg][i] + dth[i];
+
+ }
+ }
+ if (det == 2) {
+ for (int u=0; u < 4; u++) {
+ th0[leg][u] = dth[u];
+ }
+ }
+ //printf("後退代入終了\n");
+}
+