python gis轨迹

参考资料：https://github.com/stulp/dmpbbo/blob/master/demo_robot/README.md

如何逐步训练和优化一个真正的机器人上的DMP

前提：你已经阅读了 Black Box Optimization of Dynamical Movement Primitives教程。

本教程将描述在真实机器人上培训和优化DMP所涉及的步骤。让机器人运行dmpbbo的最简单方法可能是复制这个目录cp -uva dmp_bbo_robot my_own_optimization并根据您的机器人和任务调整cpp和py文件。

你可以调用该文件夹里的demo_robot.bash脚本自动运行下面的所有步骤。

在本教程中考虑的任务中，机器人必须将一个球扔到一个特定的区域，如下所示。“机器人”用它的末端执行器做一个椭圆运动(蓝色轨迹)，在0.6秒后释放球(黑色圆圈)，使球在空中飞过(绿色轨迹)直到落地。目的是把球扔到一个特定的位置(“地板”上的绿色标记)。地板上的凹痕说明有误差幅度。

Step 1: Train the DMP with a demonstration

通常使用演示过的轨迹初始化DMP，这样优化就不必从头开始。假设优化算法是局部的，这样的初始化是必要的，以避免不解决任务的局部极小值。
这个初始化是用以下命令完成的(二进制文件来自step1A_trainDmpFromTrajectoryFile.cpp):

./step1A_trainDmpFromTrajectoryFile trajectory.txt results/dmp.xml results/policy_parameters.txt results/train/ 5

它从文件中读取轨迹(格式请参阅Trajectory class)，训练DMP，并通过boost::serialization将其保存到xml文件中。这个DMP的策略参数也存储在policy_parameters.txt中，这些策略参数将在后续优化步骤中进行优化。为了分析拟合过程和调整训练元参数，将中间拟合结果写入results/train/。这些可以画出:

python3 step1B_trainDmpFromTrajectoryFilePlot.py results/train/

step1A_trainDmpFromTrajectoryFile调用中的“5”是一个元参数，在本例中是基函数的数量。第一个图显示了每个DMP维度内函数近似的结果，其中黑点是输入数据，蓝线是近似值，竖直的红线是它们之间的差值(“残差”)。x和y轴用模糊的“输入”和“输出”加以注释。这是因为DMP内的函数逼近器不知道它拟合的是什么。

第三幅图显示了演示的轨迹和再现的轨迹(第二幅图显示了将训练好的DMP集成的更详细的结果)。所演示的轨迹是在上图中所示的椭圆运动。

在上面的结果中，拟合的不是很好，即输入数据与拟合函数相差很大。因此，我们也看到重现的轨迹与演示的轨迹不太吻合。这是因为5基函数不足以准确地拟合数据。所以我们把基函数的数目增加到10。

./step1A_trainDmpFromTrajectoryFile trajectory.txt results/dmp.xml results/policy_parameters.txt results/train/ 10
 python3 step1B_trainDmpFromTrajectoryFilePlot.py results/train/

得到如下结果:

这种拟合非常好，所以让我们用10个基函数。注意，20个基函数，拟合效果更好。但更多的基函数意味着更高维的搜索空间为后续的优化，因此缓慢收敛到(局部)最优。这是在选择基函数的数量时需要预料到的权衡。

Step 2: Define the task (i.e. cost function) and implement executing rollouts on the robot

定义任务需要创建一个继承自dmp_bbo.Task的类，并实现以下功能:
1.evaluateRollout(cost_vars,...).这是成本函数，它将成本相关的变量(cost_vars)作为输入，并返回与rollout相关的成本。因此，cost_vars定义了机器人在执行rollout时需要记录的变量，因为需要这些变量来计算成本。
2.plotRollout(cost_vars,...).这个函数可视化了一个rollout。
一个Python脚本例子是可用的，它将定义的任务写入一个目录。

python3 step2_defineTask.py results/

该任务将与成本相关的变量转换为成本。负责执行rollouts的机器人应该将成本相关的变量写入一个文件。因此，被使用者必须为机器人编写一个接口，该机器人读取dmp、执行dmp并将结果写入包含成本相关变量的文件中。在dmp_bbo_robot示例中，该接口是可执行的robotPerformRollout(由robotPerformRollout.cpp编译)。

./robotPerformRollout results/dmp.xml results/cost_vars_demonstrated.txt

执行rollout的结果可以可视化如下:

python3 plotRollout.py results/cost_vars_demonstrated.txt results/task.p

这使用Task中的plotRollouts(cost_vars，…)函数来绘制rollout。下面是它输出的一个示例。

对于这个任务的代价函数，唯一相关的变量是球的着陆位置和每个时间步上的加速度(加速度也会被削减)。但是在实践中，我通常在cost_vars中存储更多的信息以方便可视化，例如末端执行器轨迹和球轨迹。使用evaluateRollout(cost_vars，…)来计算成本不需要这些，但使用plotRollouts(cost_vars，…)当然有助于提供合理的plot。

在实践中，为cost-var收集信息并非小事一桩。以Meka机器人的球杯实验为例，机器人会记录末端执行器的位置，并在每个时间步长进行存储。球的轨迹通过外部摄像机记录下来，传递给机器人，机器人将其存储在与末端执行器位置一起的cost_vars矩阵中。

Step 3: Tune the exploration noise for the optimization

在随机优化过程中，DMP的参数将从一个高斯分布中采样(这些参数是通过DMP所继承的Parameterizable类设置的)。例如，请参阅step1A_trainDmpFromTrajectoryFile.cpp中的“set<string> parameters_to_optimization”代码)。这个分布的平均值将是通过监督学习对DMP进行演示培训而得到的参数。
这种分布的协方差矩阵决定了探索的规模。这个量级是用sigma定义的，其中协方差矩阵的对角线用sigma^2初始化。Sigma不能太低，否则探索的随机性可能小于机器人运动本身的随机性，无法进行学习。出于安全考虑，也不能太高;你的机器人可能达到加速度极限，关节极限，或环境中的意外。
您可以通过调用以下三个脚本来调优这个参数，以适应不同的探索强度:

SIGMA=0.1      # Exploration magnitude to try (start low!)
N_SAMPLES=10 # Number of samples to generate
# Generate samples with this magnitude
# This will save samples to directories 
#     results/tune_exploration_0.1/rollout001/policy_parameters.txt
#     results/tune_exploration_0.1/rollout002/policy_parameters.txt
#     etc
python3 step3A_tuneExploration.py results/policy_parameters.txt results/distribution_initial_covar.txt results/tune_exploration_${SIGMA}/ ${SIGMA} ${N_SAMPLES}
# Execute the Dmps with sampled parameters on the robot
./step3B_performExplorationRollouts.bash results/dmp.xml results/tune_exploration_${SIGMA}/
# Plot the rollouts to see the variance in the movements
python3 step3C_tuneExplorationPlot.py results/tune_exploration_${SIGMA}/ results/task.p

以下是sigma0.1、1.0、3.0和10.0的研究结果。值0.1可能太低了，因为在末端执行器的运动中几乎没有任何变化。10.0肯定太高了!如果你在你的机器人上执行这个，你就是一个比我更勇敢的人。如果你的勇敢弄坏了你的机器人，别怪我!)考虑到这些结果，我觉得值应该在1.0到3.0之间比较合适。在本教程中，我们将继续学习3.0，因为我们不能在模拟中破坏任何机器人。

如果你正在执行协方差矩阵的优化矩阵(CMA),即UpdaterCovarAdaptation,我会设置初始sigma为3.0,max_level 为3.0(这样探索不适应超过3.0)和min_level 0.3(为了避免过早收敛的一个维度)。这些参数在step4A_oneOptimizationUpdate.py中设置，我们下一个要讲。

Step 4: Run the optimization (step by step)

现在，我们训练了一个dmp(存储在dmp.xml中)，指定了任务(存储在task.p中)，并调优了探索(存储在distribution_initial_covar.txt中)。现在该运行优化了!这是一个迭代过程，有两个主要步骤(以及绘制中间结果的可选步骤)。每次迭代称为一次“更新”，因为它涉及策略参数的一次更新。

Step 4A: Update parameters

这是一个高度自动化的过程，其调用如下

python3 step4A_oneOptimizationUpdate.py  results/

这将自动查找最近的更新(例如results/update0083/)，并读取此更新目录中发布的所有cost_vars(存储在update0083/rollout001/cost_vars.txt, update0083 / rollout002 / cost_vars.txt等)。然后，它计算每个cost_vars(使用task.evaluateRollout(…))的成本，并更新策略参数。最后，它对新的策略参数进行采样，并将它们保存在一个新的更新目录中(即update0084/rollout001/policy_parameters.txt, update0084 / rollout002 / policy_parameters.txt等)
注意:在第一次调用时，该脚本只写入示例，但不读取rollouts，因为还没有rollouts。

Step 4B: Perform rollouts

在机器人上执行rollout操作与上面相同的./robotPerformRollout可执行文件。有一个方便的bash脚本

robotPerformRollouts.bash results/dmp.xml results/update00084/

循环所有rolloutNNNN/目录和每次调用./robotPerformRollout。最后,step4B_performRollouts.bash确定当前更新(例如update0084/)，调用robotPerformRollouts.bash与此目录

./step4B_performRollouts.bash results/

注意，步骤4B中的所有脚本/程序都是特定于您的机器人的。例如，您可能有一个实现策略的Simulink模型，而不是robotPerformRollouts.bash。您可能有一个python脚本或一些基于ros的解决方案。只要它坚持目录结构中的惯例，在update00084/目录中进行更新，在rollout001/中进行发布，从policy_parameters.txt中读取策略参数，并在这些目录中写入成本相关的变量cost_var .txt，一切都可以适用。

Step 4C: Plotting intermediate results

迭代地执行上面的两个步骤将导致(你可能会以某种方式编写此脚本)

python3 step4A_oneOptimizationUpdate.py results/ 
./step4B_performRollouts.bash results/
python3 step4A_oneOptimizationUpdate.py results/
./step4B_performRollouts.bash results/
python3 step4A_oneOptimizationUpdate.py results/
./step4B_performRollouts.bash results/

如果你对中间结果感兴趣，你可以用:

python3 step4C_plotOptimization.py results/

这将自动确定最后的更新目录是什么，并绘制到目前为止的优化过程，如下所示。左边的图表显示了每次更新后的评估展示，红色的是第一次，更多的绿色展示对应于最近的展示。第二个图显示了搜索空间的2维(在本例中2*10基函数为20D)。第三个图显示了每次更新时的探索幅度(sigma)。在这里，它衰减，衰减因子为0.9，这是在step4A_oneOptimizationUpdate.py中指定的。最后的图表显示了学习曲线。黑线对应的是评估推出的成本，它是基于高斯分布的更新平均值。细线对应不同的成本组成部分，在这里是到着陆地点的距离，以及加速的成本。最后，灰色点对应于优化过程中的每次铺开，即那些从高斯分布中采样的灰色点。

我们看到15次之后，“机器人”已经学会在指定的区域扔球。加速度稍微增加了，因为做这个动作需要比演示中的速度稍微高一些。