webots自学笔记（五）使用物理插件ODE建立铰链

      在一些三维制图软件或仿真软件里，都有运动副的概念，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的效果了，如果没做出来，也没关系，给出原文件，仿真文件下载地址。

     如果有什么疑问，欢迎再下方提问。