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(