Vrep学习笔记（二）

四、基于Dummy和path的路径规划

4.1 Dummy和path

• Path用来自定义各种运动路径，从Path上我们可以给定某一个时刻机器人运动到某一点的位置以及其姿态，定义机器人在整个路径中每一时刻的运动过程，V-REP中默认的Path是通过给定一些关键的控制点，并在这些控制点中间采用贝塞尔曲线插值来得到光滑路径的。Path在创建以后，用户也可以通过添加自定义的控制点和设定它的属性来定义不同形状的路径，包括在三维空间中的运动。
• Dummy是一种带有姿态信息的虚拟“质点”，Dummy可以沿着Path向前运动，实时获取当前时刻在Path上的位置和姿态，作为路径上的一个“领航者”。Dummy可以绑定到机械臂的末端上作为一个“引导点”.
• 在V-REP中，使用鼠标右键->Add->…来快速新建Dummy和Path。如果想要Dummy跟随某一条Path运动，首先要将这个Dummy“绑定”到对应的Path下面，使用鼠标选中并将Dummy拉到Path的子目录下.
• 在构建完Dummy和Path以后，需要为Dummy创建一个进程式的Child script来获取Dummy的运动信息。

function sysCall_threadmain()
    dummy_guider_handle = sim.getObjectHandle('DummyGuider')
    path_handle = sim.getObjectHandle('Path')
    
    print('start follow path')
    sim.followPath(dummy_guider_handle, path_handle, 3, 0, 0.1, 1)
end

• 在仿真时，这个child script会启动一个新的进程，其中followpath指的是dummy跟随哪一个path运动，字3表示同时获取Path的位置和姿态信息（1表示只获取位置，2表示只获取姿态），0.1表示Dummy会以0.1m/s的速度前进，加速度为1
• 在YouBot的Child script中调用sim.getObjectPosition和sim.getObjectOrientation两个函数来实时获取Dummy的位置和姿态。-1表示获取的是绝对信息，即在世界坐标系下的位置和姿态信息.

dummy_guider_position = sim.getObjectPosition(dummy_guider_handle, -1)
dummy_guider_orientation = sim.getObjectOrientation(dummy_guider_handle, -1)

• 获取到了Dummy的位置和姿态以后，以Dummy的运动位置和姿态作YouBot机器人中心点运动的“领航者”，通过获取Dummy的速度来得到YouBot机器人的中心点目标运动速度。在本文的Circle型Path中，中心点的运动状态可以计算为：

-- Path 1: one circle
vx = 0
vy = dummy_guider_velocity
omega = dummy_guider_velocity / circle_path_R 

• 我们建立的YouBot运动学模型是基于YouBot机器人自身的局部坐标系建立的，而非全局世界坐标系，所以Dummy的运动状态要转换为YouBot机器人的局部坐标系中的运动状态才能使用，在这里，YouBot随Circle型路径运动的线速度为0.1m/s，在YouBot机器人的局部坐标系中，相当于vy = 0.1m/s，而vx = 0。

4.2 圆形路径规划

• 跟path绑定的dummy点设置在path的圆心，dummy有自己的child script。而youbot机器人的dummy_bot设置在底盘的中心点

function sysCall_threadmain()
	--获取dummy和path的句柄，这个句柄就是指代这个物体自己
    dummy_guider_handle = sim.getObjectHandle('DummyGuider')
    path_handle = sim.getObjectHandle('Path')
    
    print('start follow path')
    sim.followPath(dummy_guider_handle, path_handle, 3, 0, 0.1, 1)
end

• 机器人脚本初始化

function sysCall_init()
    -- 机器人、机器人dummy点、领航的dummy点
    you_bot = sim.getObjectHandle('youBot')
    you_bot_dummy = sim.getObjectHandle('youBotDummy')
    dummy_guider_handle = sim.getObjectHandle('DummyGuider')
    
    -- 轮子初始值
    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 size parameters
    wheel_R = 0.05
    a = 0.165
    b = 0.228
    
    dummy_guider_position = {0, 0, 0}
    dummy_guider_orientation = {0, 0, 0}
    circle_path_R = 1
    target_pos = {0, 0, 0}
    
    -- 初始化机器人
    youbot_init_orientation = {-1.57, 0, -1.57}
    sim.setObjectOrientation(you_bot, -1, youbot_init_orientation)

    -- 为圆形路径做初始化
    youbot_init_position = {1, 0, 9.5341e-02}
end

• 机器人控制

-- Control joints of YouBot
function sysCall_actuation()
    -- 保持五个关节臂的运动状态
    for i = 1,5,1 do
        sim.setJointPosition(arm_joints[i], desired_joint_angles[i])
    end

    simu_time = sim.getSimulationTime()

    -- dummy的速度
    dummy_guider_velocity = 0.1

    -- Path 1: one circle
    vx = 0
    vy = dummy_guider_velocity
    omega = dummy_guider_velocity / circle_path_R 

    center_velocity = {vx, vy, omega}
    chassisInverseKinematics(center_velocity[1], center_velocity[2], center_velocity[3], wheel_R, a, b)
    -- print('time=', simu_time, 'target_pos[1]=', target_pos[1], 'center_velocity=', center_velocity)
    
    -- Apply the desired wheel velocities
    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

• 在八字形路径下，机器人要根据dummy点的位置，来变换角速度的方向

    -- Path 2: two circles
    vx = 0
    vy = dummy_guider_velocity_2
    target_pos_2  = sim.getObjectPosition(dummy_guider_handle_2, -1)
    if target_pos_2[1] > 0 then
        omega = dummy_guider_velocity_2 / circle_path_R 
    else
        omega = -dummy_guider_velocity_2 / circle_path_R 
    end

五、机械臂轨迹规划

5.1 Dummy在path上运动

• 增加一个circle path ，可以点击item shift，利用鼠标改变整个path的空间位置和姿态。
  
• 选中path后，点击path edit可以编辑path中的控制点，可以新增控制点、移动原有控制点，改变控制点的位置和姿态。
  
  
• 设置完path后，可以新增一个dummy点，将dummy点拖拽到path下方，为dummy点创建一个child script，设置dummy随这个path而运动。

function sysCall_threadmain()
    dummy_handle = sim.getObjectHandle('Dummy')
    path_handle = sim.getObjectHandle('Path0')
    print('start follow path')
    sim.followPath(dummy_handle, path_handle, 1, 0, 0.1, 1)
end

5.2 使用vrep自带的逆运动学模块

• V-REP自带的逆运动学（IK）模块不仅可以支持官方提供的模型，也支持用户自己设计的机器人模型，所以用户在设计好自己的机器人模型以后可以导入到V-REP中，定义好机器人模型连杆和关节的约束关系，构建好机器人模型的“树结构”，然后就可以使用V-REP自带的逆运动学模块了。在V-REP官方提供的模型当中，许多需要使用IK模块的机器人模型的关节已经被设定为IK模式了，例如YouBot机器人，因此我们可以直接使用。但是对于一个新设计的机器人模型来说，V-REP逆运动学模块的使用过程大致可以分为以下6个步骤：

1. 将所有参与逆运动学计算的关节设定为IK模式；

2. 添加target dummy、tip dummy；

3. 添加Path，并让target dummy跟随Path运动；

4. Link target dummy 和 tip dummy；

5. 添加IK group和IK element；

6. 添加script，开始仿真。