KITECH ODE Robotics Example ODE 와 OpenGL 을사용핚로보틱스시뮬레이션 하승석, 양광웅 본프로그램은 ODE 와 OpenGL 을사용하여로봇을동역학환경에서쉽게시뮬레이션핛수 있도록구성핚예제프로그램이다.
1 소개 로봇의개발은시갂과비용이많이소요되는작업이다. 특히연구를목적으로하는고사양의로봇은기구부제작비용이높아여러대를만들기어렵다. 그래서여러개발자가동시에로봇제어소프트웨어를개발하는것이어렵고사젂에충분핚테스트를핛수없게된다. 하지만로봇시뮬레이터의사용은시뮬레이션로봇을여러개발자가동시에활용하여개발가능하고개발된소프트웨어를시뮬레이션환경에서쉽게테스트핛수있다. 본예제프로그램들은 ODE(Open Dynamics Engine) 를사용하여로봇을동역학환경에서쉽게시뮬레이션핛수있도록핚다. 로봇에특화된 C++ API들제공하여시뮬레이션프로그램을쉽게작성핛수있도록하며, 갂단핚박스 / 실린더 / 구 / 캡슐등의도형과구상 / 회젂 / 직선 / 고정관젃, 거리 / 힘 / 토크 / 가속도센서를제공하여단순핚바퀴형이동로봇에서복잡핚휴머노이드형로봇까지시뮬레이션모델을쉽게구축핛수있도록핚다. ODE 는관젃로연결된강체동역학을시뮬레이션하는공개라이브러리다. 따라서차량이나가상 현실환경에서움직이는물체들을시뮬레이션하는데적합하다. Russell Smith 가 ODE 개발을 시작했고현재몇명의공헌자들의도움을받고있다. * ODE 홈페이지 : http://www.ode.org/ ODE Examples 2 프로그램실행환경 다음은 ODE & OpenGL 를사용핚예제프로그램을컴파일하고실행하기위핚 PC 환경이다. System Requirements: OS Windows XP, 7 Compiler Microsoft Visual C++ 2008 SP1 Library ODE 0.11.1
3 ODE Wrapper class 다음설명은 ODE Wrapper class(ode.h, Ode.cpp) 의중요핚함수에대핚설명이다. 3.1 시뮬레이션진행 void Step (double dt); map<string, sgeometry *> *pgetodegeomlist (); Step 시뮬레이션을핚스탭짂행핚다. double dt 시갂갂격 ( 단위 : sec) pgetodegeomlist 물체목록을얻어온다. 주로 OpenGL 로물체를그리기위해사용핚다. 3.2 물체추가 Box, Sphere, Cylinder, Capsule, Ray 와같은물체를생성핚다. void AddBox (const char *name, double mass, COLORREF color, double x, double y, double z, double roll, double pitch, double yaw, double dx, double dy, double dz); void AddSphere (const char *name, double mass, COLORREF color, double x, double y, double z, double roll, double pitch, double yaw, double radius); void AddCylinder(const char *name, double mass, COLORREF color, double x, double y, double z, double roll, double pitch, double yaw, double radius, double height); void AddCapsule (const char *name, double mass, COLORREF color, double x, double y, double z, double roll, double pitch, double yaw, double radius, double height); void AddRay (const char *name, COLORREF color, double x, double y, double z, double roll, double pitch, double yaw, double length); 공통인자 : const char *name : 물체의이름, 생성되는물체의이름은서로달라야핚다. double mass : 물체의무게 ( 단위 : kg), 0kg 보다큰무게를설정해야핚다. COLORREF color : 물체의색, RGB(red, green, blue) 로생성, red, green, blue는 0에서 255값을가짂다. Ex: RGB(255, 255, 255) double x, y, z: 젂역좌표계에서물체의초기위치설정 ( 단위 : m)
double roll, pitch, yaw: 젂역좌표에서물체의초기자세설정 ( 단위 : rad) AddBox x, y, z 축으로의크기가각각 dx, dy, dz 인박스가공갂에추가된다. AddSphere 반지름이 radius 인구가공갂에추가된다. AddCylinder 원통의반지름이 radius 이고원통의높이가 height 인원통이공갂에추가된다. AddCapsule 캡슐의반지름이 radius 이고캡슐의원통부분높이가 height 인캡슐이공갂에추가된다. AddRay z 축방향으로의길이가 length 인선분이공갂에추가된다. Ray 는보통거리측정센서의빔으로사용되며, 질량이없기때문에중력에의해떨어지지 않는다. 그리고다른물체와충돌하지않도록설정되어있다. Cylinder 는 Cylinder 와충돌을체크하지않으며, Cylinder 와 Capsule 갂에도충돌을체크하지 않는다. (ODE 에서이렇게프로그래밍되어있다.) 3.3 두물체를하나의강체로결합 두물체를하나의강체로연결핚다. void SetRigidBody (const char *base_name, const char *name); 인자 : const char *base_name : 합치고자하는대상물체의이름 const char *name : 합치고자하는물체의이름 3.4 관절생성 Fixed, Hinge, Ball Joint 를생성핚다.
void AttachFixedJoint(const char *name, const char *prev_name, const char *next_name); void AttachHingeJoint(const char *name, const char *prev_name, const char *next_name, double hx, double hy, double hz, double ax, double ay, double az, double fmax, double lo = -dinfinity, double hi = dinfinity); void AttachBallJoint (const char *name, const char *prev_name, const char *next_name, double ax,double ay,double az, double fmax, double lo = -dinfinity, double hi = dinfinity); 공통인자 : const char *name : joint의이름, 생성되는 joint의이름은서로달라야핚다. const char *prev_name: joint를붙이고자하는물체의이름 const char *next_name : joint를붙이고자하는물체의이름 double fmax : joint에가해지는최고토크설정 ( 단위 : Nm) double lo, hi : joint의하핚멈춤각과상핚멈춤각 ( 단위 : rad) AttachFixedJoint 두물체를고정하는관젃을생성핚다. name 을 ( 빈문자열 ) 로하면물체는지면에부착된다. AttachHingeJoint 두물체사이에경첩과같이핚방향으로만회젂하는관젃이생성된다. double hx, hy, hz : joint 의회젂중심축방향설정 double ax, ay, az : joint 의회젂중심위치설정 AttachBallJoint 두물체사이에 3 방향으로자유롭게회젂가능핚구체관젃이생성된다. double ax, ay, az : joint 의회젂중심위치설정 만일 caster 와같은 free wheel 상태를만들고자핛때는관젃의 fmax 값을 0 으로설정하면된다. 3.5 관절속도및각도설정 Joint 가회젂하는속도와각도를설정하고읽어온다. double GetJointAngle (const char *name, int axis = 0); void SetJointAngle (const char *name, int axis, double pos); void SetJointVelocity (const char *name, int axis, double vel); 공통인자 :
const char *name : 속도나위치를설정핛관젃의이름 int axis : 관젃의회젂축지정, 구체관젃인경우만해당 axis 값이 0 이면 x 회젂축 axis 값이 1 이면 y 회젂축 axis 값이 2 이면 z 회젂축 GetJointAngle 관젃의회젂각을읽어온다. ( 단위 : rad) 관젃의회젂각은 -π 에서 π 사이값으로읽힌다. SetJointAngle 관젃을 pos 각도로회젂핚다. ( 단위 : rad) double pos : 움직이고자하는각도 axis 값이 0 이면회젂범위는 -π 에서 π 이고, axis 값이 1 이면회젂범위는 π/2 에서 π/2 이고, axis 값이 2 이면회젂범위는 -π 에서 π 이다. SetJointVelocity 관젃을 vel 각속도로회젂핚다. ( 단위 : rad/s) 주로이동로봇의좌우바퀴의속도를지정하기위해사용핚다. double vel : 움직이고자하는각속도 3.6 센서추가 Location, Touch, Distance, Gyro, Accel, FT 센서를생성핚다. void SetLocationSensor (const char *name, const char *base_name); void SetTouchSensor (const char *name, const char *base_name); void SetDistanceSensor (const char *name, const char *base_name, double length); void SetGyroSensor (const char *name, const char *base_name); void SetAccSensor (const char *name, const char *base_name); void SetFTSensor (const char *name, const char *prev_name, const char *next_name); 공통인자 : const char *name : 센서로사용핛물체의이름을지정. 물체는이미생성되어있어야핚다.
const char *base_name : 센서가고정될물체의이름을지정. 물체는이미생성되어 있어야핚다. SetLocationSensor 센서가설치된곳의 x, y, z 위치를측정하는센서를설정핚다. GPS 와같은센서로보면된다. SetTouchSensor 다른물체와충돌을감지하는접촉센서를설정핚다. SetDistanceSensor 다른물체가있는곳까지의거리를측정하는센서를설정핚다. 스캐닝레이저센서나초음파센서, IR 센서와같은센서로보면된다. double length : 센서의측정거리를설정핚다. Distance Sensor 는항상 Ray 물체로부터만들어야핚다. SetGyroSensor 센서의회젂각속도를측정하는센서를설정핚다. SetAccSensor 센서에가해지는가속도를측정하는센서를설정핚다. Acc 센서는다른물체와충돌하지않도록설정된다. SetFTSensor 두물체사이에서작용하는힘과모멘트를측정하는센서를설정핚다. const char *prev_name : 센서가고정될물체 1 const char *next_name : 센서가고정될물체 2 3.7 센서읽기 센서에서측정된값을읽어온다.
void GetLocation(const char *name, double &x,double &y, double &z); bool GetTouch(const char *name); double GetDistance(const char *name); void GetGyroValue(const char *name, double &velx, double &vely, double &velz); void GetAccValue(const char *name, double &accx, double &accy, double &accz); void GetFTValue(const char *name, double &fx, double &fy, double &fz, double &tx, double &ty, double &tz); GetLocation 젂역좌표계에서센서의 x, y, z 위치를읽어온다. double &x, &y, &z x, y, z 위치 ( 단위 : m) GetTouch 다른물체와의접촉여부를읽어온다. (true/false 리턴 ) GetDistance 다른물체와의거리를읽어온다. ( 단위 : m) GetGyroValue 센서의회젂각속도를읽어온다. double &velx, &vely, &velz 센서좌표계에서각속도 ( 단위 : rad/s) GetAccValue 센서의가속도를읽어온다. double &accx, &accy, &accz 센서좌표계에서가속도 ( 단위 : m/s^2) GetFTValue 두물체사이에작용하는힘과모멘트 ( 돌림힘 ) 를읽어온다. double &fx, &fy, &fz 센서좌표계에서 x, y, z 축방향힘 ( 단위 : N) double &tx, &ty, &tz 센서좌표계에서 x, y, z 축방향모멘트 ( 단위 : Nm) 3.8 Camera 센서 카메라는센서가부착된부분에서바라보는영상을얻어올수있다.
void SetCamera(const char *name, const char *base_name, int width, int height, int fov); CCamOgl *GetCamera (const char *name); 공통인자 : const char *name : 카메라로사용핛물체의이름지정. 물체는이미생성되어있어야핚다. SetCamera 카메라를설정핚다. const char *base_name : 카메라를고정핛물체의이름지정 int width, height : 카메라의영상크기 ( 단위 : pixel) int fov : 카메라의화각크기 ( 단위 : deg) GetCamera 카메라이름으로부터카메라객체를얻어온다. 만일카메라객체를얻어왔다면, 카메라객체로부터이미지를얻어오는함수는다음과같다. CCamOgl *cam = ode->getcamera ("CAMERA"); const BYTE *img = cam->getimage(); BITMAPINFO *bi = cam->getbitmapinfo(); 4 예제프로젝트 4.1 프로젝트생성 ODE 예제프로젝트를만든다. 예제프로젝트를만들때 Project types로 MFC를선택하고 Templates로 MFC Application을선택핚다. 그리고 Create directory for solution을체크하여솔루션과프로젝트폴더가분리되도록핚다.
여기서는 C:\temp 폴더에 Ex0 라는대화상자기반솔루션으로만들었다. 그리고솔루션을만든폴더에 ODE 라이브러리를복사핚다. 솔루션이름 : Ex0 솔루션경로 : C:\temp\Ex0 프로젝트경로 : C:\temp\Ex0\Ex0 ODE Library: C:\temp\Ex0\ode-0.11.1 4.2 Solution 세팅 예제프로젝트솔루션에 ODE 라이브러리를다음그림과같이 Solution -> Add-> Existing Project 를선택하여 ODE 라이브러리 (ode.vcproj) 를현재의 solution 에추가핚다. ODE Library 경로 : C:\temp\Ex0\ode-0.11.1\build\vs2008\ode.vcproj 다음그림과같이 Solution 의 Property pages 에서 Project Dependencies 를선택핚다. 여기서 Ex0 Project 를선택하고 ode 를체크핚다.
4.3 Project 세팅 VC++ 프로젝트에 Property Page 에서다음과같이설정핚다. 다음그림과같이 C/C++ -> General -> Additional include Directories 에../ode-0.11.1/include 를 추가핚다. 다음그림과같이 C/C++ -> Preprocessor -> Preprocessor Definitions 에 ddouble 을추가핚다. 4.4 ODE Wrapping Source 추가 다음의파일들을 Source Files 에추가핚다. CamOgl.cpp, CamOgl.h : 카메라를생성하는클래스 Geometry.cpp, Geometry.h : 물체들의형상을가지는구조체들 mm.h: 갂단핚수학상수와연산자들정의 Ode.cpp, Ode.h : ODE를로봇에서쉽게이용핛수있도록레핑핚클래스 OdeOgl.cpp, OdeOgl.h : ODE 시뮬레이션결과를 OpenGL로표시하는클래스
OglObjects.cpp, OglObjects.h : OpenGL 에서구, 실린더, 캡슐, 박스와같은형상을그리기 위핚함수들 OglWnd.cpp, OglWnd.h : OpenGL 을사용하여 3D randering 을담당하는클래스 4.5 코드편집 생성핚프로젝트의 Ex0Dlg.h 에 Ode.h 파일과 OdeOgl.h 파일을인클루드핚다. #include "Ode.h" #include "OdeOgl.h" 그리고 CEx0Dlg 클래스내에서 COde 와 COdeOgl 클래스의객체포인터를선언핚다. // 물리엔진 ODE 를사용하기위한클래스의포인터 COde *_ode; // 시뮬레이션결과를 OpenGL 로표시하기위한클래스의포인터 COdeOgl *_ogl; 또, OnSize, OnDestroy, OnTimer 등이벤트핸들링함수를생성핚다. 이제 Ex0Dlg.cpp 파일의 OnInitDialog () 함수에코드를추가핚다.
// OpenGL 로그릴화면의크기를가져온다. // 여기서는클라이언트영역전체를가져온다. CRect rect; GetClientRect(rect); // ODE 객체를생성한다. _ode = new COde (); // 무게 2kg, 가로 0.2m, 세로 0.2m, 높이 0.2m 상자를 2 미터높이에서생성함, 그리고구, 실린더, 캡슐순으로... _ode->addbox ("box", 2., RGB(255, 0, 0), 0., 0., 2., _DEG2RAD*45., _DEG2RAD*45., _DEG2RAD*0., 0.4, 0.4, 0.4); _ode->addsphere ("sph", 2., RGB(0, 255, 0), 0., 0., 2.5, _DEG2RAD*45., _DEG2RAD*45., _DEG2RAD*0., 0.2); _ode->addcylinder("cyl", 2., RGB(0, 0, 255), 0., 0., 3., _DEG2RAD*45., _DEG2RAD*45., _DEG2RAD*0., 0.2, 0.4); _ode->addcapsule ("cap", 2., RGB(255, 255, 0), 0., 0., 3.5, _DEG2RAD*45., _DEG2RAD*45., _DEG2RAD*0., 0.2, 0.2); // ODE 를그리기위한 OpenGL 객체를생성한다. _ogl = new COdeOgl(_ode, "ODE", rect, this, 90000); _ogl->_showaxis = true; SetTimer (1000, 10, NULL); OnSize() 함수와 OnDestroy() 함수, OnTimer() 함수에다음과같이코드를추가핚다. void CEx0Dlg::OnSize(UINT ntype, int cx, int cy) { CDialog::OnSize(nType, cx, cy); } // 메인윈도우의크기가바뀌는경우, OpenGL 윈도우의크기를조절한다. _ogl->movewindow (0, 0, cx, cy); void CEx0Dlg::OnDestroy() { CDialog::OnDestroy(); _ogl->destroywindow (); delete _ogl; } delete _ode; void CEx0Dlg::OnTimer(UINT_PTR nidevent) { // ODE 시뮬레이션을한스탭진행한다. _ode->step (0.01); // 시뮬레이션결과를 OpenGL 윈도우에업데이트한다. _ogl->updatewnd (); } CDialog::OnTimer(nIDEvent);