2017年7月5日の日記

MeArm っぽいロボットアームの制御

tech | 17:38 |

MeArm のパクりっぽいやつ(設計はオープンだからパクりとはいわない気はする)を AliExpress で買ってみました。https://shop.mime.co.uk/

ロボットアームは複数の関節の回転運動を同時に行って特定座標への移動を行います。よって、実座標系からサーボモータの制御回転角への変換処理が必要になります。

FK IK

Forward Kinematics
Inverse Kinematics

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

MeArm の構造と関節角度

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

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

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

ベース

ベースは簡単です。90°で正面を向くようにして、0〜180°で可動します

ショルダー (後ろからみて右)

ショルダーも簡単です。90°でアームが真上を向くようにして、30〜150° (後〜前) 程度で可動します。

エルボー (後ろからみて左)

これは若干ややこしいです。MeArm の構造の場合、ショルダーの動きによって肘関節も連動してしまうためです。

エルボー用サーボは90°でアームが後ろに向くようにして、0°でアームが真上、180°でアームが真下に向くようにしてあります。

このセットアップでショルダー用サーボが90°、エルボー用サーボが90°のとき、肘関節も90°になります。ショルダー用サーボの角度が変化すると、肘関節もその角度分だけ動くため、エルボー用サーボに与える角度はショルダー用サーボに与える角度をひいて求める必要があります。

実装

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

シチズン Q&Qのバンドを交換した

| 17:54 |

1000円の時計。3年前に買って、去年に電池を交換したけど、バンドがかなりヘタって、もうダメという感じだったので交換した。

[ランドン] 【バネ棒＋工具付】時計ベルト時計バンド本革カーフ 18mm 茶 - Randon(ランドン) — [ランドン] 【バネ棒＋工具付】時計ベルト時計バンド本革カーフ 18mm 茶 cho45

Randon(ランドン)

★ 4.0 / 5.0
cho45

780円。交換は簡単。今のところは問題ないみたい。これにより 1780円+オレの技術料の価値の時計となった。