FK IK

• Forward Kinematics
• Inverse Kinematics

FK は関節の根本から回転させていって先端の座標を求める処理で、IK は先端座標から逆算して関節の回転角を求める処理です。ロボットアームの場合は IK ができれば良いことになります。

MeArm の構造と関節角度

MeArm は根本にサーボモータが設置されておりリンク機構を介して関節を動かしているので、サーボモータの角度と関節の角度の対応を理解する必要があります。

• X/Y/Z 論理座標系
• IK 回転角
• 物理サーボモータ回転角

の3つの要素があり、順に変換していく必要があります。

実装

mbed 用の実装 と Arduino用のインターフェイスを混ぜたような実装を書いて実験しました。

正確に動かすためには正確に制御方法を理解する方法があるので、結局ある程度自分でコードを試行錯誤して理解する必要がある気がします。

キャリブレーション方法が特に難しいのですが、以下のような方法で行いました。ロジックがあっていることが大前提ですが「動かしてみてあってるか確認する」のが難しいので、コードの検証はしっかりやらないとダメです。

• 電源を切った状態で複数の座標にアームを動かしてみて、そのときの各サーボモータの角度を記録する
• 実際に座標を計算して、サーボモータへ入力すべき角度との差分や係数を求める

#include <Arduino.h>
#include <Servo.h>

#include "math.h"
#include <Servo.h>

#define DEBUG 0

constexpr float DEG2RADIAN(float deg) {
	return deg * M_PI / 180;
}
constexpr float RADIAN2DEG(float rad) {
	return rad * 180 / M_PI;
}

class MeArm {
private:
	Servo base, shoulder, elbow, claw;
	float baseServoMin, baseServoMax, baseCoefficient, baseOffset;
	float shoulderServoMin, shoulderServoMax, shoulderCoefficient, shoulderOffset;
	float elbowServoMin, elbowServoMax, elbowCoefficient, elbowOffset;
	float clawServoMin, clawServoMax, clawCoefficient, clawOffset;

	float currentX, currentY, currentZ;
public:
	static const int16_t LENGTH_SEGMENT0 = 18; // base to shoulder
	static const int16_t LENGTH_SEGMENT1 = 80; // shoulder to elbow
	static const int16_t LENGTH_SEGMENT2 = 80; // elbow to wrist
	static const int16_t LENGTH_SEGMENT3 = 55; // wrist to grip point
	static const int16_t LENGTH_BASE_HEIGHT = 60;
	static const int16_t MAX_REACH = 220;

	static constexpr float lawOfCosines(float adj1, float adj2, float opp) {
		return acos((adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2));  
	}


	MeArm() {
		setServoCoefficient(
			0, 180, 1,  0,
			0, 180, -1, 0,
			0, 180, -1, 0,
			0, 180, 1, 0
		);
	}

	void begin(int pinBase, int pinShoulder, int pinElbow, int pinClaw) {
		base.attach(pinBase);
		shoulder.attach(pinShoulder);
		elbow.attach(pinElbow);
		claw.attach(pinClaw);
		base.write(90);
		shoulder.write(90);
		elbow.write(90);
		claw.write(90);
	}

	void setServoCoefficient(
		float _baseServoMin, float _baseServoMax, float _baseCoefficient, float _baseOffset,
		float _shoulderServoMin, float _shoulderServoMax, float _shoulderCoefficient, float _shoulderOffset,
		float _elbowServoMin, float _elbowServoMax, float _elbowCoefficient, float _elbowOffset,
		float _clawServoMin, float _clawServoMax, float _clawCoefficient, float _clawOffset
	) {
		baseServoMin = _baseServoMin;
		baseServoMax = _baseServoMax;
		baseCoefficient = _baseCoefficient;
		baseOffset = _baseOffset;
		shoulderServoMin = _shoulderServoMin;
		shoulderServoMax = _shoulderServoMax;
		shoulderCoefficient = _shoulderCoefficient;
		shoulderOffset = _shoulderOffset;
		elbowServoMin = _elbowServoMin;
		elbowServoMax = _elbowServoMax;
		elbowCoefficient = _elbowCoefficient;
		elbowOffset = _elbowOffset;
		clawServoMin = _clawServoMin;
		clawServoMax = _clawServoMax;
		clawCoefficient = _clawCoefficient;
		clawOffset = _clawOffset;
	}

	// https://developer.mbed.org/users/eencae/code/MeArm/docs/tip/MeArm_8cpp_source.html
	// by eencae
	bool solveInverseKinematics(
			float x, float y, float z,
			float& thetaBase, float& thetaShoulder, float& thetaElbow
	) const {
		float target = sqrt(x * x + y * y);
		if (target > MAX_REACH) return false;

		float wrist  = target - LENGTH_SEGMENT3;
		float shoulder = LENGTH_SEGMENT0;

		// shoulder to wrist
		float s2w = sqrt( (wrist - shoulder) * (wrist - shoulder) + (z - LENGTH_BASE_HEIGHT) * (z - LENGTH_BASE_HEIGHT) );
		if (s2w == 0.0) {
			return false;
		}

		if (DEBUG) {
			Serial.print("x = "); Serial.print(x); Serial.print(" ");
			Serial.print("y = "); Serial.print(y); Serial.print(" ");
			Serial.print("z = "); Serial.print(z); Serial.print(" ");
			Serial.print("target = "); Serial.print(target); Serial.print(" ");
			Serial.print("wrist = "); Serial.print(wrist); Serial.print(" ");
			Serial.print("shoulder = "); Serial.print(shoulder); Serial.print(" ");
			Serial.print("s2w = "); Serial.print(s2w); Serial.print(" ");
		}

		float thetaSw = acos( (wrist - shoulder) / s2w );
		if (isnan(thetaSw)) {
			Serial.println("invalid thetaSw");
			return false;
		}

		if (z < LENGTH_BASE_HEIGHT) {
			thetaSw *= -1.0;
		}

		float thetaElbow0 = lawOfCosines(LENGTH_SEGMENT1, LENGTH_SEGMENT2, s2w);
		if (isnan(thetaElbow0)) {
			Serial.println("thetaElbow0 is NaN");
			return false;
		}
		float thetaShoulder0 = lawOfCosines(s2w, LENGTH_SEGMENT1, LENGTH_SEGMENT2);
		if (isnan(thetaShoulder0)) {
			Serial.println("thetaShoulder0 is NaN");
			return false;
		}
		thetaShoulder = thetaSw + thetaShoulder0;
		thetaElbow = thetaElbow0;
		thetaBase = atan2(y, x);
		return true;
	}

	bool convertIKAnglesToServoAngles(
			float thetaBase, float thetaShoulder, float thetaElbow,
			float& thetaBaseServo, float& thetaShoulderServo, float& thetaElbowServo
		) {
		thetaElbow = M_PI - thetaShoulder - thetaElbow;
		// thetaElbow = thetaElbow - thetaShoulder - (M_PI/2.0);
		if (DEBUG) {
			Serial.print(" beta="); Serial.print(RADIAN2DEG(thetaElbow)); Serial.print(" ");
		}

		// convert angles to real angles
		thetaBaseServo = RADIAN2DEG(thetaBase) * baseCoefficient + baseOffset;
		if (thetaBaseServo < baseServoMin) thetaBaseServo = baseServoMin;
		if (baseServoMax < thetaBaseServo) thetaBaseServo = baseServoMax;

		thetaShoulderServo = RADIAN2DEG(thetaShoulder) * shoulderCoefficient + shoulderOffset;
		if (thetaShoulderServo < shoulderServoMin) thetaShoulderServo = shoulderServoMin;
		if (shoulderServoMax < thetaShoulderServo) thetaShoulderServo = shoulderServoMax;

		thetaElbowServo = RADIAN2DEG(thetaElbow) * elbowCoefficient + elbowOffset;
		if (thetaElbowServo < elbowServoMin) thetaElbowServo = elbowServoMin;
		if (elbowServoMax < thetaElbowServo) thetaElbowServo = elbowServoMax;
		return true;
	}

	bool goDirectlyTo(float x, float y, float z) {
		float thetaBase, thetaShoulder, thetaElbow;
		bool ok = solveInverseKinematics(x, y, z, thetaBase, thetaShoulder, thetaElbow);
		if (DEBUG) {
			Serial.print("goDirectlyTo "); Serial.print(ok); Serial.print(" ");
			Serial.print(RADIAN2DEG(thetaBase)); Serial.print(" ");
			Serial.print(RADIAN2DEG(thetaShoulder)); Serial.print(" ");
			Serial.print(RADIAN2DEG(thetaElbow)); Serial.print(" ");
		}
		if (ok) {
			convertIKAnglesToServoAngles(
				thetaBase, thetaShoulder, thetaElbow,
				thetaBase, thetaShoulder, thetaElbow
			);
			if (DEBUG) {
				Serial.print("converted: ");
				Serial.print(thetaBase); Serial.print(" ");
				Serial.print(thetaShoulder); Serial.print(" ");
				Serial.print(thetaElbow); Serial.print(" ");
			}
			base.write(thetaBase);
			shoulder.write(thetaShoulder);
			elbow.write(thetaElbow);
			currentX = x;
			currentY = y;
			currentZ = z;
			if (DEBUG) Serial.println("");
			return true;
		} else {
			if (DEBUG) Serial.println("");
			return false;
		}
	}

	bool gotoPoint(float x, float y, float z, float step=5, float wait=20) {
		float x0 = currentX; 
		float y0 = currentY; 
		float z0 = currentZ;
		float distance = sqrt((x0-x)*(x0-x)+(y0-y)*(y0-y)+(z0-z)*(z0-z));
		for (float i = 0; i < distance; i+= step) {
			bool ok = goDirectlyTo(x0 + (x-x0)*i/distance, y0 + (y-y0) * i/distance, z0 + (z-z0) * i/distance);
			if (!ok) return false;
			delay(wait);
		}
		bool ok = goDirectlyTo(x, y, z);
		if (!ok) return false;
		delay(wait);
		return true;
	}

	void openGripper() {
		moveGripper(M_PI/2);
	}

	void closeGripper() {
		moveGripper(0);
	}

	void moveGripper(float thetaClaw) {
		float thetaClawServo = RADIAN2DEG(thetaClaw) * clawCoefficient + clawOffset;
		if (thetaClawServo < clawServoMin) thetaClawServo = clawServoMin;
		if (clawServoMax < thetaClawServo) thetaClawServo = clawServoMax;
		claw.write(thetaClawServo);
	}
};

MeArm arm;

void setup() {
	Serial.begin(9600);
	Serial.println("start");

	if (0) {
		Serial.println("debug");
		Servo middle, left, right, claw ;  // creates 4 "servo objects"
		middle.attach(2);
		left.attach(3);
		right.attach(4);
		claw.attach(5);
		middle.write(90);
		left.write(90);
		right.write(90);
		claw.write(120);
		for (;;);

		uint16_t prev = 0;
		for (;;) {
			uint16_t read = static_cast<uint32_t>(analogRead(0)) * 180 / 512;
			if (read != prev) {
				Serial.print("read=");
				Serial.println(read);
				left.write(read);
				prev = read;
				delay(250);
			}
		}
		for (;;) {
			Serial.println("0");
			left.write(0);
			right.write(0);
			delay(2000);
			Serial.println("90");
			left.write(90);
			right.write(90);
			delay(2000);
			Serial.println("180");
			left.write(180);
			right.write(180);
			delay(2000);
		}
	}

	arm.begin(2, 4, 3, 5);
	arm.setServoCoefficient(
		0, 180, 1,  0,
		20, 170, -1, 170,
		20, 130, -1, 106,
		75, 115, -0.5, 115
	);
	arm.goDirectlyTo(0, 100, 50);
	// arm.openGripper();
}

void loop() {
	Serial.println("loop");
//	arm.gotoPoint(0,100,50);
//
//	arm.goDirectlyTo(0, 110, 60);  delay(1000);// b=90 s=90 e=30
//	arm.goDirectlyTo(100, 110, 60);  delay(1000);// b=90+45 s=100 e=40
//	arm.goDirectlyTo(100, 160, 60);  delay(1000);// b=90+45 s=120 e=45

	arm.openGripper();
	arm.gotoPoint(0,110,60); arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,110,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,160,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,160,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,160,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,110,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,110,60);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.closeGripper();
	delay(1000);

	arm.openGripper();
	arm.gotoPoint(0,110,20); arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,110,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,160,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,160,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,160,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,110,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,110,20);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.closeGripper();
	delay(1000);

	arm.openGripper();
	arm.gotoPoint(0,110,110); arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,110,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(100,160,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,160,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,160,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint(-100,110,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.gotoPoint( 0,110,110);  arm.openGripper(); delay(250);arm.closeGripper();  delay(250);
	arm.closeGripper();
	delay(1000);
}

ref.

• https://appliedgo.net/roboticarm/ golang での解説。わかりやすい。
• https://developer.mbed.org/users/eencae/code/MeArm/docs/tip/MeArm_8cpp_source.html