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

2023-01-04 16:54:09

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

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

码农公寓

相关文章