原創文章，來自“博客園，_阿龍clliu” http://www.cnblogs.com/clliu/，轉載請注明原文章出處。
在一些三維制圖軟件或仿真軟件里，都有運動副的概念，webots的節點里好像沒有，不要擔心，在物理插件里可以做到，不過要學習有關於ODE（開源動力學引擎的）一些內容。在webots中，怎樣建立一個鉸鏈呢，我做了一個簡單的例子，小區門口的那種可以控制的欄桿，效果圖如下。
那每一個桿件，它們的關系在怎么定義呢？其實關系是在ODE中定義的，我們只需要建立一些沒有約束關系的桿件就行。場景樹貼出在下面，雖然這些節點是以servo定義的，但是在不需要約束關系的桿件，例如連桿3，將type改成none，那么，桿件3與gan Robot的關系就不是servo關系了，在仿真中可以看成兩個零件。模型就不介紹怎么建立了，后面會給出整個仿真的文件。
建立完模型之后，點擊菜單欄的 向導 —>新物理插件，代碼如下：
1 //來自“博客園，_阿龍clliu” http://www.cnblogs.com/clliu/， 2 #include <ode/ode.h> 3 #include <plugins/physics.h> 4 5 6 7 dBodyID getBody(const char *def) { 8 dBodyID body = dWebotsGetBodyFromDEF(def); 9 if (! body) dWebotsConsolePrintf("Warning: did not find body with DEF name: %s", def); 10 return body; 11 } 12 13 14 void webots_physics_init(dWorldID world, dSpaceID space, dJointGroupID contactJointGroup) { 15 16 //得到桿件的ID 17 dBodyID link1 = getBody("link1"); 18 dBodyID link2 = getBody("link2"); 19 dBodyID link3 = getBody("link3"); 20 dBodyID link4 = getBody("link4"); 21 dBodyID link5 = getBody("link5"); 22 dBodyID link6 = getBody("link6"); 23 24 //創建一個鉸鏈副 25 dJointID hingeJoint = dJointCreateHinge(world, 0); 26 //定義鉸鏈副約束的兩個零件，link1和link3 27 dJointAttach(hingeJoint, link1, link3); 28 29 //定義鉸鏈的作用點 30 dVector3 hinge_interface; 31 //將link1零件自身坐標系的（0，0，0）點轉為全局坐標系坐標,賦給hinge_interface 32 dBodyGetRelPointPos(link1, 0, 0, 0, hinge_interface); 33 34 //設置鉸鏈作用點 35 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 36 //設置鉸鏈作用軸（作用方向） 37 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 38 39 //link4?link3?? 40 hingeJoint = dJointCreateHinge(world, 0); 41 dJointAttach(hingeJoint, link4, link3); 42 43 dBodyGetRelPointPos(link4, 0, -0.05, 0, hinge_interface); 44 45 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 46 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 47 48 //link5?link3?? 49 hingeJoint = dJointCreateHinge(world, 0); 50 dJointAttach(hingeJoint, link5, link3); 51 52 dBodyGetRelPointPos(link5, 0, -0.05, 0, hinge_interface); 53 54 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 55 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 56 57 //link6?link3?? 58 hingeJoint = dJointCreateHinge(world, 0); 59 dJointAttach(hingeJoint, link6, link3); 60 61 dBodyGetRelPointPos(link6, 0, -0.05, 0, hinge_interface); 62 63 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 64 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 65 66 //link4?link2?? 67 hingeJoint = dJointCreateHinge(world, 0); 68 dJointAttach(hingeJoint, link4, link2); 69 70 dBodyGetRelPointPos(link4, 0, 0.05, 0, hinge_interface); 71 72 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 73 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 74 75 //link5?link2?? 76 hingeJoint = dJointCreateHinge(world, 0); 77 dJointAttach(hingeJoint, link5, link2); 78 79 dBodyGetRelPointPos(link5, 0, 0.05, 0, hinge_interface); 80 81 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 82 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 83 84 //link6?link2?? 85 hingeJoint = dJointCreateHinge(world, 0); 86 dJointAttach(hingeJoint, link6, link2); 87 88 dBodyGetRelPointPos(link6, 0, 0.05, 0, hinge_interface); 89 90 dJointSetHingeAnchor(hingeJoint, hinge_interface[0], hinge_interface[1], hinge_interface[2]); 91 dJointSetHingeAxis(hingeJoint, 0, 0, 1); 92 } 93 94 void webots_physics_step() { 95 96 } 97 98 void webots_physics_draw() { 99 100 } 101 102 int webots_physics_collide(dGeomID g1, dGeomID g2) { 103 104 return 0; 105 } 106 107 void webots_physics_cleanup() { 108 109 }
設置完成物理引擎，在worldInfo節點下physics節點下選擇該物理插件。控制器代碼如下：
1 //來自“博客園，_阿龍clliu” http://www.cnblogs.com/clliu/， 2 #include <webots/robot.h> 3 #include <webots/servo.h> 4 #include <assert.h> 5 #include <math.h> 6 7 #define TIME_STEP 32 8 #define rad_2_deg(X) ( X / pi * 180.0 ) 9 #define deg_2_rad(X) ( X / 180.0 * pi ) 10 #define pi 3.1415926 11 #define frep 1 12 13 int main(int argc, char **argv) 14 { 15 16 double t = 0; 17 double servo_pos; 18 19 20 wb_robot_init(); 21 22 WbDeviceTag servo; 23 servo = wb_robot_get_device("link2"); 24 assert(servo); 25 26 while (wb_robot_step(TIME_STEP) != -1) { 27 28 29 servo_pos = 45 + 45 * sin(frep * t + pi); 30 wb_servo_set_position(servo,deg_2_rad(servo_pos)); 31 32 t += (double)TIME_STEP / 1000.0; 33 }; 34 35 36 wb_robot_cleanup(); 37 38 return 0; 39 }
如果沒有什么錯誤，就能實現之前GIF的效果了，如果沒做出來，也沒關系，給出原文件，仿真文件下載地址。
如果有什么疑問，歡迎再下方提問。