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.

  1. トップ
  2. tech
  3. MeArm っぽいロボットアームの制御

[シチズン キューアンドキュー]CITIZEN Q&Q 腕時計 Falcon (フォルコン) アナログ表示 ホワイト V722-850 メンズ -

5.0 / 5.0

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

【バネ棒+工具付】 時計ベルト 時計バンド 本革 カーフ 18mm 茶 【Randon】 -

4.0 / 5.0

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

鏡を使うと外から瞳孔を見ることはできるけど、内側は解剖しないと普通は見れない。しかしピンホールを使うと直接自分の瞳孔を観察できた。

T=0.05mm のステンレス板に φ0.1mm の穴をあけてみたところ (ピンホール)、これに目をできるだけ近づけてそこそこ強い光源を見ると、自分の瞳孔を見ることができることに気がついた。

簡単に再現する方法

ある程度小さい穴なら以下のような現象が再現できるので、手でピンホールを作って室内蛍光灯とかを見るだけでよかった。

こんな感じにして穴を片目にあてる。単に穴を通して向こう側が見えるだけに思えるが、もう片方の眼にあたる光を手で遮ったりすると瞳孔の動きがわかる。

歪な形の穴を通した場合、光量によって歪な形のまま大きさが変化する。いまいちなんで瞳孔の動きが観察できるのかわからない。

まだら模様の円内と、ぎざぎざした円縁

一定の光源のはずの円内はまだら模様にみえる。

その周辺にはぎざぎざした縁の円があって、急激に暗くなる。

なぜこれが瞳孔だと確認できるのか

片方の眼を遮光した状態で、もう片方の眼にピンホールをあてると大きな円が見える。この状態のまま片目の遮光を外すと急激に円が収縮する。もう一度遮光しなおすとゆるやかに円が拡大する。

よくみると静止していても円は拡大したり収縮したりする。

光源と網膜の間でこのような動きをするのは瞳孔だけなので、これは瞳孔だと思う。

瞳孔内のまだら模様は何なのか

まだら模様は眼球を回転させても移動しない。瞳孔の大きさも関係ない。常に一定の模様が見える。

角膜・水晶体・硝子体のうちのどれかが関係してそうだけどわからない。ちなみに角膜前についた埃も見えるのだけど、まだら模様よりすこし遅れて動くことから、たぶん角膜表面よりは瞳孔に近そう。

なぜ眼の外のピンホールで瞳孔が見えるのか

よくわからない。ピンホールを近づけすぎると、ピンホールが作る像が瞳孔でケラれてしまうからかな? だとすると瞳孔が見えているというよりは瞳孔の影が見えているといえる。

  1. トップ
  2. tech
  3. 自分の瞳孔を内側から見る方法を発見してしまった
  1. トップ
  2. photo
  3. 自分の瞳孔を内側から見る方法を発見してしまった