原创文章，来自“博客园，_阿龙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的效果了，如果没做出来，也没关系，给出原文件，仿真文件下载地址。
如果有什么疑问，欢迎再下方提问。