先週までの振り返り
先週の記事では、逆運動学を解くためにプログラムの試作などを行いました。
数値解法への挑戦、pinocchioの試用、解析的手法で腕と脚に挑戦などを行いました。
今週の概要
先週までにおこなった逆運動学の解析的手法に関して、脚を解析的に解き、プログラムに起こします。そして、そのプログラムをWebots_ros2に実装してロボットを制御します。
進捗記録
2022/12/29
- 逆運動学を解析的に解く
・ROBOTIS-OP2の左脚の逆運動学を解析的に解いた。ROBOTIS-OP2の左脚の機構とステータスは、以下のようになっている。ここでは左脚としているが、world -> p1 のリンクのy要素の正負を逆にすれば右脚となる。
左脚の構成とステータス 参考: GitHub, cyberbotics, "webots/projects/robots/robotis/darwin-op/protos/Darwin-op.proto", https://github.com/cyberbotics/webots/blob/R2022b/projects/robots/robotis/darwin-op/protos/Darwin-op.proto
GitHub, ROBOTIS-GIT, "ROBOTIS-OP2-Common/robotis_op2_description/urdf/robotis_op2.structure.leg.xacro", https://github.com/ROBOTIS-GIT/ROBOTIS-OP2-Common/blob/master/robotis_op2_description/urdf/robotis_op2.structure.leg.xacro
これを元に、解析的に逆運動学を解くと、以下の式を得る。
- 解析解を元にプログラムを作成する
・上で得られた解を元に、プログラムに起こして正確な解が得られるかを確認する。
・実装する場合はC++で記述する予定だが、今回は試作に近い段階であることからPython3で記述している。（個人の練度的にPythonのほうが作りやすい）
拙いが、以下のプログラムを作成した。
#!/usr/bin/env python3 import numpy as np def identityMatrix(): return np.matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) def Rx(theta1): return np.matrix([[1, 0 , 0 ], [0, np.cos(theta1), -np.sin(theta1)], [0, np.sin(theta1), np.cos(theta1) ]] ) def Ry(theta2): return np.matrix([[np.cos(theta2) , 0, np.sin(theta2)], [0 , 1, 0 ], [-np.sin(theta2), 0, np.cos(theta2)]] ) def Rz(theta3): return np.matrix([[np.cos(theta3), -np.sin(theta3), 0], [np.sin(theta3), np.cos(theta3) , 0], [0 , 0 , 1]] ) Pw1 = np.matrix([-0.005, 0.037, -0.1222]) P12 = np.matrix([0, 0, 0]) P23 = np.matrix([0, 0, 0]) P34 = np.matrix([0, 0, -0.093]) P45 = np.matrix([0, 0, -0.093]) P56 = np.matrix([0, 0, 0]) # Target_Point Rw6_target = identityMatrix() Pw6_target = np.array([0.05, 0.1, -0.194]) """ IK """ qqq = Pw1 - Pw6_target P16_target = np.array((Rw6_target * qqq.T).T)[0] a = np.abs(P34[0,2]) b = np.abs(P45[0,2]) c = np.sqrt(P16_target[0]**2 + P16_target[1]**2 + P16_target[2]**2) Q3 = -np.arccos((a**2 + b**2 - c**2)/(2 * a * b)) + np.pi alfa = np.arcsin((a * np.sin(np.pi - Q3))/c) Q4 = -np.arctan2(P16_target[0], np.sign(P16_target[2]) * np.sqrt(P16_target[1]**2 + P16_target[2]**2)) - alfa Q5 = np.arctan2(P16_target[1], P16_target[2]) # print(P16_target[1], P16_target[2], np.arctan2(P16_target[1], P16_target[2])) # When general Rw6_target R1_2_3 = np.dot(Rw6_target, np.dot(Rx(Q5).T, np.dot(Ry(Q4).T, Ry(Q3).T))) Q0 = np.arctan2(-1*R1_2_3[0, 1], R1_2_3[1, 1]) Q1 = np.arctan2(R1_2_3[2, 1], -1*R1_2_3[0, 1] * np.sin(Q0) + R1_2_3[1, 1] * np.cos(Q0)) Q2 = np.arctan2(-1*R1_2_3[2, 0], R1_2_3[2, 2]) # When identity-matrix Rw6_target # Q[0] = np.arctan2(0, np.cos(Q[5])) # Q[1] = np.arctan2(np.sin(Q[5]), np.cos(Q[5]) * np.cos(Q[0])) # Q[2] = np.arctan2(np.cos(Q[5]) * np.sin(Q[3] + Q[4]), np.cos(Q[5]) * np.cos(Q[3] + Q[4])) Q = np.array([Q0, Q1, Q2, Q3, Q4, Q5]) print("target[m]: ", Pw6_target) print("Angles[rad]: ", Q, ",\n______[deg]: ", (Q*180/np.pi).astype(np.int16)) """ FK """ Rw1 = Rz(Q0) R12 = Rx(Q1) R23 = Ry(Q2) R34 = Ry(Q3) R45 = Ry(Q4) R345 = Ry(Q2+Q3+Q4) R24 = Ry(Q2+Q3) R56 = Rx(Q5) Pw1 = np.matrix([[-0.005], [0.037], [-0.1222]]) P12 = np.matrix([[0], [0], [0]]) P23 = np.matrix([[0], [0], [0]]) P34 = np.matrix([[0], [0], [-0.093]]) P45 = np.matrix([[0], [0], [-0.093]]) P56 = np.matrix([[0], [0], [0]]) P6a = np.matrix([[0], [0], [0]]) FK_result = Rw1 @ R12 @ R24 @ P45 + Rw1 @ R12 @ R23 @ P34 + Pw1 print("FK_result[m]: ", FK_result.T, "\n")
アルゴリズムは手書きした式とほぼ同じで、それをプログラムに起こしただけとなっている。
・このプログラムを実行することで、以下の結果が得られた。
target: 目標位置, Angles: IKを解いた結果得られた各関節角度[PelvYL, PelvL, ~, FootL], FK_result: IKの結果を用いて解いたFKの結果 -----axis-z----- target[m]: [[-0.005 0.037 -0.1687]] Angles[rad]: [-0. 0. -1.31811607 2.63623214 -1.31811607 0. ] , ______[deg]: [ 0 0 -75 151 -75 0] FK_result[m]: [[-0.005 0.037 -0.1687]] target[m]: [[-0.005 0.037 -0.2152]] Angles[rad]: [-0. 0. -1.04719755 2.0943951 -1.04719755 0. ] , ______[deg]: [ 0 0 -59 119 -59 0] FK_result[m]: [[-0.005 0.037 -0.2152]] target[m]: [[-0.005 0.037 -0.2617]] Angles[rad]: [-0. 0. -0.72273425 1.4454685 -0.72273425 0. ] , ______[deg]: [ 0 0 -41 82 -41 0] FK_result[m]: [[-0.005 0.037 -0.2617]] ----- -----axis-x----- target[m]: [ 0.15 0.037 -0.2152] Angles[rad]: [-0. 0. -1.26831795 0.47588225 0.7924357 0. ] , ______[deg]: [ 0 0 -72 27 45 0] FK_result[m]: [[ 0.15 0.037 -0.2152]] target[m]: [ 0.05 0.037 -0.2152] Angles[rad]: [-0. 0. -1.48504012 1.90193948 -0.41689936 0. ] , ______[deg]: [ 0 0 -85 108 -23 0] FK_result[m]: [[ 0.05 0.037 -0.2152]] target[m]: [-0.1 0.037 -0.2152] Angles[rad]: [-0. 0. 0.02150706 1.549058 -1.57056506 0. ] , ______[deg]: [ 0 0 1 88 -89 0] FK_result[m]: [[-0.1 0.037 -0.2152]] ----- -----axis-y----- target[m]: [-0.005 0.15 -0.2152] Angles[rad]: [-0. 0.88218221 -0.66515354 1.33030708 -0.66515354 -0.88218221] , ______[deg]: [ 0 50 -38 76 -38 -50] FK_result[m]: [[-0.005 0.15 -0.2152]] target[m]: [-0.005 0.1 -0.2152] Angles[rad]: [-0. 0.59540988 -0.92238109 1.84476219 -0.92238109 -0.59540988] , ______[deg]: [ 0 34 -52 105 -52 -34] FK_result[m]: [[-0.005 0.1 -0.2152]] ----- target[m]: [ 0.05 0.1 -0.194] Angles[rad]: [-0. 0.72020877 -1.45894155 1.87302709 -0.41408554 -0.72020877] , ______[deg]: [ 0 41 -83 107 -23 -41] FK_result[m]: [[ 0.05 0.1 -0.194]]
この結果から分かるように、targetと逆運動学の解析解が一致している。このことから、このプログラムであれば正しい逆運動学の解析解が得られることが分かる。
・以上のことから、実装するプログラムは上記のプログラムと同じアルゴリズムを用いることにする。
2023/1/18
１．試作プログラム（C++編）
・上記で示したPythonの試作プログラムとそのアルゴリズムを元に、C++で試作プログラムを書いた。C++で行列計算を行うために、Eigen3という、行列計算に特化した外部ライブラリを用いている。
・以下に、ソースコードと出力結果を示す。
#include "iostream" #include "cmath" #include "Eigen/Dense" bool DEBUG = true; #define PI 3.141592 using namespace Eigen; using namespace std; Matrix3d Rx(double theta) { Matrix3d R; R << 1, 0, 0, 0, cos(theta), -sin(theta), 0, sin(theta), cos(theta); return(R); } Matrix3d Ry(double theta) { Matrix3d R; R << cos(theta) , 0, sin(theta), 0 , 1, 0 , -sin(theta), 0, cos(theta); return(R); } Matrix3d Rz(double theta) { Matrix3d R; R << cos(theta), -sin(theta), 0, sin(theta), cos(theta) , 0, 0 , 0 , 1; return(R); } Matrix3d IdentifyMatrix(void) { Matrix3d R; R << 1, 0, 0, 0, 1, 0, 0, 0, 1; return(R); } double sign(double arg) { return((arg >= 0) - (arg < 0)); } namespace Parameters { Matrix3d R_target; Vector3d P_target; array<Vector3d, 7> P; array<float, 6> Q; void set_Parameters(void) { // パラメータの正式な取得方法は、は他ファイルに記述してそれを読み込む、 他nodeから読み込む if(DEBUG == true) { Vector3d P_w1(-0.005, 0.037, -0.1222); Vector3d P_12(0, 0, 0); Vector3d P_23(0, 0, 0); Vector3d P_34(0, 0, -0.093); Vector3d P_45(0, 0, -0.093); Vector3d P_56(0, 0, 0); Vector3d P_6a(0, 0, 0); P = {P_w1, P_12, P_23, P_34, P_45, P_56, P_6a}; } else if(DEBUG ==false) { // 正式な処理 } } } namespace Kinematics { using namespace Parameters; Vector3d FK(void) { array<Matrix3d, 6> R; R = {Rz(Q[0]), Rx(Q[1]), Ry(Q[2]), Ry(Q[3]), Ry(Q[4]), Rz(Q[5])}; return ( R[0] * R[1] * R[2] * R[3] * R[4] * R[5] * P[6] + R[0] * R[1] * R[2] * R[3] * R[4] * P[5] + R[0] * R[1] * R[2] * R[3] * P[4] + R[0] * R[1] * R[2] * P[3] + R[0] * R[1] * P[2] + R[0] * P[1] + P[0] ); } void IK(void) { Vector3d P_16; P_16 = R_target.transpose() * (P[0] - P_target); double A, B, C; A = abs(P[3](2)); B = abs(P[4](2)); C = sqrt(pow(P_16(0), 2) + pow(P_16(1), 2) + pow(P_16(2), 2)); Q[3] = -1 * acos((pow(A, 2) + pow(B, 2) - pow(C, 2)) / (2 * A * B)) + PI; double alfa; alfa = asin((A * sin(PI - Q[3])) / C); Q[4] = -1 * atan2(P_16(0), sign(P_16(2)) * sqrt(pow(P_16(1), 2) + pow(P_16(2), 2))) - alfa; Q[5] = atan2(P_16(1), P_16(2)); Matrix3d R_w3; R_w3 = R_target * Rx(Q[5]).transpose() * Ry(Q[4]).transpose() * Ry(Q[3]).transpose(); Q[0] = atan2(-R_w3(0, 1), R_w3(1, 1)); Q[1] = atan2(R_w3(2, 1), -R_w3(0, 1) * sin(Q[0]) + R_w3(1, 1) * cos(Q[0])); Q[2] = atan2(-R_w3(2, 0), R_w3(2, 2)); } } int main(void) { using namespace Parameters; // 最初に、パラメータ設定を行う bool SETUP; // 他nodeからのフラグで決めたい if(DEBUG == true) {SETUP = true;} if(DEBUG == true || SETUP == true) {set_Parameters();} if(DEBUG == true) { Q = {0, 0, 0, 0, 0, 0}; R_target = IdentifyMatrix(); P_target << -0.005, 0.037, -0.1687; } else if(DEBUG ==false) { // 他nodeからの情報を記録 Q = {0, 0, 0, 0, 0, 0}; R_target = IdentifyMatrix(); P_target << -0.005, 0.037, -0.1687; } Kinematics::IK(); Vector3d result_FK; result_FK = Kinematics::FK(); cout << "Target Points[m]: " << P_target.transpose() << endl; cout << "Target Rotation-Matrix: \n" << R_target << endl; cout << "Result IK: Joint-Angles[rad]: "; for(const auto &el : Q) { cout << el << ", "; } cout << endl; cout << "Result FK: Target Points[m]: " << result_FK.transpose() << endl; return(0); }
Target Points[m]: -0.005 0.037 -0.1687 Target Rotation-Matrix: 1 0 0 0 1 0 0 0 1 Result IK: Joint-Angles[rad]: -0, 0, -1.31811, 2.63623, -1.31812, 0, Result FK: Target Points[m]: -0.00500004 0.037 -0.1687
・以上の出力結果から、Pythonでの試作プログラムと同様に、正しい逆運動学解が得られていることが分かる。このプログラムをWebots_ros2の外部Pluginに適用してやれば、逆運動学解をWebots上のロボットに反映できるだろう。
２．Webots上のロボットを動かす
・上記のC++の試作プログラムを、そのまま外部PluginのWalkingPatternGenerator.cppに適用して、Webots上のロボットを動かした。
・上記のC++試作プログラムの出力結果に示されている関節角度[rad]をWebots上のロボットの左脚に適用した。
・以下に、その動作後の様子を示す。左脚をただ動かしただけなので、当然、自立できずに転倒してしまっているが、脚の関節角度は指示通りであることが見て取れる。
以上で、逆運動学解を元にロボットを動かせた。
