Vrep学习笔记（一）

本文转载自知乎：AI与机器人

一、用代码控制YouBot机器人

1.1 YouBot机器人模型

• YouBot机器人包括底盘和机械臂。
• 底盘带有四个麦轮，麦轮分为A轮和B轮，可以前后左右移动。
• 机械臂有5个旋转关节和相应的连杆机构串联组成，末端装有夹持机构。每个关节可以进行单独的控制。在Vrep中已经构建好了基本的关节模型和运动控制接口，只需要调用响应的控制接口来控制各个关节就行了。

1.2 Vrep中基本的代码框架

• V-REP的控制脚本至少包含Main script和Child script。

• Main script对应每一个Scene，一个Scene只有一个。这部分是V-REP软件运行时会自动加载的控制脚本，在实际使用时我们不用修改这里的内容。

• Child script对应每一个模型以及模型当中的部分关节等。每一个模型都会包含一个跟自己绑定的Child script，如果涉及到更加底层的控制过程，我们可以给每一个关节、每一个传感器等都定义一个自己的Child script。因此可以修改YouBot机器人的Child script里面的代码来实现机器人的控制。

• 脚本中都会包含一些基本函数，但不是每一个都一定要包含。一般sysCall_init()、sysCall_cleanup()是默认包含的。V-REP在运行时后台自动调用这些函数，不能修改这4个函数的名字，否则V-REP在运行时无法找到对应的函数。
  
  1.3 用Lua代码控制YouBot

• 初始化函数部分：从V-REP环境中加载各个关节的Handle。Handle是一种类似文件句柄的识别符，给它传递必要的参数来对它进行操作就可以控制对应的模型。Lua语言编写的代码中，所有定义的变量都默认为全局变量，例如在sysCall_init()函数中定义的wheel_joints这个变量就可以被其他函数调用。而想要编写仅用于本函数的局部变量，需要使用local关键字。

-- 初始化YouBot机器人
function sysCall_init()
    -- 获得机器人的句柄
    you_bot = sim.getObjectHandle('youBot')
    
    -- 获得四个轮子的句柄，写到一个数组里
    wheel_joints = {
   -1,-1,-1,-1} -- 前左，后左，后右，前右
    wheel_joints[1] = sim.getObjectHandle('rollingJoint_fl')
    wheel_joints[2] = sim.getObjectHandle('rollingJoint_rl')
    wheel_joints[3] = sim.getObjectHandle('rollingJoint_rr')
    wheel_joints[4] = sim.getObjectHandle('rollingJoint_fr')
    
    -- 获取五个关节的句柄，写到一个数组里
    arm_joints = {
   -1,-1,-1,-1,-1} -- set default values
    for i=0,4,1 do
        arm_joints[i+1] = sim.getObjectHandle('youBotArmJoint'..i)
    end
    
    -- 初始化时每个关节要到达的位置
    desired_joint_angles = {
   0,0,0,0,0}
    
    -- 初始化每个关节
    for i = 1,5,1 do
        sim.setJointPosition(arm_joints[i], desired_joint_angles[i])
    end

    -- 设置最大的关节速度
    max_joint_velocity = 50*math.pi/180 
    
    -- 用户自定义变量，默认是全局变量
    switch_pos_flag = 0
    go_forward_flag = 1 -- 机器人向前移动的标志
end

• 定义各个关节如何随时间运动：获取V-REP软件的仿真计时，并根据仿真的时间每隔10秒更新一次底盘轮子的运动方向，以及各个机械臂关节的目标运动角度。
• 轮子的控制过程是直接设定轮子的运动速度和方向，通过调用sim.setJointTargetVelocity()来实现对某个指定轮子的控制过程，根据时间改变了轮子的运动方向。
• 机械臂关节的控制过程是使用的位置控制API函数sim.setJointPosition()。由于该函数将会直接将指定关节设定为目标角度，如果我们让关节直接从某一个位置“跳转”到目标位置，当目标位置与当前位置距离较远时，该关节会以最大运行加速度猛烈运动，会导致该关节的运动发生冲击，引起机器震荡。因此，，在给定机械臂各个关节目标角度的时候，不能直接给一个最后的目标位置，需要设定关节最大的一个运动速度，并通过计算当前位置与目标位置的偏差，将其拆分为“多段”位置分时输入到机器人的关节当中。

-- 控制youbot机器人的关节
function sysCall_actuation() 

	--获取当前关节位置
    current_joint_angles = {
   0,0,0,0,0}
    for i=1,5,1 do
        current_joint_angles[i] = sim.getJointPosition(arm_joints[i])
    end
	--最大允许变动范围
    max_variation_allowed = max_joint_velocity*sim.getSimulationTimeStep()
    --当前时间
    simu_time = sim.getSimulationTime()
    
    -- 每隔10秒作出以下改变改变
    if (simu_time % 10 == 0) then
        print('\nSimulation time = '..simu_time)
        
        -- 每隔20秒改变前进后退
        if (simu_time % 20 == 0) then
            if go_forward_flag == 1 then
                go_forward_flag = -1 -- go backward
            else
                go_forward_flag = 1 -- go forward
            end
        end
        
        -- 每隔10秒设定不同的关节位置，有3个位置
        if switch_pos_flag == 0 then
            print("Switch to position 1")
            desired_joint_angles = {
   180,0,0,0