Webots建模指南2 – 机器人建模

嗨伙计们，月更侠罗伯特祥又来和大家见面了！今天我们来聊一聊Webots中关于机器人建模的那点事儿，相信有了前面的基础，今天的文章对你来说So easy！话不多说，我们来看看怎么玩儿吧~

1.四轮差速小车建模

环境建模我们就略过去了，直接上图：

HingeJoint节点的使用：

首先添加一个Robot节点，在子节点children下新建一个Shape作为车身，重命名为car-body，然后在boundingObject下添加car-body节点作为碰撞边界。

车体有了，接下来我们就来添加它的旋转关节，这需要使用HingeJoint节点实现，先来看下它的建模结构：

从上图中我们可以清楚的看到HingeJoint的子节点结构，其中HingeJointParameters用来设置关节的一些属性参数，device用来添加驱动电机、关节角度传感器以及刹车，endPoint用来对关节进行形状建模以及下一个串联关节的建模。

具体细节就不展示了，我们直接来看第一个轮子的建模效果（调整完的效果）：

关节调试：

为了方便建模，我们直接调好这个轮子的参数，然后直接使用Ctrl+C大法。把车体调整成方便调试的姿态（下图为调整完的效果）：

小提示：轮子的操作是以Robot坐标系来操作的，点击Robot节点可以看到坐标系朝向。

写一个简单的控制器程序，来测试一下车轮转向：

哎哎哎？这是什么情况？轮子咋还绕车体了，这也不是我要的结果啊！！！

别着急，jointParameters下的anchor看到没，它就是用来设置旋转中心的，通过它可以设置一个与父节点同向的坐标系定义。通常情况，我们将它的值和endPoint下的translation设置成相同即可。

嗯这次确实不饶车体旋转了，可是这个旋转轴是不是有点问题啊~

jointParameters下的axis看到没，直接盘它！再来看看效果：

这次倒是正常了，就是旋转方向不太对啊~那咱就来给他换个方向，axis直接添个负号就完事儿了。

然后就是一段复制粘贴，完成了四个轮子的建模。这里要注意啦，别忘了修改旋转中心anchor和电机传感器的name啊，不然要出问题的~

摩擦力设置：

给它个速度，让它没事儿走两步：

嗯不错不错，能走了，可是怎么又出问题了，给它差速的时候为啥旋转不了啊！

四轮差速嘛，摩擦力很关键的。来来来，我们继续盘它！WorldInfo下面有个contactProperties节点，给它添加一个ContactProperties子节点，在这个子节点下面，我们把库伦摩擦coulombFriction改为0.5，再来测试一下，是不是发现问题解决了~

继续画蛇添足：

给它添个摄像头，再添个机械臂~

为了方便大家复现建模过程，下面直接给出建模过程中的参数设置：

嗯，有nei味儿了，来看下整体效果：

注意，我们车轮的建模顺序是从左前轮开始，顺时针进行的，如下图所示：

2. 运动学建模

假设S为轴距，L为轮距，r为车轮半径，左前轮为1号轮，右前轮为2号轮，左后轮为3号轮，右后轮为4号轮，参数如下：

S = 0.4m

L = 0.41m

R = 0.1m

根据叠加原理，将小车运动分解为绕小车中心的转动和运动方向的平动。同一侧轮子转速相同，假设左侧为v1，右侧为v2，于是可以得到：

① 直线运动速度为  v=(v1+v2)/2

② 旋转速度为  w=(v1-v2)sin(alpha/2)

最终得到四轮小车运动学模型如下：

3. 控制器

控制器中，我们实现键盘控制小车的运动，并且获取RGB图像和深度图像。

懒的封装类了，话不多说，简单粗暴点儿直接上代码！

"""my_first_controller controller."""
# -*- coding: UTF-8 -*-
# author：罗伯特祥
# Created on:2020-05-13

from controller import Robot,Keyboard

def main():
    # 实例化机器人
    robot = Robot()
    
    # 获取当前世界的基本时间步长
    timestep = int(robot.getBasicTimeStep())
    
    # 实例化keyboard类
    keyboard = Keyboard()
    keyboard.enable(10*timestep )
    
    # 关联电机设备
    wheel1 = robot.getMotor('wheel1')
    wheel2 = robot.getMotor('wheel2')
    wheel3 = robot.getMotor('wheel3')
    wheel4 = robot.getMotor('wheel4')
    
    # 关联并使能传感器设备
    camera = robot.getCamera('kinect color')
    cameraRange = robot.getRangeFinder('kinect range')
    camera.enable(4*timestep)
    cameraRange.enable(4*timestep)
    
    # 设置电机运行模式
    wheel1.setPosition(float('inf'))
    wheel2.setPosition(float('inf'))
    wheel3.setPosition(float('inf'))
    wheel4.setPosition(float('inf'))
    
    # 设置初始速度
    wheel1.setVelocity(0)
    wheel2.setVelocity(0)
    wheel3.setVelocity(0)
    wheel4.setVelocity(0)
    
    R_ = 0.1  # 车轮半径
    S_ = 0.4  # 轴距
    L_ = 0.41 # 轮距
    # Main loop:
    while robot.step(timestep) != -1:
        # 运动学模型
        v1 = wheel1.getVelocity() * R_
        v2 = wheel2.getVelocity() * R_
        vel = (v1+v2)/2
        
        # 逆时针为正
        temp_ = L_ * L_ + S_ * S_
        omega = (v2 - v1) * L_ / temp_
        print("小车速度：v = " + str(vel) + ", omega = " + str(omega))
    
        # 获取键盘操作
        key = keyboard.getKey()
        if key == ord('Q'):
            wheel1.setVelocity(1)
            wheel2.setVelocity(3)
            wheel3.setVelocity(3)
            wheel4.setVelocity(1)
        elif key == ord('E'):
            wheel1.setVelocity(3)
            wheel2.setVelocity(1)
            wheel3.setVelocity(1)
            wheel4.setVelocity(3)
        elif key == Keyboard.UP:
            wheel1.setVelocity(2)
            wheel2.setVelocity(2)
            wheel3.setVelocity(2)
            wheel4.setVelocity(2)
        elif key == Keyboard.DOWN:
            wheel1.setVelocity(-2)
            wheel2.setVelocity(-2)
            wheel3.setVelocity(-2)
            wheel4.setVelocity(-2)
        elif key == Keyboard.LEFT:
            wheel1.setVelocity(-2)
            wheel2.setVelocity(2)
            wheel3.setVelocity(2)
            wheel4.setVelocity(-2)
        elif key == Keyboard.RIGHT:
            wheel1.setVelocity(2)
            wheel2.setVelocity(-2)
            wheel3.setVelocity(-2)
            wheel4.setVelocity(2)
        else:
            wheel1.setVelocity(0)
            wheel2.setVelocity(0)
            wheel3.setVelocity(0)
            wheel4.setVelocity(0)
        
main()

操作如下：

Q：左转弯

E：右转弯

↑：前进

↓：后退

←：左旋转

→：右旋转

这操作，我竟然有种当年玩QQ飞车的感觉~

来看看最终效果：

妥妥的一个youBot，具体怎么玩儿，剩下的就交给屏幕前的你了！下次我们来讲讲Webots在ROS中是怎么玩儿的~

总结

① 旋转关节HingeJoint中，HingeJointParameters用来设置关节的一些属性参数，device用来添加驱动电机、关节角度传感器以及刹车，endPoint用来对关节进行形状建模以及下一个串联关节的建模；

② 旋转关节的旋转是参考其父节点Robot坐标系的；

③ WorldInfo下的contactProperties节点可以设置摩擦力；

④ 复制粘贴实现的建模，最后不要忘记修改旋转中心anchor和电机传感器的name；

⑤ Robot节点是可以包含Robot节点的，这有利于复合机器人建模，提高机器人的复用率；

⑥ KeyBoard、相机、深度图像等节点的使用，其中Kinect是RGB相机和深度相机的一个集合，RGB相机设备name为kinect color，深度设备name为kinect range，云台电机name为tilt motor，具体操作见控制器代码；

本文设计的项目文件：https://share.weiyun.com/xkLYeC2S

读万卷书也要行万里路，我是罗伯特祥，下次见！

最后附上几张机器人建模的思维导图，想要高清图？古月居公众号回复“建模仿真518”获取吧！