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

原创文章，来自“博客园，_阿龙clliu” http://www.cnblogs.com/clliu/，转载请注明原文章出处。

在一些三维制图软件或仿真软件里，都有运动副的概念，webots的节点里好像没有，不要担心，在物理插件里可以做到，不过要学习有关于ODE（开源动力学引擎的）一些内容。在webots中，怎样建立一个铰链呢，我做了一个简单的例子，小区门口的那种可以控制的栏杆，效果图如下。

那每一个杆件，它们的关系在怎么定义呢？其实关系是在ODE中定义的，我们只需要建立一些没有约束关系的杆件就行。场景树贴出在下面，虽然这些节点是以servo定义的，但是在不需要约束关系的杆件，例如连杆3，将type改成none，那么，杆件3与gan Robot的关系就不是servo关系了，在仿真中可以看成两个零件。模型就不介绍怎么建立了，后面会给出整个仿真的文件。

         

建立完模型之后，点击菜单栏的  向导 —>新物理插件，代码如下：

 //来自“博客园，_阿龙clliu” http://www.cnblogs.com/clliu/，
 #include <ode/ode.h>
 #include <plugins/physics.h>

 dBodyID getBody(const char *def) {
   dBodyID body = dWebotsGetBodyFromDEF(def);
   if (! body) dWebotsConsolePrintf("Warning: did not find body with DEF name: %s", def);
   return body;
 }

 void webots_physics_init(dWorldID world, dSpaceID space, dJointGroupID contactJointGroup) {

    //得到杆件的ID
    dBodyID link1 = getBody("link1");
    dBodyID link2 = getBody("link2");
    dBodyID link3 = getBody("link3");
    dBodyID link4 = getBody("link4");
    dBodyID link5 = getBody("link5");
    dBodyID link6 = getBody("link6");

    //创建一个铰链副
    dJointID hingeJoint = dJointCreateHinge(world, );
    //定义铰链副约束的两个零件，link1和link3
    dJointAttach(hingeJoint, link1, link3);

    //定义铰链的作用点
    dVector3 hinge_interface;
    //将link1零件自身坐标系的（0，0，0）点转为全局坐标系坐标,赋给hinge_interface
    dBodyGetRelPointPos(link1, , , , hinge_interface);

    //设置铰链作用点
    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    //设置铰链作用轴（作用方向）
    dJointSetHingeAxis(hingeJoint, , , );

    //link4?link3??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link4, link3);

    dBodyGetRelPointPos(link4, , -0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );

    //link5?link3??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link5, link3);

    dBodyGetRelPointPos(link5, , -0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );

    //link6?link3??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link6, link3);

    dBodyGetRelPointPos(link6, , -0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );

    //link4?link2??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link4, link2);

    dBodyGetRelPointPos(link4, , 0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );

    //link5?link2??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link5, link2);

    dBodyGetRelPointPos(link5, , 0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );

    //link6?link2??
    hingeJoint = dJointCreateHinge(world, );
    dJointAttach(hingeJoint, link6, link2);

    dBodyGetRelPointPos(link6, , 0.05, , hinge_interface);

    dJointSetHingeAnchor(hingeJoint, hinge_interface[], hinge_interface[], hinge_interface[]);
    dJointSetHingeAxis(hingeJoint, , , );
 }

 void webots_physics_step() {

 }

 void webots_physics_draw() {

 }

 int webots_physics_collide(dGeomID g1, dGeomID g2) {

   return ;
 }

 void webots_physics_cleanup() {

 }

设置完成物理引擎，在worldInfo节点下physics节点下选择该物理插件。控制器代码如下：

 //来自“博客园，_阿龙clliu” http://www.cnblogs.com/clliu/，
 #include <webots/robot.h>
 #include <webots/servo.h>
 #include <assert.h>
 #include <math.h>

 #define TIME_STEP 32
 #define rad_2_deg(X) ( X / pi * 180.0 )
 #define deg_2_rad(X) ( X / 180.0 * pi )
 #define pi 3.1415926
 #define frep 1

 int main(int argc, char **argv)
 {

   double t = ;
   double servo_pos;

   wb_robot_init();

   WbDeviceTag servo;
   servo = wb_robot_get_device("link2");
   assert(servo);

   while (wb_robot_step(TIME_STEP) != -) {

     servo_pos =  +  * sin(frep * t + pi);
     wb_servo_set_position(servo,deg_2_rad(servo_pos));

     t += (double)TIME_STEP / 1000.0;
   };

   wb_robot_cleanup();

   return ;
 }

如果没有什么错误，就能实现之前GIF的效果了，如果没做出来，也没关系，给出原文件，仿真文件下载地址。

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