Processing에서 구현해 보는 Robot Arm 소개

한 때 잠시 Processing이라는 언어로 재미난 일을 몇 개 한적이 있습니다. 처음 소개한 후 시리얼 통신으로 받은 데이터를 정~말 심플하게 그려보기 위해서 한 번 사용했구요[바로가기] 그 후 기구학 공부를 하다가 정말 심플하게 그려볼려고^^ 또 사용했지요[바로가기] 그게 2015년 1월이니 그 후 1년 반이나 들여다 보지 않았는데요. 제가 몇 번 만나서 알고 지내는 한 멋진 선생님[페이스북 바로가기]께서 어느날 문득 올려놓은 페이스북 글에서 꽤 재미난 동영상을 올려두셔서 마구마구 찾아보았답니다. 그랬더니 그 글에 나온 동영상의 원본글을 찾게 되었지요^^

그 글 제목은 Create a Robotics Simulator using Processing이라는 글[바로가기]입니다. Processing에서 로봇 팔을 3D 상에서 간결하게 묘사하는 것을 다루고 있더라구요 ... 우와~~~ 그래서 살짝 다시 Processing을 만났지요. 응? 못 본 사이에 뭔가 Processing도 멋지게 바뀌었더군요. ㅎㅎ.

흠... 디버그 모드라는 것까지 생기고 말이죠^^ 아무튼 아까 이야기한 원본 글[바로가기]을 따라가 보기로 했답니다.^^

float rotX, rotY;

void setup(){
    size(1200, 800, OPENGL);
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   fill(#FF9F03);
   noStroke();
   
   translate(width/2, height/2);
   rotateX(rotX);
   rotateY(-rotY); 
   box(300);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}

먼저 원본 글에서는 아주 기초적인 글 부터 학습을 시키네요... 참 좋은 글인듯 합니다. 아무튼~ 위 코드는 화면에 아주 심플한 Cube를 하나 생성시킵니다. 그건 box()라는 함수입니다. 이미 setup에서 3D라고 선언이 되었기 때문에 box(300)은 가로, 세로, 높이가 모두 300인 Cube가 됩니다. 그리고 mouseDragged() 함수에 의해 회전 각도가 변화하게 되도록 되어 있는 코드입니다. 이 코드를 실행하면~!~

이런 결과를 얻을 수 있습니다.^^

PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;
float alpha = -1, beta = -2, gamma;


void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("r5.obj");
    shoulder = loadShape("r1.obj");
    upArm = loadShape("r2.obj");
    loArm = loadShape("r3.obj");
    end = loadShape("r4.obj");
    
    shoulder.disableStyle();
    upArm.disableStyle();
    loArm.disableStyle(); 
}

void draw(){  
   background(32);
   smooth();
   lights();
   
   fill(#FFE308); 
   noStroke();
   
   translate(width/2,height/2);
   scale(-4);
   translate(0,-40,0);
   rotateX(rotX);
   rotateY(-rotY);    
     shape(base);
     
   translate(0, 4, 0);
   rotateY(gamma);
     shape(shoulder);
      
   translate(0, 25, 0);
   rotateY(PI);
   rotateX(alpha);
     shape(upArm);
      
   translate(0, 0, 50);
   rotateY(PI);
   rotateX(beta);
     shape(loArm);
      
   translate(0, 0, -50);
   rotateY(PI);
     shape(end);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}

이제 다음 단계로 위 코드를 보면 됩니다. 위 코드에서는 3D 도면을 PShape으로 선언한 변수에 loadShape으로 도면을 읽을 수 있습니다. 이를 적절히 옮기고(translate) 회전시켜(rotateX, rotateY) 배치를 해 둡니다. 그리고 나면

위와 같이 동작됨을 알 수 있습니다.^^. 위 프로젝트는 

demoRobotArmImport.zip

입니다.^^. 사실 여기까지는 손쉽게 되더라구요^^ 뭐 당연히 따라한 거니 뭐 당연하죠^^

이제 위에 보이는 그림과 같은 모습을 연출할 건데요. 저 로봇팔의 끝단의 움직임을 만들고, 각 관절은 역기구학을 풀도록 하게 될겁니다.

float F = 50;
float T = 70;
float millisOld, gTime, gSpeed = 4;

void IK(){
  float X = posX;
  float Y = posY;
  float Z = posZ;

  float L = sqrt(Y*Y+X*X);
  float dia = sqrt(Z*Z+L*L);

  alpha = PI/2-(atan2(L, Z)+acos((T*T-F*F-dia*dia)/(-2*F*dia)));
  beta = -PI+acos((dia*dia-T*T-F*F)/(-2*F*T));
  gamma = atan2(Y, X);
}

void setTime(){
  gTime += ((float)millis()/1000 - millisOld)*(gSpeed/4);
  if(gTime >= 4)  gTime = 0;  
  millisOld = (float)millis()/1000;
}

void writePos(){
  IK();
  setTime();
  posX = sin(gTime*PI/2)*20;
  posZ = sin(gTime*PI)*10;
}

먼저 위 코드에서 로봇 팔의 끝단의 움직임에 대한 지령이 writePos() 함수입니다. 거기서는 먼저 각 관절의 각도를 alpha, beta, gama로 두고 역구기학을 푸는 IK()함수와 시뮬레이션 시간을 조절하는 setTime()함수를 실행하고, 팔의 끝단이 무한대로 그리도록 posX와 posY 변수를 조절합니다. 역기구학 부분은 간단해 보이긴 하지만, 나중에 필요한 경우는 직접 다시 풀어서 적용하는게 좋을 겁니다.^^ 누가 구해 놓은걸 다시 순기구학으로 돌리는 건 꽤 귀찮아서 직접구하는게 더 빠를겁니다^^

PShape base, shoulder, upArm, loArm, end;
float rotX, rotY;
float posX=1, posY=50, posZ=50;
float alpha, beta, gamma;


float[] Xsphere = new float[99];
float[] Ysphere = new float[99];
float[] Zsphere = new float[99];

void setup(){
    size(1200, 800, OPENGL);
    
    base = loadShape("robot_parts/r5.obj");
    shoulder = loadShape("robot_parts/r1.obj");
    upArm = loadShape("robot_parts/r2.obj");
    loArm = loadShape("robot_parts/r3.obj");
    end = loadShape("robot_parts/r4.obj");
    
    shoulder.disableStyle();
    upArm.disableStyle();
    loArm.disableStyle(); 
}

void draw(){ 
   writePos();
   background(32);
   smooth();
   lights(); 
   directionalLight(51, 102, 126, -1, 0, 0);
    
    for (int i=0; i< Xsphere.length - 1; i++) {
    Xsphere[i] = Xsphere[i + 1];
    Ysphere[i] = Ysphere[i + 1];
    Zsphere[i] = Zsphere[i + 1];
    }
    
    Xsphere[Xsphere.length - 1] = posX;
    Ysphere[Ysphere.length - 1] = posY;
    Zsphere[Zsphere.length - 1] = posZ;
   
   noStroke();
   
   translate(width/2,height/2);
   rotateX(rotX);
   rotateY(-rotY);
   scale(-4);
   
   for (int i=0; i < Xsphere.length; i++) {
     pushMatrix();
     translate(-Ysphere[i], -Zsphere[i]-11, -Xsphere[i]);
     fill (#D003FF, 25);
     sphere (float(i) / 20);
     popMatrix();
    }
    
   fill(#FFE308);  
   translate(0,-40,0);   
     shape(base);
     
   translate(0, 4, 0);
   rotateY(gamma);
     shape(shoulder);
      
   translate(0, 25, 0);
   rotateY(PI);
   rotateX(alpha);
     shape(upArm);
      
   translate(0, 0, 50);
   rotateY(PI);
   rotateX(beta);
     shape(loArm);
      
   translate(0, 0, -50);
   rotateY(PI);
     shape(end);
}

void mouseDragged(){
    rotY -= (mouseX - pmouseX) * 0.01;
    rotX -= (mouseY - pmouseY) * 0.01;
}

그리고 나서 위 메인 코드를 사용하면 됩니다. 아까 기구학 푸는 코드의 함수를 이용해서 로봇팔을 그립니다.

demoRobotArm.zip

실제 구동영상은

인데요.. 꽤 심플하게 잘 구현된 듯 합니다. 확실히 Processing이 이러한 접근은 꽤 편한듯 해요~~~^^ 좋은 자료 알려주시진 박 선생님께도 감사를~~~^^...