我是跳过了正逆运动学的内容，可以参考1。
机器人手臂的运动规划是一个很宽泛的领域，包含有寻多子问题。比如，人们可能会关注机器人可以执行的最佳轨迹，以在最短时间内从某个开始姿态到某个最终姿态，这受制于致动器工作和关节运动范围的限制。另一种变形是根据驱动器的机械特性对手臂运动进行规划，比如提重物最省力的运动规划。常见的问题是机器人的各个关节从一个初始位置运动到目标位置过程中的避障问题。要执行的任务可能需要特定的末端执行器轨迹，例如， 如激光切割，密封剂分配或缝焊。 在这种情况下，终点的速度可能受到任务的限制（例如，对于最佳材料移除速率，焊枪速度或分配速率）。在要求苛刻的情况下，运动计划必须实时进行，例如， 产生动作去抓球。 在另一个极端情况下，可以离线计算用于刻板行为的一些运动计划（例如，将手臂折叠成紧凑的运动姿势），并将结果保存用于执行作为死记硬背的技能。 在通信延迟较大的情况下，监督控制可能是最好的方法。 在这种情况下，可以在提交远程系统执行批准的计划之前计划一个动作并在模拟中预览结果。

笛卡尔Cartesian运动规划

cartesian_planner 包包含了多种笛卡尔规划器的源码，包括arm7dof_cartesian_planner.cpp, ur10_cartesian_planner.cpp, baxter_cartesian_planner.cpp等为各自机器人定义的规划库。这些规划库的任务是沿着笛卡尔路径采样点，为路径上的每一个点的位姿计算逆运动学选项，找到完成笛卡尔路径的最优角空间序列。

通常，需要跟踪的笛卡尔路径是任务独立的。笛卡尔规划器展示了路径采样与计算吸引人的角空间轨迹的方法，但是他们的通用性都有一定的限制。这里描述的笛卡尔运动规划器有许多运动规划选项：

• 在角空间坐标中指定一个起始位姿和角速度，在笛卡尔空间中指定目标位姿。根据起始角度到目标姿态的逆运动学解计算关节空间的轨迹。
• 指定角空间中的起始位置和速度，确定笛卡尔空间中的目标位姿，通过沿着一条直线的采样点计算笛卡尔轨迹。这样满足末端抓手方向的轨迹可以很快获得，并且在现行运动过程中得到保持。
• 指定起始位姿、角速度和期望的笛卡尔单步长运动。采样的笛卡尔位姿将会从起始点位姿开始计算，沿着一条直线开始变换，并保持初始的方向不变。

• 指定笛卡尔起始点和终止点相对于基座的位姿。只考虑终点的方向。忽略起点的方向。在从起始位置到终止位置的直线运动过程中，保证固定方向的一系列位姿就能计算出来。

  example_arm7dof_cart_path_planner_main.cpp 节点显示了如何使用笛卡尔规划器arm7dofcartesianplanner.cpp，这个规划器依赖于相关的运动学库arm7dof_ki_ik和关节空间规划器的支持。同样的，也提供了Baxter机器人和Universal Robots UR10机器人的例子。
  运行roscore后输入下面的命令启动。会产生一个保存关节路径的文件arm7dof_poses.path

rosrun cartesian_planner example_arm7dof_cart_path_planner_main

对于这个例子，沿笛卡尔路径计算60个样本。 在每个采样点，逆运动学选项以基础接头0.1rad的增量进行计算。 这导致每个笛卡尔采样点大概有200个IK解决方案（在这个例子中，每个笛卡尔姿态的解决方案范围从130到285个IK）。
主要代码

// this will be an arm7dof planner, because we will link with this library
    CartTrajPlanner cartTrajPlanner; //instantiate a cartesian planner object

    R_gripper_up = cartTrajPlanner.get_R_gripper_up();

    //specify start and end poses:
    a_tool_start.linear() = R_gripper_up;
    a_tool_start.translation() = flange_origin_start;
    a_tool_end.linear() = R_gripper_up;
    a_tool_end.translation() = flange_origin_end;

    //do a Cartesian plan:
    found_path = cartTrajPlanner.cartesian_path_planner(a_tool_start, a_tool_end, optimal_path);

根据起始点和目标点进行轨迹规划，结果保存在optimal_path中。一共61行数据，保存在路径文件中。
每一行指明了七个关节的角度，是笛卡尔空间位姿的逆解。从所有逆解中找到最优的角空间值是有支持库提供的，将在下一部分提到。
例子中使用的规划库可以进行离线规划。另外规划库可以使用action server执行运动规划。这些将在后续提到。

角空间规划的动态规划

从逆运动学解中寻找最优角空间解是一项很有挑战的事。这里描述了一种角空间规划的动态编程方法，这种方法应用在了大量的机器人中，至少是在低维度的机器人中，比如七自由度的机器人。
从大量的选项中寻找最优角空间序列的方法是采用动态规划的方法。从概念上来解释考虑如下的前馈图（这个图是这样构成的：完成一条路径计划走多少步就有多少层，即多少列；每一个姿势即每一层有多少个解，那每一列中就有多少个元素）

这个网络由六层组成（六列），每一层都包括了一定数量的节点（每一列中的圆圈）。第l层的第m个节点由 $n_{l,m}$ 表示，它可能与第l+1层的第n个节点有连接，但是 $n_{l,m}$ 与其它层的节点没有连接（连接不会反向传播）。

这个网络中的一条路径从L1层的某个节点开始，在L6层中的某个节点结束，经过了路径上的全部六个节点。任何这样的路径都有一个相应的损失函数C，是所有的状态损失和变换损失的总和。状态损失是经过一个给定节点相关的损失，转换损失是从节点 $n_{l,m}$ 到节点 $n_{l+1,n}$ 的链路相关的损失，可以表示成 $C_{l,m,n}$ 。路径损失函数 $C$ 是标量，是所有的状态和转换损失的总和（在本例中，有六个状态和五个转换）。因此，一个备选路径可以路径损失函数来进行评分。要解决问题的最优目标是寻找到一条从L1层到L6层所需路径成本最小的路径。

对于关节角空间的规划问题，我们可以将每一层解释为沿着特定的笛卡尔路径采样得到的六自由度任务空间姿态。其目的是让工具法兰沿着这条路径运动，比如演示一个切的操作，我们可以沿着这条路径序列标记六个点。对于切的操作，我们没有必要跳过或者返回这些点。而且，每一个笛卡尔子目标点必须实现序列化。我们需要末端点到达每一个笛卡尔位姿。然而，通过计算逆运动学，我们可以找到有许多角空间解可以到达目标位置。我们在第l个笛卡尔位姿中可能会选择第m个角空间解，即节点 $n_{l,m}$ 。（依赖于所选择的采样分辨率，在每一个任务空间采样点或每一层中，我们都可能需要考虑大量的逆运动学可替代解）。在网络中的每一个节点都描述了机器人关节空间中的一个姿势。从每一层选择一个节点就描述了关节空间轨迹，这就保证了能够通过沿着任务空间轨迹采样的一系列笛卡尔空间点。

对于上文中提到的给定节点的状态损失函数可能通过多种评价标准来打分。比如，一个机器人的姿态可能会导致自我碰撞或与环境碰撞的话，就会被分配一个无穷大的损失值（或者，更简单的，从网络中移除）。一个节点如果接近碰撞，那么就会得到一个很高的状态损失值。接近奇异点的节点也会有一个惩罚值。其他依赖姿势的评价准则也会考虑其中。转换损失应该惩罚大幅度的运动。比如，一条路径包括从l层到l+1层移动基关节 $/pi$ 个弧度，这会在高惯性的关节产生很高的加速度……同样的，突然的腕部翻转也是不允许的，尽管两层的逆运动学解都是正确的，但是在层与层之间关节角度大幅度的变化都是不允许的。

不同的最优路径解决方案会导致不同的状态分配策略和转换惩罚方案。设计这需要考虑如何分配关节空间姿势的损失函数，如何分配转换损失，笛卡尔空间采样点的位置和数量，和每一个采样点逆运动学解的数量。我们已经把规划问题表示成了一个前馈图，可以采用图解决技术来整理我们的问题，产生一组关节空间序列来使机器人通过选择的笛卡尔空间姿势。

对于这类图，可以有效地应用动态规划的方法。过程如下。从最后一层向前反馈，每一个节点都分配一个损失值。对于最后一层，损失值只含有状态损失。

回到L-1层（本例中的L5层），这一层的每个节点都分配了一个损失值。比如L5层中的节点1， $n_{5,1}$ ，只有两个操作：以 $c_{5,1}+c_{5,1,1}+c_{6,1}$ 的成本变换到节点 $n_{6,1}$ ，或者以 $c_{5,2}+c_{5,1,2}+c_{6,2}$ 的成本转换到节点 $n_{6,2}$ ，对于节点 $n_{5,1}$ ，它的到达成本应该被分配成两种操作的最小值，记为 $c_{5,1}$ ， $n_{5,2}$ 节点的损失值也按同样的方式计算，记作 $c_{5,2}$ 。

继续向前，当计算到l层节点层损失值时，可以假设l+1层的成本损失值都已经计算得到了。为了计算l层的m节点的损失值 $c_{l,m}$ ，考虑l+1层的每个节点n，然后计算相关的 $c_{l,m,n}=c_{l,m}+c_{l+1,n}$ 。从这些选项中找到最小的损失值，赋给 $c_{l,m,n}$ 。在这个过程中，损失值计算了网络中的所有节点，并且计算这些成本在每一层都是相当有效的。

考虑到为每个节点分配成本的成本，通过网络可以通过遵循成本 $c_{l,m}$ 最陡的梯度来找到通过网络的最佳路径。

总之，角空间的动态规划就是要构建一个上方的图，该图的层数与所要规划路径长度和单步步长有关，比如我想规划一段从 $(1.5,0.3,1.5)$ 到 $(-1.5,0.3,1.5)$ 的路径，单步规划步长取为 $0.05$ ，3除以0.05有60段共计61个点，所以取规划图的层数为61层，每一个层也就是在每一个规划点处可能有n个解，每一个解对应一个节点（node），由于规划的机器人有七个自由度，所以每个节点的维数为7。规划过程中要先求出每一层中所有节点的值，然后根据节点静态及动态cost函数进行动态规划，从这样的一个图中找出一个最优解。

笛卡尔运动action server

……