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,0}
            switch_pos_flag = 1
        elseif switch_pos_flag == 1 then
            print("Switch to position 2")
            desired_joint_angles = {120*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}
            switch_pos_flag = 2
        else
            print("Switch to position 3")
            desired_joint_angles = {-120*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}
            switch_pos_flag = 0
        end   
    end
  
    -- 设置轮子的运动速度
    wheel_velocity = go_forward_flag*0.8;
    sim.setJointTargetVelocity(wheel_joints[1], wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[2], wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[3], wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[4], wheel_velocity)
      
    -- 设置关节运动到的位置
    for i=1,5,1 do
        delta = desired_joint_angles[i] - current_joint_angles[i]
        if (math.abs(delta) > max_variation_allowed) then
            delta = max_variation_allowed*delta/math.abs(delta) -- limit the variation of each joint
        end
        
        sim.setJointPosition(arm_joints[i], current_joint_angles[i] + delta)
    end
end 

二、用键盘控制 YouBot机器人

• 自定义了一个键盘事件检测函数，用来检测键盘上哪些按键被按下了。检测的方式是调用simGetSimulatorMessage()这个函数来获取键盘的按下情况，并通过比对键盘码来设定YouBot的运动方向。

function getKeyboardStatus()
	--message为键盘是否被按下
	--data存储键盘按下的信息，然后将该信息传到用户自定义的方向上
    message,data,data2=simGetSimulatorMessage()
    if (message==sim_message_keypress) then
        if (data[1]==2007) then -- up key
            print("up")
            move_direction = 1
        elseif (data[1]==2008) then -- down key
            print("down")
            move_direction = 2
        elseif (data[1]==2009) then -- left key
            print("left")
            move_direction = 3
        elseif (data[1]==2010) then -- right key
            print("right")
            move_direction = 4
        elseif (data[1]==string.byte('q')) then -- q key
            print("q")
            move_direction = 5
        elseif (data[1]==string.byte('w')) then -- w key
            print("w")
            move_direction = 6
        end

        if (data[1]==string.byte(' ')) then -- space key
            wheel_velocity = 0
            print("stop")
        else
            wheel_velocity = 1
        end
    end
end

• 获得了键盘的信息以后，我们需要设定YouBot底盘上每个轮子的运动方向来实现上述的几个功能

function setYouBotMovementDirection()
    -- Check movement direction
    if (move_direction == 1) then -- go forward
        lf_dire = -1
        rf_dire = -1
        lr_dire = -1
        rr_dire = -1
    elseif (move_direction == 2) then -- go backward
        lf_dire = 1
        rf_dire = 1
        lr_dire = 1
        rr_dire = 1
    elseif (move_direction == 3) then -- go left
        lf_dire = 1
        rf_dire = -1
        lr_dire = -1
        rr_dire = 1
    elseif (move_direction == 4) then -- go right
        lf_dire = -1
        rf_dire = 1
        lr_dire = 1
        rr_dire = -1
    elseif (move_direction == 5) then -- turn left
        lf_dire = 1
        rf_dire = -1
        lr_dire = 1
        rr_dire = -1
    elseif (move_direction == 6) then -- turn right
        lf_dire = -1
        rf_dire = 1
        lr_dire = -1
        rr_dire = 1
    end
end

• 在sysCall_actuation()这个函数接口里面调用我们刚刚自定义的2个函数来控制每个轮子的运动：

function sysCall_actuation()
    -- Check keyboard press events
    getKeyboardStatus()
    setYouBotMovementDirection()

    -- 关节臂保持不动
    for i = 1,5,1 do
        sim.setJointPosition(arm_joints[i], desired_joint_angles[i])
    end
    
    -- 控制轮子运动
    sim.setJointTargetVelocity(wheel_joints[1], lf_dire*wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[2], lr_dire*wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[3], rr_dire*wheel_velocity)
    sim.setJointTargetVelocity(wheel_joints[4], rf_dire*wheel_velocity)
end 

三、YouBot底盘运动学与路径规划

3.1 底盘运动学

• 麦轮中的A轮向前运动时会提供向左的运动速度，向后时会提供向右的运动速度。B轮的侧向运动方向与A轮刚好相反。

• 逆运动学求解：给定机器人的空间运动轨迹反求4个轮子在各个时刻的运动速度，将底盘的运动状态合并到中心点处，关系为：
  

• 给定世界坐标系X-O-Y如图所示，其中Y的正方向表示YouBot机器人向正前方运动，X的正方向表示YouBot机器人向正右方运动。所有与X、Y正方向同向的量的符号都为正，反之则为负，所有的角度旋转方向的定义遵循右手定则。则底盘的中心点运动情况可以分解为中心点沿X方向运动的速度，沿Y方向运动的速度和绕中心点旋转的角速度：
  
  

• 求解中心点的运动状态与四个轮子的运动状态之间的关系，轮子在前后运动时也会侧向运动，而侧向运动的速度与前后 运动的速度呈线性比例关系，因此向前运动和侧向运动的速度合成为kv1。
  
  
  
  
  
  
  
  3.2 底盘运动学代码

• 在V-REP中，YouBot的4个轮子的正方向与我们定义的正方向刚好相反，因此在计算完成以后4个轮子的速度方向均要反向。完成了YouBot机器人的底盘逆运动学模型建模后，只需要指定中心点位置的运动状态（见公式（1）），即可求出各个轮子的运动状态。

function chassisInverseKinematics(vx, vy, omega, wheel_R, a, b)
    omega_1 = (vy - vx + (a+b)*omega)/wheel_R
    omega_2 = (vy + vx - (a+b)*omega)/wheel_R
    omega_3 = (vy - vx - (a+b)*omega)/wheel_R
    omega_4 = (vy + vx + (a+b)*omega)/wheel_R
    
    -- set the right direction for each wheel
    v_wheel_1 = -omega_1
    v_wheel_2 = -omega_2
    v_wheel_3 = -omega_3
    v_wheel_4 = -omega_4
end

3.3 路径规划

• 控制机器人进行左右移动和转向。在代码的初始化部分，需要定义dumpy点，以及初始位姿

function sysCall_init()
    -- 获得机器人句柄，dummy句柄
    you_bot = sim.getObjectHandle('youBot')
    you_bot_dummy = sim.getObjectHandle('youBotDummy')
    
    -- 轮子的初始句柄
    wheel_joints = {-1,-1,-1,-1} -- front left, rear left, rear right, front right
    wheel_joints[1] = sim.getObjectHandle('rollingJoint_fr')
    wheel_joints[2] = sim.getObjectHandle('rollingJoint_fl')
    wheel_joints[3] = sim.getObjectHandle('rollingJoint_rl')
    wheel_joints[4] = sim.getObjectHandle('rollingJoint_rr')
    
    -- 关节的初始句柄
    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 = {180*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}
    
    -- 设置关节的初始位置
    for i = 1,5,1 do
        sim.setJointPosition(arm_joints[i], desired_joint_angles[i])
    end

    -- 设置最大速度
    max_joint_velocity = 100*math.pi/180
    
    --四个轮子的角速度
    omega_1 = 0
    omega_2 = 0
    omega_3 = 0
    omega_4 = 0

    -- youBot的参数
    wheel_R = 0.05 -- 0.05 m
    a = 0.165 -- 0.228 m
    b = 0.228 -- 0.25 m
    
    --dummy点后面要去的位置和方向
    dummy_guider_position = {0, 0, 0}
    dummy_guider_orientation = {0, 0, 0}
    circle_path_R = 1
    theta = 0
    target_pos = {0, 0, 0}
    
    --机器人的初始位置和方向
    youbot_init_position = {0, 0, 9.5341e-02}
    sim.setObjectPosition(you_bot, -1, youbot_init_position)
    youbot_init_orientation = {-1.57, 0, -1.57}
    sim.setObjectOrientation(you_bot, -1, youbot_init_orientation)
    
    err_pos = {0,0,0}
    err_pos_last = {0,0,0}
    err_vel = {0,0,0}
end

• 机器人控制部分

function sysCall_actuation()
    -- 保持机器人关节臂不动
    for i = 1,5,1 do
        sim.setJointPosition(arm_joints[i], desired_joint_angles[i])
    end

    simu_time = sim.getSimulationTime()
    
    --根据时间设定目标位置和方向
    if simu_time < 10 then
        target_pos = {0, 1, 0}
    elseif simu_time < 20 then
        target_pos = {0, 0, 0}
    elseif simu_time < 30 then
        target_pos = {1, 0, 0}
    elseif simu_time < 40 then
        target_pos = {0, 0, 0}
    elseif simu_time < 50 then
        target_pos = {1, 1, 0}
    elseif simu_time < 60 then
        target_pos = {0, 0, 0}
    elseif simu_time < 70 then
        target_pos = {0, 0, math.pi/2}
    elseif simu_time < 80 then
        target_pos = {0, 0, 0}
    elseif simu_time < 90 then
        target_pos = {0, 0, -math.pi/2}
    elseif simu_time < 100 then
        target_pos = {0, 0, 0}
    end

    --获取dummy点现在的位置和方向
    current_pos = sim.getObjectPosition(you_bot_dummy,-1)
    current_orientation = sim.getObjectOrientation(you_bot_dummy, -1)
    current_pos[3] = current_orientation[3]

    
    -- Simple PID control
    KP_pos = 1 -- 1
    KP_omega = 0.3

    KD_pos = 20  -- 20
    KD_omega = 0
	--计算要从当前点到目标点，中心点的运动状态是多少
    err_pos_last = err_pos
    err_pos = {target_pos[1] - current_pos[1], target_pos[2] - current_pos[2], target_pos[3] - current_pos[3]}
    err_vel = {err_pos[1] - err_pos_last[1], err_pos[2] - err_pos_last[2], err_pos[3] - err_pos_last[3]}

    center_velocity = {KP_pos*err_pos[1] + KD_pos*err_vel[1], KP_pos*err_pos[2] + KD_pos*err_vel[2], KP_omega*err_pos[3] + KD_omega*err_vel[3]}
	--根据中心点的运动状态，求四个轮子的运动状态
    chassisInverseKinematics(center_velocity[1], center_velocity[2], center_velocity[3], wheel_R, a, b)
    
    -- 设置四个轮子的运动状态
    sim.setJointTargetVelocity(wheel_joints[1], v_wheel_1)
    sim.setJointTargetVelocity(wheel_joints[2], v_wheel_2)
    sim.setJointTargetVelocity(wheel_joints[3], v_wheel_3)
    sim.setJointTargetVelocity(wheel_joints[4], v_wheel_4)
end