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

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

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

 

posted @ 2017-03-18 17:36  _阿龙clliu  阅读(4003)  评论(5)  编辑  收藏  举报