【人型ロボ開発記＃2】2足歩行モーション（3軸脚）
脚の動作過程・逆運動学は同じものです。(＃1参照)
klaft0729.hatenablog.jp
Arduinoコードは2脚を別々で動作させるため、時間変数で座標位置を算出するコードに改良しています。
各脚のモーションは反転しているものとし、右脚が踏み込む際に、左脚が地面を蹴る動作となっています。
↓実行結果はこちらです。
www.youtube.com
↓Arduinoコード
#include <Servo.h>
int Servo1_Socket = 3;
int Servo2_Socket = 5;
int Servo3_Socket = 6;
int Servo4_Socket =9;
int Servo5_Socket =10;
int Servo6_Socket =11;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
float th1 = 0, th2 = 0, th3 = 0, th4 = 0, th5 = 0, th6 = 0;
float phi = 3.14;
float L1 = 60.2, L2 = 60.2, L3 = 33;
float xR, yR, alphaR, xL, yL, alphaL;
float t, totalTime, lamda;
float xR_p1, yR_p1, xR_p2, yR_p2;
float alphaR_p1, alphaR_p2;
float xL_p1, yL_p1, xL_p2, yL_p2;
float alphaL_p1, alphaL_p2;
void setup(){
Serial.begin(115200);
servo1.attach(Servo1_Socket,500,2500);
servo2.attach(Servo2_Socket,500,2500);
servo3.attach(Servo3_Socket,500,2500);
servo4.attach(Servo4_Socket,500,2500);
servo5.attach(Servo5_Socket,500,2500);
servo6.attach(Servo6_Socket,500,2500);
set_servo();
//初期座標
xR = 0; yR = 116.56; alphaR = 0;
xL = 0; yL = 116.56; alphaL = 0;
Serial.println("Initialize");
}
void loop(){
//process1 R：1-2 L：5-1(1)
Serial.println("process1/////////////////////////////////////////////////");
//入力座標
xR_p1 = 70; yR_p1 = 90; alphaR_p1 = 60;
xR_p2 = 53; yR_p2 = 103; alphaR_p2 = 60;
xL_p1 = -70; yL_p1 = 90; alphaL_p1 = 120;
xL_p2 = -35; yL_p2 = 90; alphaL_p2 = 105;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process2 R：2-3 L：5-1(2)
Serial.println("process2/////////////////////////////////////////////////");
//入力座標
xR_p1 = 53; yR_p1 = 103; alphaR_p1 = 60;
xR_p2 = 0; yR_p2 = 110; alphaR_p2 = 90;
xL_p1 = -35; yL_p1 = 90; alphaL_p1 = 105;
xL_p2 = -17.5; yL_p2 = 90; alphaL_p2 = 82.5;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process3 R：3-4 L：5-1(3)
Serial.println("process3/////////////////////////////////////////////////");
//入力座標
xR_p1 = 0; yR_p1 = 110; alphaR_p1 = 90;
xR_p2 = -62; yR_p2 = 98; alphaR_p2 = 120;
xL_p1 = -17.5; yL_p1 = 90; alphaL_p1 = 82.5;
xL_p2 = 56.88; yL_p2 = 90; alphaL_p2 = 65.62;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process4 R：4-5 L：5-1(4)
Serial.println("process4/////////////////////////////////////////////////");
//入力座標
xR_p1 = -62; yR_p1 = 98; alphaR_p1 = 120;
xR_p2 = -70; yR_p2 = 90; alphaR_p2 = 120;
xL_p1 = 56.88; yL_p1 = 90; alphaL_p1 = 65.62;
xL_p2 = 70; yL_p2 = 90; alphaL_p2 = 60;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process5 R：5-1(1) L：1-2
Serial.println("process5/////////////////////////////////////////////////");
//入力座標
xR_p1 = -70; yR_p1 = 90; alphaR_p1 = 120;
xR_p2 = -35; yR_p2 = 90; alphaR_p2 = 105;
xL_p1 = 70; yL_p1 = 90; alphaL_p1 = 60;
xL_p2 = 53; yL_p2 = 103; alphaL_p2 = 60;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process6 R：5-1(2) L：2-3
Serial.println("process6/////////////////////////////////////////////////");
//入力座標
xR_p1 = -35; yR_p1 = 90; alphaR_p1 = 105;
xR_p2 = -17.5; yR_p2 = 90; alphaR_p2 = 82.5;
xL_p1 = 53; yL_p1 = 103; alphaL_p1 = 60;
xL_p2 = 0; yL_p2 = 110; alphaL_p2 = 90;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process7 R：5-1(3) L：3-4
Serial.println("process7/////////////////////////////////////////////////");
//入力座標
xR_p1 = -17.5; yR_p1 = 90; alphaR_p1 = 82.5;
xR_p2 = 56.88; yR_p2 = 90; alphaR_p2 = 65.62;
xL_p1 = 0; yL_p1 = 110; alphaL_p1 = 90;
xL_p2 = -62; yL_p2 = 98; alphaL_p2 = 120;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
//process8 R：5-1(4) L：4-5
Serial.println("process8/////////////////////////////////////////////////");
//入力座標
xR_p1 = 56.88; yR_p1 = 90; alphaR_p1 = 65.62;
xR_p2 = 70; yR_p2 = 90; alphaR_p2 = 60;
xL_p1 = -62; yL_p1 = 98; alphaL_p1 = 120;
xL_p2 = -70; yL_p2 = 90; alphaL_p2 = 120;
t = 0;
totalTime = 1;
while(t <= 0.9){
Coordinate();
ik();
set_servo();
thprint();
t = t + 0.1;
}
}
void Coordinate(){
lamda = t / totalTime;
//座標計算
//Right Leg
xR_p1 = xR_p1 + (xR_p2 - xR_p1) * lamda;
yR_p1 = yR_p1 + (yR_p2 - yR_p1) * lamda;
alphaR_p1 = alphaR_p1 + (alphaR_p2 - alphaR_p1) * lamda;
xR = xR_p1;
yR = yR_p1;
alphaR = alphaR_p1;
//Left Leg
xL_p1 = xL_p1 + (xL_p2 - xL_p1) * lamda;
yL_p1 = yL_p1 + (yL_p2 - yL_p1) * lamda;
alphaL_p1 = alphaL_p1 + (alphaL_p2 - alphaL_p1) * lamda;
xL = xL_p1;
yL = yL_p1;
alphaL = alphaL_p1;
}
void ik(){
//逆運動学計算
float xR1, yR1, kR, th1_r, th2_r, th3_r, alphaR_r;
float xL1, yL1, kL, th4_r, th5_r, th6_r, alphaL_r;
float AR, BR, CR, DR;
float AL, BL, CL, DL;
//Right ik
alphaR_r = alphaR * phi / 180;
xR1 = xR - L3 * cos(alphaR_r);
yR1 = yR - L3 * sin(alphaR_r);
AR = xR1*xR1 + yR1*yR1 + L1*L1 + L2*L2;
BR = xR1*xR1 + yR1*yR1;
CR = L1*L1*L1*L1 + L2*L2*L2*L2;
kR = sqrt(AR * AR - 2 * (BR * BR + CR));
th1_r = atan2(yR1, xR1) - atan2(kR, BR + L1*L1 - L2*L2);
th2_r = atan2(kR, BR - L1*L1 - L2*L2);
th3_r = phi / 2 + alphaR_r - (th1_r + th2_r);
th1 = th1_r * 180 / phi;
th2 = th2_r * 180 / phi;
th3 = th3_r * 180 / phi;
//Left ik
alphaL_r = alphaL * phi / 180;
xL1 = xL - L3 * cos(alphaL_r);
yL1 = yL - L3 * sin(alphaL_r);
AL = xL1 * xL1 + yL1 * yL1 + L1*L1 + L2*L2;
BL = xL1 * xL1 + yL1 * yL1;
CL = L1*L1*L1*L1 + L2*L2*L2*L2;
kL = sqrt(AL * AL - 2 * (BL * BL + CL));
th4_r = atan2(yL1, xL1) - atan2(kL, BL + L1*L1 - L2*L2);
th5_r = atan2(kL, BL - L1*L1 - L2*L2);
th6_r = phi / 2 + alphaL_r - (th4_r + th5_r);
th4 = th4_r * 180 / phi;
th5 = th5_r * 180 / phi;
th6 = th6_r * 180 / phi;
}
void set_servo(){
//サーボ出力
servo1.write(th1);
servo2.write(th2);
servo3.write(th3);
servo4.write(th4);
servo5.write(th5);
servo6.write(th6);
delay(5);
}
void thprint(){
//座標・サーボ角モニタ
Serial.print("xR：");
Serial.print(xR);
Serial.print(" yR：");
Serial.print(yR);
Serial.print(" alphaR：");
Serial.print(alphaR);
Serial.print(" th1：");
Serial.print(th1);
Serial.print(" th2：");
Serial.print(th2);
Serial.print(" th3：");
Serial.println(th3);
Serial.print("xL：");
Serial.print(xL);
Serial.print(" yL：");
Serial.print(yL);
Serial.print(" alphaL：");
Serial.print(alphaR);
Serial.print(" th4：");
Serial.print(th4);
Serial.print(" th5：");
Serial.print(th5);
Serial.print(" th6：");
Serial.println(th6);
}シリアルモニタで各足先座標・関節角をモニタ出来ます。
ご覧いただきありがとうございました。
