[Robot Laboratory 4] 2. MFC UI Design
Updated:
로봇학실험4 (Robot Laboratory4) Project
해당 수업에서 배운 내용은 다음과 같다.
- ODE(Open dynamics Engine)를 통한 기구학 Simulation
- MFC UI 제작
- ATMega128과 Maxon 모터를 이용한 각종 모터 제어
해당 포스팅에서는 MFC 코드에 대한 설명을 할 것이다. ODE 관련 포스팅에서 언급한 것 처럼, 코드는 깃허브 Repository에 업로드를 해 놓았으니 아래 링크를 타고 들어가 참고하기 바란다.
→
Repository로 이동
MFC (Microsoft Foundation Class library)
쉽게 말하면 마이크로소프트 회사에서 만든 윈도우 UI 제작 라이브러리이다. 언어는 C++로 작성되며, 사실 UI 제작에 있어서는 MFC는 설정할 것이 너무 많고 세세하기에 사용하기 편한 Qt를 요즘에는 많이 사용한다고 한다. 그러나 MFC가 사용하기 귀찮은 만큼 사용자의 필요에 맞춰 제작이 가능하다는 장점이 있다. 어쨌든 해당 프로젝트에서는 ATMega128과 컴퓨터의 UART 통신 및 ODE를 움직이게 하기 위한 값을 보내는 용도로 MFC를 제작한다.
ODE 관련 포스팅 때처럼, MFC 코드를 열어보면 다음과 같은 cpp 파일 중에서 “RobotExp_4Dlg.cpp”에서 UI 코딩을 진행하게 된다.
UI 초기 설정
UI에 필요한 버튼이나 텍스트 창 등을 배치한 후, 다음과 같이 각각의 오브젝트에 대해 변수를 선언해 준다.
최종 UI 결과를 보면 다음과 같다.
MFC 코드 설명
1. Forward Kinematics 버튼 작동
해당 부분에 원하는 각도 값을 대입한 후에 [Forward] 버튼을 누르면 정기구학이 계산되어 [End Effector Position] 창에 좌표 값이 출력이 된다. 해당 부분의 코드를 보면 다음과 같다.
1
2
3
4
5
6
7
8
9
10
11
//// void CRobotExp_4Dlg::OnBnClickedButtonForward() ////
char cTmp[10]; // 문자열 저장할 배열 변수 선언
double dTmp[2]; // cTmp를 실수형으로 변환한 값을 저장할 변수 선언
editCur_button = 0; // 버튼모드 0: ODE에 대한 현재 값을 출력
m_editTarPos1.GetWindowText(cTmp, 10); // editTarPos1에서 각도 값을 입력받고 cTmp에 저장
dTmp[0] = atof(cTmp); // 문자열로 받은 cTmp를 실수형으로 변환하여 dTmp에 저장
m_editTarPos2.GetWindowText(cTmp, 10);
dTmp[1] = atof(cTmp); // ..위와 동일..
먼저 원하는 각도 값을 UI를 통해 입력하면 이를 문자열로써 읽어 들인 다음, double형으로 변환하여 다시 저장한다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//// void CRobotExp_4Dlg::OnBnClickedButtonForward() ////
DataType_t jointData;
// jointData라는 이름으로 DataType_t라는 자료형의 변수 선언
GET_SYSTEM_MEMORY("JointData", jointData);
// JointData라는 이름으로 저장되어 있는 값들을 jointData에 가져옴
jointData.Q_tar[0] = dTmp[0] * DEG2RAD;
// 입력받은 각도를 라디안으로 바꾼 후 jointData에 저장
jointData.Q_tar[1] = dTmp[1] * DEG2RAD;
SET_SYSTEM_MEMORY("JointData", jointData);
// 시스템 메모리에 "JointData"라는 이름으로 jointData 값들 저장
double dPos[3] = {0, 0, 0}; // 정기구학 풀이로 도출된 x, y, z좌표값을 저장할 배열 선언
SolveForwardKinematics(dTmp[0], dTmp[1], dPos);
// 입력받은 목표값 dTmp를 가지고 계산한 다음
// dPos에 계산 결과 x y z를 저장
그 다음, double형으로 변환한 각도 값을 shared memory에 저장을 한다. 이후 정기구학을 계산하는 함수에 parameters로 값을 넘긴다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
void CRobotExp_4Dlg::SolveForwardKinematics(double dAngle, double dAngle2, double* pdPos)
{
double q1 = dAngle * DEG2RAD - (90.0 * DEG2RAD);
// ODE상에서의 0도는 지면을 뚫고 아래를 향한 상태인데 일반 정기구학 관점에서는
// ODE상에서 90일 때가 0도이므로 90도의 위상 차이가 발생함. 이를 90도를 빼서 보상해줌
double q2 = dAngle2 * DEG2RAD;
double l1 = 1.0, l2 = 0.5;
pdPos[1] = (l1 * cos(q1)) + (l2 * cos(q1 + q2)); // y
// In matlab x = cos(theta1)/2 + (9*cos(theta1)*cos(theta2))/20 - (9*sin(theta1)*sin(theta2))/20
pdPos[2] = (l1 * sin(q1)) + (l2 * sin(q1 + q2)); // z
// In matlab y = sin(theta1)/2 + (9*cos(theta1)*sin(theta2))/20 + (9*cos(theta2)*sin(theta1))/20
pdPos[0] = 0; // x
}
정기구학을 계산하는 함수이다. 처음 UI에서 입력 받은 각도 값을 해당 함수로 가져 오는데, 잘 보면 $\theta_1$에 90도를 빼는 것을 볼 수 있다. 왜냐? 여기 ODE에서의 좌표는 일반적인 호도법을 따르기에 지평선에 수평인 방향으로 0도이다. 그러나 우리는 실제 모터를 돌릴 것을 생각하여 ODE상에서는 지면을 뚫고 아래로 내려가 있을 때가 0도인 것이다. 따라서 우리가 원하는 0도와, 기존 ODE상의 0도가 90도 위상 차이가 나므로, $\theta_1$에서 90도를 빼 주는 것이다. 게다가 해당 코드에서는 x, y가 아니라 y, z를 기준으로 코드를 작성하였다. 시뮬레이션을 처음 실행하면 모니터 방향으로 나오는 방향이 x이기에 y, z 좌표 기준으로 코드를 작성하는 것이 더 간편하므로 그렇게 진행했다.
정기구학 수식의 경우, 간단한 2DOF end effect의 좌표를 삼각함수로 계산하면 쉽게 구할 수 있다.
따라서 해당 값을 저장한 후, 출력을 하게 된다.
2. Inverse Kinematics 버튼 작동
정기구학 계산과는 반대로, 좌표 값을 입력하면, 그 위치로 가기 위해 필요한 각도 값을 계산하는 버튼이다. 코드를 보면 다음과 같다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
//// void CRobotExp_4Dlg::OnBnClickedButtonInverse() ////
char cTmp[10]; // 문자열 저장할 배열 변수 선언
double dTmp[3]; // cTmp를 실수형으로 변환한 값을 저장할 변수 선언
double r; // 원점에서부터 입력한 좌표까지의 직선거리를 저장할 변수 선언
m_editTarX.GetWindowText(cTmp, 10);
// m_editTarX에서 입력받은 값을 cTmp에 저장
dTmp[0] = atof(cTmp);
// cTmp에 저장된 값을 실수형으로 변환 후 dTmp에 저장
m_editTarY.GetWindowText(cTmp, 10);
dTmp[1] = atof(cTmp); // ..위와 동일..
m_editTarZ.GetWindowText(cTmp, 10);
dTmp[2] = atof(cTmp); // ..위와 동일..
정기구학 코드 부분과 마찬가지로, UI를 통해 좌표 값을 문자열로 입력 받고, double형으로 변환하여 다시 저장하게 된다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//// void CRobotExp_4Dlg::OnBnClickedButtonInverse() ////
// 예외처리 >> end effect가 도달할 수 없는 지점(특이점)으로 좌표 값을 입력하면 프로그램 뻑 나는 거 방지해주는 코드
r = sqrt(pow(dTmp[1], 2) + pow(dTmp[2], 2));
// 원점에서부터 주어진 좌표값까지의 직선거리 r 계산
if (!((0.5 <= r) && (r <= 1.5))) {
// 만약 r의 범위가 0.5 <= r <= 1.5 가 아니라면
dTmp[1] = 0;
dTmp[2] = -1.5;
// Singularity를 방지하기 위해 입력 좌표를 강제로 (0, -1.5)(즉 0도)로 초기화
m_EditRecv.SetWindowText("**Singularty Occured**");
// 추가로 현 입력 좌표가 singularity를 알리는 메세지 출력
}
else { m_EditRecv.SetWindowText(""); }
// 입력 좌표가 singularity가 아니면 다시 텍스트 clear
그런데 여기에서 한 가지 예외처리 코드가 추가되는데, 각도의 경우, 값이 아무리 커도 360으로 나눈 나머지 값을 사용하면 되지만, 좌표 값은 그렇지 않다. 링크의 길이 때문에 생기는 특이점이 존재하기에, 애초에 링크가 도달할 수 없는 곳을 입력 받았을 때의 처리를 해 주어야 한다. 우리는 그래서 특이점 좌표 값을 입력하면 강제로 0도 위치로 초기화를 시키고 특이점이라는 것을 텍스트를 띄워 사용자가 알 수 있게 알림 기능도 만들었다. 제대로 된 좌표 값을 입력하면 해당 텍스트는 다시 지워지도록 하였다.
1
2
3
4
5
6
7
//// void CRobotExp_4Dlg::OnBnClickedButtonInverse() ////
double dAngle[2] = { 0, 0 };
// 역기구학 풀이로 도출된 각도 값을 저장할 배열 선언
SolveInverseKinematics(dTmp[0], dTmp[1], dTmp[2], dAngle);
// 입력받은 목표값 dTmp를 가지고 계산한 다음
// dPos에 계산 결과 Pos1, Pos2(각도)를 저장
다음으로 이번에는 역기구학을 풀어주는 함수에 입력 받은 좌표 값을 넘겨주어 각도를 계산하도록 한다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
void CRobotExp_4Dlg::SolveInverseKinematics(double dX, double dY, double dZ, double* pdAngle)
{
double x = dX;
double y = dY;
double z = dZ;
double s2, c2;
double s1, c1;
double l1 = 1.0, l2 = 0.5;
c2 = (pow(y, 2) + pow(z, 2) - (pow(l1, 2) + pow(l2, 2))) / (2. * l1 * l2);
DataType_t ode_data;
GET_SYSTEM_MEMORY("JointData", ode_data);
// 링크의 up down을 결정하기 위해 theta2 값을 불러 옴
double theta2 = fmod((ode_data.Q_tar[1] * RAD2DEG), 360.0);
// 360도 이상일 때의 예외처리
if (theta2 < -180) theta2 += 360;
// theta2가 -180도 이상일 때의 예외처리
if(theta2 >= 0 && theta2 <= 180 )
// theta2의 부호 및 180도 이상일 때의 예외처리
s2 = (abs)(sqrt(1 - pow(c2, 2)));
else // theta2가 0보다 작을때
s2 = (double)-sqrt(1 - pow(c2, 2));
c1 = ((l1 + l2 * c2) * y + (l2 * s2 * z)) / (pow(l1 + l2 * c2, 2) + pow(l2 * s2, 2));
s1 = ((l1 + l2 * c2) * z - (l2 * s2 * y)) / (pow(l1 + l2 * c2, 2) + pow(l2 * s2, 2));
pdAngle[1] = /*(double)180 / pi * */ atan2(s2, c2);
// arctan이므로 라디안이 아닌 각도 값으로 계산됨
pdAngle[0] = /*(double)180 / pi * */ atan2(s1, c1) + (90.0 * DEG2RAD);
// 역기구학에서도 마찬가지로 위상 차이가 90도가 발생하기에 90도를 더해서 보상함
// 그런데 atan2 결과 값은 rad값이므로 90도 값을 rad로 변환 후 보상해 준다
}
해당 함수가 역기구학을 계산하는 함수이다. 역기구학 계산 함수는 정기구학 함수와 다르게 예외 처리가 조금 더 많다. 아무래도 2DOF 기준으로 해가 두 가지가 나오므로 $\theta_2$의 값에 따라 결과가 달라진다. 따라서 우리는 $\theta_2$가 음수인지 양수인지, 180도를 넘었는지, 360도를 넘었는지에 대한 예외 처리 코드를 작성하였다.
역기구학 수식은 2DOF 기준으로 다음의 식을 코드로 그대로 옮겨 적었다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
//// void CRobotExp_4Dlg::OnBnClickedButtonInverse() ////
char pszTmp[512]; // 문자열을 저장할 temp 변수 선언
sprintf_s(pszTmp, "%.4lf", dAngle[0] * RAD2DEG);
// 역기구학을 통해 계산된 Pos1 각도 값을 pszTmp에 저장
m_editTarPos1.SetWindowText(pszTmp);
// pszTmp에 저장된 문자열을 m_editTarPos1에 출력
sprintf_s(pszTmp, "%.4lf", dAngle[1] * RAD2DEG); // ..위와 동일..
m_editTarPos2.SetWindowText(pszTmp);
DataType_t jointData;
GET_SYSTEM_MEMORY("JointData", jointData);
jointData.Q_tar[0] = dAngle[0];
jointData.Q_tar[1] = dAngle[1];
SET_SYSTEM_MEMORY("JointData", jointData);
...
역기구학 함수를 통해 각도 값이 계산이 되면 이를 shared memory에 저장하고 각도 값을 UI에 출력을 한다.
3. 계산된 값을 토대로 ODE 움직이기
정/역기구학을 보면 서로 계산이 반대이지만, 코드를 보면 각도 값을 shared memory에 저장하는 것을 볼 수 있었다. 이는 바로 ODE에서 로봇을 움직이기 위한 값으로 쓰기 위해 저장을 하는 것이며, [ODE.cpp] 파일에 가면 시뮬레이션을 하는 함수가 있다. 해당 코드는 다음과 같다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
//// [ODE.cpp] void SimLoopDrawStuff(int pause) ////
DataType_t jointData;
ControlData_t graph;
bool connected;
GET_SYSTEM_MEMORY("JointData", jointData);
g_tar_q[0] = jointData.Q_tar[0];
g_tar_q[1] = jointData.Q_tar[1];
jointData.Q_cur[0] = g_cur_q[0];
jointData.Q_cur[1] = g_cur_q[1];
SET_SYSTEM_MEMORY("JointData", jointData);
GET_SYSTEM_MEMORY("connect", connected);
GET_SYSTEM_MEMORY("graph", graph);
...
사실 해당 부분에도 예외 처리가 상당히 들어가 있다. 일단 정/역기구학 버튼 함수에서 저장한 각도 값을 여기 함수로 불러 온다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//// [ODE.cpp] void SimLoopDrawStuff(int pause) ////
...
if (connected)
g_tar_q[0] = graph.position;
// 첫 번째 관절에 대한 예외처리 코드
if (g_tar_q[0] >= 360.0 * DEG2RAD) g_tar_q[0] = fmod(g_tar_q[0], 360.0 * DEG2RAD);
if (g_tar_q[0] <= -360.0 * DEG2RAD) g_tar_q[0] = fmod(g_tar_q[0], 360.0 * DEG2RAD);
// 두 번째 관절에 대한 예외처리 코드
if (g_tar_q[1] >= 360.0 * DEG2RAD) g_tar_q[1] = fmod(g_tar_q[1], 360.0 * DEG2RAD);
if (g_tar_q[1] <= -360.0 * DEG2RAD) g_tar_q[1] = fmod(g_tar_q[1], 360.0 * DEG2RAD);
...
다음 코드를 보면, 일단 각 관절들이 -360도~+360도에서 움직일 수 있도록 하는 예외 처리 코드가 있다. 절대값 360도를 넘기면 360을 나눈 나머지 값으로 변경을 하는 것이다.
위 if문으로 두 줄이 적혀 있는데, 우리는 ODE 상에서 정/역기구학을 풀면, 도출된 값을 따라 로봇이 움직이게끔 했다. 그런데 실제 모터를 돌릴 때에는 현재 모터의 움직임을 똑같이 따라가야 하기에, 용도가 겹쳐 버린다. 따라서 이를 구분할 필요가 있다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
//// [RobotExp_4Dlg.cpp] void CRobotExp_4Dlg::OnBnClickedCheckOpen() ////
...
// PORT 열기 시도
if (((CCommWork*)_commWorker.GetWork()) -> OpenPort(port.GetBuffer(), nTmp))
{
bool connected;
GET_SYSTEM_MEMORY("connect", connected);
connected = true;
SET_SYSTEM_MEMORY("connect", connected);
// 성공하면 Thread를 만들고 버튼의 텍스트를 Close로 변경
_commWorker.StartWork();
m_CheckOpen.SetWindowText("Close");
DataType_t ode_data;
ControlData_t motor_data;
GET_SYSTEM_MEMORY("JointData", ode_data);
ode_data.Q_tar[0] = ode_data.Q_tar[1] = 0.;
SET_SYSTEM_MEMORY("JointData", ode_data);
...
해당 코드는 [RobotExp_4Dlg.cpp] 문서에서 ATMega128과 USART 통신을 위해 PORT를 열어주는 Open 버튼 함수 중 일부이다. PORT가 열릴 때 connected 변수를 true로 바꾸는데, 이 connected 변수도 shared memory에 선언되어 있는 변수이므로 해당 변수를 다시 [ODE.cpp] 문서로 가져온다.
1
2
3
4
5
6
7
8
//// [ODE.cpp] void SimLoopDrawStuff(int pause) ////
...
if (connected)
g_tar_q[0] = graph.position;
...
따라서 해당 if문이 true라는 의미는 즉, 모터(ATMega128)와 연결이 되었다는 의미로, 모터와 연결된 순간 ODE의 로봇의 움직임에 관여하는 값은 정/역기구학 버튼 연산 값이 아닌 실시간으로 모터(ATMega128)에서 보내주는 shaft의 각도 값으로 바뀌게 된다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//// [ODE.cpp] void SimLoopDrawStuff(int pause) ////
...
PControl(); // 제어기 선언
//루프에 바디를 그리는 함수를 집어넣는 과정: 각 물체마다 설정해 주어야 함
dReal dR, dLength;
// dSpaceCollide(g_Space, 0, &SpaceCollide); // 지면과의 충돌 고려 ON
dsSetColor(1, 0, 0); // 컬러 설정 ( RGB 255를 100%(1)로 보고 설정 )
dGeomCapsuleGetParams(g_oObj[0].geom, &dR, &dLength); // 강체 생성: 캡슐 형태
// Parameter로 캡슐 정보, 반경, 길이 정보를 넘김
dsDrawCapsuleD(dBodyGetPosition(g_oObj[0].body), // 디자인된 강체를 작업하는 월드에 생성
dBodyGetRotation(g_oObj[0].body), (float)dLength, (float)dR); // ㄴ> 월드에 해당 강체를 그리기 위해
// 강체에 대한 정보들을 Parameter로 넘김
dsSetColor(0, 1, 0);
dGeomCapsuleGetParams(g_oObj[1].geom, &dR, &dLength);
dsDrawCapsuleD(dBodyGetPosition(g_oObj[1].body),
dBodyGetRotation(g_oObj[1].body), (float)dLength, (float)dR);
dsSetColor(0, 0, 1);
dGeomCapsuleGetParams(g_oObj[2].geom, &dR, &dLength);
dsDrawCapsuleD(dBodyGetPosition(g_oObj[2].body),
dBodyGetRotation(g_oObj[2].body), (float)dLength, (float)dR);
double dt = 0.01;
dWorldStep(g_World, dt);
// dJointGroupEmpty(g_Contactgroup);
또한 제어기를 선언을 한 것을 볼 수 있는데, 이는 ODE 상에서 로봇이 프레임 끊기듯이 움직이는 것이 아닌 부드럽게 움직이도록 하는 제어기이다. 단순 P 제어기로 이루어져 있다.
마지막으로 각 링크에 대한 정보들을 종합하여 해당 함수에서 그리게 된다.
4. 현재 값을 UI 상에 띄우기
UI를 보면 사용자가 직접 입력할 수 있는 칸이 있는 반면, 현재 시스템의 위치를 표시만 하는 칸이 있다. 그런데 이 또한 정기구학 계산 결과를 띄우는 경우와, 모터의 현재 값들을 띄우는 경우 둘 다 사용되기에, 둘 다 코드로 단순히 작성하면 모터의 현재 값들을 띄우는 경우가 무시되어 모터를 돌릴 때 현재 값이 표시가 되지 않는 문제가 있었다. 따라서 우리는 이러한 경우 mode 변수를 사용해서 예외 처리를 하였다.
정기구학 버튼 함수를 보면 “editCur_button” 이라는 변수가 있다.
1
2
3
4
5
6
7
8
9
10
11
12
//// [RobotExp_4Dlg.cpp] void CRobotExp_4Dlg::OnBnClickedButtonForward()
////
...
char cTmp[10]; // 문자열 저장할 배열 변수 선언
double dTmp[2]; // cTmp를 실수형으로 변환한 값을 저장할 변수 선언
editCur_button = 0; // 버튼모드 0: ODE에 대한 현재 값을 출력
...
그리고 set 버튼 함수를 보면 마찬가지로 존재하는 것을 볼 수 있다.
1
2
3
4
5
6
7
8
9
10
11
//// [RobotExp_4Dlg.cpp] void CRobotExp_4Dlg::OnBnClickedButtonSet()
////
ControlData_t motor_data;
DataType_t ode_data;
editCur_button = 1;
// 버튼모드 1: 실제 모터에 대한 현재 값을 출력
...
우리는 Set 버튼은 모터(ATMega128)에 값을 주는 통신 버튼이므로 ODE의 정기구학을 푸는 것과는 관련이 없다. 그래서 각 버튼을 누를 때마다 editCur_button의 값이 바뀌게 해 놓는다. 그러고 이러한 현재 값들을 UI에 띄워주는 OnTimer 함수를 보면 다음과 같이 코드를 작성해 놓았다.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
//// [RobotExp_4Dlg.cpp] void CRobotExp_4Dlg::OnTimer(UINT_PTR nIDEvent)
////
void CRobotExp_4Dlg::OnTimer(UINT_PTR nIDEvent)
{
ControlData_t motor_data;
DataType_t ode_data;
GET_SYSTEM_MEMORY("JointData", ode_data);
// 시스템 메모리에 저장되어 있는 ODE의 현재 위치에 대한 정보
GET_SYSTEM_MEMORY("Comm1Work_Controller_Current", motor_data);
// 시스템 메모리에 저장되어 있던 ATMega128 패킷 데이터를 불러옴
CString str; // 문자열 선언
str.Format("%.4f", ode_data.Q_cur[1] * RAD2DEG);
m_editCurPos2.SetWindowText(str);
if(editCur_button == 0) str.Format("%.4f", ode_data.Q_cur[0] * RAD2DEG);
else if(editCur_button == 1) str.Format("%.4f", motor_data.position * RAD2DEG);
m_editCurPos1.SetWindowText(str);
str.Format("%.4f", motor_data.velocity * RAD2DEG);
m_editCurVel.SetWindowText(str);
str.Format("%.4f", motor_data.current * 0.0683); // Torque constant
m_editCurToq.SetWindowText(str);
...
}
실제 모터의 현재 위치 값과 ODE 상의 링크 1의 현재 값 둘 다 표시하기에 서로 충돌이 발생한다. 따라서 Set버튼을 누르면 ATMega128을 통해 받은 데이터를 shared memory를 통해 가져와 출력을 하고, Forward 버튼을 누르면 Forward 버튼 함수에서 shared memory에 저장한 각도 값을 가져와 출력을 하도록 작성하였다.
Leave a comment