Processing에서 Two Link Planar를 정방향 기구학으로 해석한 시뮬레이션

얼마전에 제가 즐겨보는 예능의 PD인 나영석 피디의 인터뷰 중 이런 이야기가 있더군요. 

“그냥, 정당한 인간적 대우를 해주면 된다. 70~80명의 스태프가 거기 있는 이유는 각자 하나하나 소중한 역할이 있어서다. 예를 들어 배차 담당을 하는 친구가 있는데 그 일이 사실 도드라지진 않지만 잘 안되면 욕은 욕대로 무지하게 먹는 자리다. 그럼 그 친구한테 권한을 주고 ‘네가 책임을 지고 해줘’ 맡기고 ‘고맙다, 수고했다, 너니까 했다’ 이런 얘길 하는 게 내가 할 수 있는 다다. 그렇게만 해도 ‘아, 피디님한테 칭찬받았어’가 아니라 ‘나도 1박2일을 위해 뭔가를 하고 있어’가 되는 거다. 그런 주인의식을 갖는 게 진짜 하늘과 땅 차이다. 그런 사소한 차이가 100%를 채운다고 믿는다.”

-. 출연자 뿐만 아니라 스태프까지 자발적 참여를 이끌어내는 비결을 묻는 질문에 대한 대답.-

출처 : 한겨례 신문 [바로가기]

어쩐지 요즘 삼시세끼가 재미있더라구요..ㅎㅎㅎ. 편안하면서 말이죠...^^

아무튼 오늘은 지난번에 이야기한 기구학의 아주아주 기초인 정방향(forward kinematics) 기구학의 기초를 다루면서 예제로 이야기[바로가기]한 Two Link Planar를 다룰려고 합니다.

그때도 이야기했지만... 이 그림은 제가 그린것이 아니며 출처를 잃어버렸습니다.ㅠㅠ.

당시엔 저걸 정방향 기구학에 대한 예제로 하나 다루면서 MATLAB을 사용했는데요. 아무래도 좀 심심했죠? 그래서... 이번에는 Processing으로 구현해볼까 하는 겁니다.^^.

float[][] RotM(char axis, float theta) {
	if (axis=='a') {
		return new float[][]{{cos(theta),-sin(theta), 0,0},{sin(theta),cos(theta), 0,0},{0,0,1,0},{0,0,0,1}};
	} else if (axis=='o') {
		return new float[][]{{cos(theta),0,sin(theta),0},{0,1,0,0},{-sin(theta),0,cos(theta),0},{0,0,0,1}};
	} else {
		return new float[][]{{1,0,0,0},{0,cos(theta),-sin(theta),0},{0,sin(theta),cos(theta),0},{0,0,0,1}};
	}
}

float[][] TransM(float x, float y, float z) {
	return new float[][]{{1,0,0,x},{0,1,0,y},{0,0,1,z},{0,0,0,1}};
}

먼저 회전행렬과 이동행렬을 구현합니다.

위 행렬들이 이전[바로가기]에 이야기했던 이동행렬과, 회전행렬들인데요. 위 코드는 프로그램으로 표현한것일 뿐이죠. 그리고, 마우스를 이용해서 1번과 2번 링크를 회전하도록 할 겁니다. 그렇게 구현한 부분이...

void drawPanel() {
	fill(50);
	textFont(titleFont);
	text("Kinematics Example of Two Link Planar",140,25);
	textFont(smallFont);
	text("by PinkWink",500,45);

	stroke(150);
	line(50,550,280,550);
	line(320,550,550,550);

	if (grabSlider==1) {
		theta1 = (float(mouseX)-(50+280)/2)/115*PI;
	}

	if (grabSlider==2) {
		theta2 = (float(mouseX)-(320+550)/2)/115*PI;
	}

	slidePos1 = theta1*115/PI + (50+280)/2;
	slidePos2 = theta2*115/PI + (320+550)/2;

	stroke(100);
	fill(255);
	if (grabSlider==1) {fill(200);}
	ellipse(slidePos1,550,10,10);
	fill(255);
	if (grabSlider==2) {fill(200);}
	ellipse(slidePos2,550,10,10);
}

void mousePressed() {
	if (mouseButton==LEFT) {
		if (abs(mouseX-(50+280)/2)<165 && abs(mouseY-550)<5) { grabSlider = 1; }
		if (abs(mouseX-(320+550)/2)<165 && abs(mouseY-550)<5) { grabSlider = 2; }
	}
}

void mouseReleased() {
	if (mouseButton==LEFT) { grabSlider = 0; }
}

이부분은 사실 제가 이전에 시리얼 통신의 데이터를 그래프로 표현하기라는 글[바로가기]에서도 사용한 방법이지요. 마우스가 눌러진 위치가 내가 버튼(혹은 슬라이더)라고 생각한 범위인지 판별하는 거지요. 뭐 GUI 툴박스를 사용하는 것도 좋지만.. 그냥 Processing 답게 이렇게 사용하는 것이 더 편하답니다.^^

그리고, 링크1에 대한 변환

과.. 링크2에 대한 변환

도 이전에 이야기했으니. 그냥 가져다 쓰기만 하면 되겠죠~~~. 여기서 위 변환행렬을 적용한 부분이...

	float[][] T_0_1 = Mat.multiply(RotM('a', theta1), TransM(a1,0,0));
	float[][] T_1_2 = Mat.multiply(RotM('a', theta2), TransM(a2,0,0));
	float[][] T_total = Mat.multiply(T_0_1, T_1_2);

	float[][] y0_1 = Mat.multiply(T_0_1, y0);
	float[][] y0_2 = Mat.multiply(T_total, y0);

	float[] originX = {y0[0][3], y0_1[0][3], y0_2[0][3]};
	float[] originY = {y0[1][3], y0_1[1][3], y0_2[1][3]};

참... 위 코드에서 Mat이라고 된 행렬 연산 기능은 지난번에 소개했던 Papaya 라이브러리[바로가기]를 사용하고 있습니다. 사실 기구학중에서 쉬운 정방향 기구학은 저렇게 변환행렬이 단지 행렬의 곱셈으로 나타나기 때문에 아주 쉽지요^^.

위 그림이 시뮬레이션 결과 입니다. 아래에 있는 슬라이드 바를 움직이면 각각 1번 링크와 1번 링크가 움직입니다. 그냥 이렇게 가면 좀 허전한듯하니 동영상(으로 볼것도 없지만)으로 한번 보시죠^

이미 GitHub[바로가기]에도 올려두어서 다운로드 하시기 편하실 테지만,  그래도 전체 코드는

import papaya.*;

float theta1 = 0;
float theta2 = 0;
float a1 = 100;
float a2 = 100;

float centerPos = 300;
float sizeOfAxis = 10;

int grabSlider = 0;
float slidePos1, slidePos2;

float[][] y0 = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
float[][] pol0 = {{10,20,10,10},{10,10,20,10},{0,0,0,0},{1,1,1,1}};

PFont titleFont = createFont("Helvetica",20,true);
PFont smallFont = createFont("Helvetica",12,true);
PFont tinyFont = createFont("Helvetica",9,true);

void setup() {
	size(600,600);
}

void draw() {
	background(255);

	drawPanel();

	float[][] T_0_1 = Mat.multiply(RotM('a', theta1), TransM(a1,0,0));
	float[][] T_1_2 = Mat.multiply(RotM('a', theta2), TransM(a2,0,0));
	float[][] T_total = Mat.multiply(T_0_1, T_1_2);

	float[][] y0_1 = Mat.multiply(T_0_1, y0);
	float[][] y0_2 = Mat.multiply(T_total, y0);

	float[] originX = {y0[0][3], y0_1[0][3], y0_2[0][3]};
	float[] originY = {y0[1][3], y0_1[1][3], y0_2[1][3]};

	noFill();
	stroke(200);
	ellipse(centerPos + y0[0][3], centerPos - y0[1][3], a1*2, a1*2);
	ellipse(centerPos + y0_1[0][3], centerPos - y0_1[1][3], a2*2, a2*2);

	strokeWeight(2);
	stroke(color(#8E8E8E));
	line(centerPos + originX[0], centerPos - originY[0], centerPos + originX[1], centerPos - originY[1]);
	line(centerPos + originX[1], centerPos - originY[1], centerPos + originX[2], centerPos - originY[2]);

	drawBodyAxis(y0, centerPos, sizeOfAxis);
	drawBodyAxis(y0_1, centerPos, sizeOfAxis);
	drawBodyAxis(y0_2, centerPos, sizeOfAxis);

	fill(50);
	textFont(tinyFont);
	text(nfs(theta1*180/PI,0,2), centerPos + originX[0], centerPos - originY[0] + 20);
	text(nfs(theta2*180/PI,0,2), centerPos + originX[1], centerPos - originY[1] + 20);
	String endPos = "( "+nfs(originX[2],0,2)+", "+nfs(originY[2],0,2)+" )";
	text(endPos, centerPos + originX[2]+20, centerPos - originY[2]+20);

	float[][] pol0 = {{10,20,10},{10,10,20},{0,0,0},{1,1,1}};
	float[][] pol0_1 = Mat.multiply(T_0_1, pol0);
	float[][] pol0_2 = Mat.multiply(T_total, pol0);

	drawObject(pol0, centerPos);
	drawObject(pol0_1, centerPos);
	drawObject(pol0_2, centerPos);
}

float[][] RotM(char axis, float theta) {
	if (axis=='a') {
		return new float[][]{{cos(theta),-sin(theta), 0,0},{sin(theta),cos(theta), 0,0},{0,0,1,0},{0,0,0,1}};
	} else if (axis=='o') {
		return new float[][]{{cos(theta),0,sin(theta),0},{0,1,0,0},{-sin(theta),0,cos(theta),0},{0,0,0,1}};
	} else {
		return new float[][]{{1,0,0,0},{0,cos(theta),-sin(theta),0},{0,sin(theta),cos(theta),0},{0,0,0,1}};
	}
}

float[][] TransM(float x, float y, float z) {
	return new float[][]{{1,0,0,x},{0,1,0,y},{0,0,1,z},{0,0,0,1}};
}

void drawObject(float[][] obTarget, float centerPos) {
	stroke(color(#000000));
	noFill();
	beginShape();
		vertex(centerPos + obTarget[0][0], centerPos - obTarget[1][0]);
		vertex(centerPos + obTarget[0][1], centerPos - obTarget[1][1]);
		vertex(centerPos + obTarget[0][2], centerPos - obTarget[1][2]);
	endShape(CLOSE);
}

void drawBodyAxis(float[][] bodyMat, float centerPos, float sizeOfAxis) {
	stroke(color(#FF0000));
	line(centerPos + bodyMat[0][3], centerPos - bodyMat[1][3],
		centerPos + bodyMat[0][3] + bodyMat[0][0]*sizeOfAxis,
		centerPos - (bodyMat[1][3] + bodyMat[1][0]*sizeOfAxis));

	stroke(color(#0017FF));
	line(centerPos + bodyMat[0][3], centerPos - bodyMat[1][3],
		centerPos + bodyMat[0][3] + bodyMat[0][1]*sizeOfAxis,
		centerPos - (bodyMat[1][3] + bodyMat[1][1]*sizeOfAxis));	
}

void drawPanel() {
	fill(50);
	textFont(titleFont);
	text("Kinematics Example of Two Link Planar",140,25);
	textFont(smallFont);
	text("by PinkWink",500,45);

	stroke(150);
	line(50,550,280,550);
	line(320,550,550,550);

	if (grabSlider==1) {
		theta1 = (float(mouseX)-(50+280)/2)/115*PI;
	}

	if (grabSlider==2) {
		theta2 = (float(mouseX)-(320+550)/2)/115*PI;
	}

	slidePos1 = theta1*115/PI + (50+280)/2;
	slidePos2 = theta2*115/PI + (320+550)/2;

	stroke(100);
	fill(255);
	if (grabSlider==1) {fill(200);}
	ellipse(slidePos1,550,10,10);
	fill(255);
	if (grabSlider==2) {fill(200);}
	ellipse(slidePos2,550,10,10);
}

void mousePressed() {
	if (mouseButton==LEFT) {
		if (abs(mouseX-(50+280)/2)<165 && abs(mouseY-550)<5) { grabSlider = 1; }
		if (abs(mouseX-(320+550)/2)<165 && abs(mouseY-550)<5) { grabSlider = 2; }
	}
}

void mouseReleased() {
	if (mouseButton==LEFT) { grabSlider = 0; }
}

입니다. 항상 하는 생각이지만, 요즘은 참 결과를 확인하는 일이 점점 쉬워지는 것 같습니다. 얼마전에 끝난 국가 직무 능력 개발인 NCS 개발에 저도 미력하지만 참여 개발진이었는데요. 그 때 한분이 말씀하신게 기억나네요. 이제 시스템 아키텍쳐만 잘 설계하면 그게 결과로 나오는 툴이 개발될려는게 아닐까?? 하는 말이죠. 응? 요딴걸 코드랍시고 공개하고서는 그런 거창한 이야기를 하다니... 하시겠지만 말이죠^^. 뭐 아무튼 이 글은 아마 금요일에 예약 발행될테니.. 즐거운 불타는 금요일 되세용~~~^^