Mobile Manipulator Paper Reading

关于在读Mobile Manipulator Door Pushing Task中遇到的一些paper的note

主要关于coordinated motion planning的一些相关方法。

Difficulties

For door Opening Task:

1. How to represent a state in this door opening task? When this representation has high dimension, it’s hard to find a solution.
2. The optimal position or state to open the door is unkown.
3. Door open problem itself need to consider a lot of stuff e.g. collide, reachability…
4. identified the position of the door

For Coordinate Motion Planning

1. base need to consider dynamics! Especially for non-omnidirectional base.
2. how to desgin the State that represent the base in this motion planning task.

Task Summary

TODO:

1. RRTX: Asymptotically optimal singlequery sampling-based motion planning with quick replanning 2016
2. An asymptotically-optimal sampling-based algorithm for Bi-directional motion planning, 2015
   • 1~2 sampling based methods: SOTA
3. Coordinating locomotion and manipulation of a mobile manipulator 1992

Reading List:

1. Receding horizon control (RHC), also known as model predictive control (MPC) https://web.stanford.edu/~boyd/papers/code_gen_rhc.html

Paper1

1. impedence control
2. reactive controller
3. compliant control
4. constrained planner
5. graph Search
   1. https://zhuanlan.zhihu.com/p/54510444

Paper2

1. support mapping

Paper List

1. Planning for Autonomous Door Opening with a Mobile Manipulator 2010
2. Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization 2013
3. Safe and Coordinated Hierarchical Receding Horizon Control for Mobile Manipulators 2020
4. Perceptive Model Predictive Control for Continuous Mobile Manipulation 2020
5. CHOMP: Gradient optimization techniques for efficient motion planning. 2009
6. STOMP: Stochastic trajectory optimization for motion planning. 2011
7. Sampling-Based Methods for Motion Planning with Constraints (survey) 2018

Planning for Autonomous Door Opening with a Mobile Manipulator

Author: Sachin Chitta er al Paper Link

Journal/Conference: ICRA 2010

本文核心：找到一了一种相对有更低dimension的state representation（graph representation），这样可以使用一些图上的搜索算法（planning method）生成一个trajectory，从而达到motion planning的目的。

Introduction:

对于door opening这个问题本身就存在很多困难

1. it is hard to identify precisely the position and size of doors and handles

2. it is hard to autonomously compute the right approach to grasping and manipulating the handle

3. it is hard to compute a coordinated arm-base motion that opens the door wide enough for the robot to navigate through it.

本文主要针对解决第三个问题，即如何plan出一个手臂和底盘相互协作来开门的动作。对于第三个问题，如果简单的使用规定好的基于先验知识的motion来开门，会导致无法适应多种多样的门。而如果是用plan的方法，opening a door using a mobile manipulation platform typically involves tight coordination in between the motion of the base and the motion of the arm. This makes the problem high-dimensional and thus hard to plan for.

所以这篇paper的主要工作就是提出了一种对于机器人state的低维graph-based representation，在保证这种representation可以包含足够的信息（环境障碍物信息，机械臂reachability的信息等）的前提下，这种低维的、graph-based representation就可以使用在graph上的一些启发式搜索方法，把原有问题的planning变为一个在图上的搜索问题，使用一些例如A*的方法来生成一个基于这个低维state representation的trajectory，这就完成了上面所述的problem 3。因为这个低维state representation也包含了足够的信息，可以计算出真正所需的底盘和arm的motion.

Related Work:

一些方法使用例如impedence control、reactive controller、constrained planner等方法，其问题是没有使用planning的方法，而另一些使用planning的方法有没有考虑到碰撞的问题，而且以上方法只考虑了pushing, no pulling!

作者这篇paper提出的planning方法：既包含了pushing and pulling；而且还考虑了如何避免碰撞。

Hardware

使用的机器人PR2。全向底盘+7dof机械臂，主要使用的sensor为两个laser，一个用来为底盘提供周围的2D representation；另一个用来获取机器人前方的3D信息（如门的信息等）。

这里还提到了7dof这种redundant dof的好处，This proves useful in extending the usable workspace of the arm, especially in cluttered environments.

System Architecture

作者将整个door opening问题分为了两个子问题：

1. 检测门和门把手 —-> 使用现成的方法 见**[Reference 1]** 该方法需要知道门的一些先验信息，该方法的输出为：门和门把手以及hinge的位置信息。NOTE：门的旋转方向为一个先验知识！
2. 规划和执行开门的动作

对于第二个子问题即：规划和执行开门的动作，作者又将其进一步的分为了一些列子问题或者说是一个解决这个问题的流程：

1. 往门的方向移动机器人，目标是可以接触到门把手
2. 伸出手臂抓住门把手，轻微的unlatch（这个过程底盘不动）
3. 由模型决定（先验知识）决定这个门是应该拉，还是应该推；并轻微的拉开or推开门
4. 规划一个手臂和底盘协同的motion来开门。这个规划的动作是手臂与底盘配合着来的，而且考虑了避免碰撞
5. 如果是使用pulling的方法，检测门和手臂的碰撞。如果可能会发生碰撞，就会换一边，转而抓住门把手的另一侧。（这个动作中底盘不动）
6. 如果没有完全打开门，则之后基于5继续执行一个pushing开门的planning

这6个子问题又可以分为两类，

• the act of reaching out and grasping the handle and opening the door slightly （1~3）
• the act of opening the door using a coordinated motion of the base and the arm of the robot. （3~6）

前一个是使用现有的解决方案：**[Reference 2]**

本文主要是解决第二个问题，即如何生成一个base & arm 协同开门的motion。

简单的分析一下这个子问题，执行一些列开门动作时，有一个重要的约束，即：gripper必须要抓着门把手。

另外一个难点就是：goal position是不确定的！在执行动作的时候，并没有一个确定的最优的goal，而是仅仅有一个要找到能打开门的position的这个目标。

如果想实现一个对于全部configuration space的planner（底盘运动+7 dof arm），这就需要一个constrained planner，而这类constrained planner在处理高纬度有约束的情况下是非常困难的。

所有作者有了本文的核心方法，也是main contribution，即：设计了一个lower-dimensional graph-based representation，基于这个就可以比较轻松的找到一个合理的motion了。

整体思路：

• 将高维的问题化简为先在平面（即只考虑底盘）上规划，找到一个手臂可以接触到门把手且比较合理的位置
• 之后使用IK计算arm的trajectory，并与base一起运动，这其中要保持手臂可以一直接触到门把手这个约束

本质上就是将三维的规划问题变为了一个2D 规划+IK 的问题。

Coordinated Arm-Base Motion Planning

整个Coordinated Arm-Base Motion Planning包含如何构建一个低维的Graph Representation，如何在这个graph上搜索，基于这个结果如何生成Coordinated Arm-Base Motion。

再次明确一下这个planning的目标：

Note that the final goal for the base motion is not specified. The goal for the entire task, however, is to open the door.

Graph Representation

总结来说，就是要将原问题变为一个在图上的搜索问题。所以需要定义好每个state是什么，state之间的转移情况，一起每个state的cost。

State Variables

明确state Variables: $s=(x,y,\theta,d)$

x,y 为底盘的位置，$\theta$为base的朝向。如果只含有这三个，显然无法包含我们的goal的信息，所以需要额外增加state的维度来包含这个信息，作者增加的额外一维即为d，即：using a single binary variable that we call door interval.

下图为一个关于 door interval的解释。

所有门可能到达的角度（前提是不与机器人发生碰撞）被分为了两个区间，如果门处于这两个区间中的一个，那么d这个变量就是那个对应的区间。可见这个d就表明了目前门所在的大致位置。

设置一开始门关闭的时候位于intervel 0，如果门移动到了intervel 1，那么就完成了我们open的目标。

并且基于这个$s=(x,y,\theta,d)$，也可以计算出可行的门打开的角度（因为要满足机器人抓住门这个约束），即这些角度就是机器人手可以到达的地方所对应的角度，而且不会发生碰撞，对于每一个state s这一系列角度的集合被称为：$\Lambda(s)$。作者对这个角度集合做了分度为1度的离散化。这种离散化的好处是极大的减小了搜索空间。

此时就可以定义一个state-space了，这个state-space只包含有非空$\Lambda(s)$的s。

所以用定义好的state来描述目标就是：A state s is a goal state if it belongs to interval 1 and if Λ(s) contains an angle that is closer than a threshold $\delta_d$ from the fully open door angle.

下面就定义state之间如何进行Transitions。

Transitions

作者在构建Transitions的时候，使用了lattice-based planning representation的方法**[Reference 3&4]**。

这种lattice-based representation是对于C-space的一种离散化，将c-sapce离散化为了一些列state，并将这些state用一些可行的、short-term的action相连接。下图为一个示例：

这种lattices就将motion planning变为了一个graph search的问题。During the search (planning), these actions are translated and rotated for each state encountered by search, and the successors of the state are computed as the corresponding end configurations of these actions. The planner also checks all actions against collisions by checking the footprint of the action (constructed from the footprint of the robot) against the map of the environment.

这里要额外明确好如何定义s中的d的transition。

• 如果$d(s)\ne d(s^{‘})$, 想要发生transition必须有：$\Lambda(s)\cap \Lambda(s^{‘}) \neq \varnothing$且x,y,θ必须相等。也就意味着：the robot is out of the way of the door and can reach the door handle
• 对于$d(s)=d(s^{‘})$，就相对简单了，只需要as the robot executes any action in our lattice, it will be able to maintain its gripper on the door handle.

Cost Function

planner使用的cost function是一个包含了2D cost（表示了base到最近obstacle的距离）和一个基于arm位姿的cost（包含了该位姿是不是出于一个比较舒服的，即：每个joint的position不接近limit的位置）

2D Costmap

将三维投影至2D来构建一个2D costmap，具体方法见：**[reference 5]**。其结果代表了离最近obstacles的距离

Arm-based cost

其表示了每个joint的position不接近limit的位置，具体计算过程见paper

Graph Search

使用Anytime Repairing A* (ARA*)算法见**[reference 8]**。值得注意的是，这个方法相比于A star算法，额外考虑了时间因素，所以有时无法获得最优解（不是最低的cost），但仍然给出了一个关于该解与最优解关系的下界。

Implementation

Arm planning Using: [Reference 7]

Reference

1. Laser-based perception for door and handle identification 2009
2. Autonomous Door Opening and Plugging In using a Personal Robot 2010
3. Generating near minimal spanning control sets for constrained motion planning in discrete state spaces 2005
4. Planning long dynamicallyfeasible maneuvers for autonomous vehicles 2009
5. The office marathon: Robust navigation in an indoor office environment 2010
6. A formal basis for the heuristic determination of minimum cost paths（A star算法）
7. Combining Planning Techniques for Manipulation Using Real-time Perception 2010
8. ARA*: Anytime A* with provable bounds on sub-optimality 2003

Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization(trajopt)

Author: John Schulman er al

Journal/Conference:

本文核心：本文本质来讲是提出了一种进行机器人motion planning的考虑了碰撞的trajectory optimization方法。

• 使用了sequential convex optimization procedure（一个原本非凸的问题，变为顺序的解一系列凸的子问题）
• 设计了一种no-collisions constraint that directly considers continuous-time safety，与上一点结合就可与解决许多motion planning方面的问题

Introduction

此note的第一篇paper给出了一种生成motion trajectory的方法，而本文的方法更倾向于优化这些已经生成好的trajectory，即：Trajectory optimization algorithms。Trajectory optimization algorithms在机器人的motion planning领域内起到了以下作用：

• they can be used to smooth and shorten trajectories generated by some other method.
• they can be used to plan from scratch: one initializes with a trajectory that contains collisions and perhaps violates constraints, and one hopes that the optimization converges to a high-quality trajectory satisfying constraints.

对于这motion planning的trajectory optimization方法有以下两个关键的组成部分：

• numerical optimization method 本文使用：sequential convex optimization，并使用$l_1$penalties来讲不等式约束和等式约束加入到objective里面
• the method of checking for collisions and penalizing them 本文使用：signed distance，并且考虑了continuous-time safety

Related Work

一下均为一些trajectory optimization的方法。

1. Practical methods for optimal control and estimation using nonlinear programming 2010

Trajectory optimization在optimal control中的作用

2. CHOMP: Gradient optimization techniques for efficient motion planning. 2009
3. CHOMP: Covariant hamiltonian optimization for motion planning. 2010
4. STOMP: Stochastic trajectory optimization for motion planning. 2011

Sequential Convex Optimization

本质上就是将一个non-convex optimization problem变为一系列凸的问题来处理，可能无法找到optimal，但一般可以到达一个local optimal。

一个机器人的motion planning问题可以被定义为一个non-convex optimization problems

对于kinematic motion planning problems，状态变量x的维数应该为：T×K，T为time-step的个数，K为dof。

所以optimization parameters可以写为: $\theta_{1:T}$，其中$\theta_{t}$表示在时间t时刻，机器人的configuration

首先确定objective，这个objective的目的是：to encourage minimum-length paths, we use the sum of squared displacements

对于对于不同的任务有不同的constraints，比如对于开门任务需要保证无碰撞，且end-effector一直与门把手相接触。

但是这个原本的优化问题仍然是非凸的，所以使用Sequential Convex Optimization来解决，本质上来就就是将原问题构建为一系列子问题，每个子问题是在当前state对于原问题的一个估计。每个subproblem的目的是generate a step $\Delta x$ 来让original problem变好一点。

其两个核心的部分如下：

1. a method for constraining the step to be small, so the solution vector remains within the region where the approximations are valid –> 使用trust region，本质就是一个bound加载state变量上
2. a strategy for turning the infeasible constraints into penalties, but eventually ensuring that all of the constraint violations are driven to zero. –>使用$l_1$penalties。即对于每个不等式约束，变为$|g_i(x)|^+, |x|^+=max(x,0)$；对于每个等式约束，变为$|h_i(x)|, |x|^+=|x|$；之后将这两个penalties乘以系数$\mu$加到objective上即可。

整体的算法如图，大体思路就比较明确了，就是通过一次次循环增加$\mu$的值，最终使得结果满足objective，而且在迭代的子问题（由原问题通过一个近似得到，即将非线性的约束做一个一阶近似）也是一个凸的问题。更多的细节见paper

至此，已经确定了如何解这样一个优化问题，下一步就是确定constraint了。

Discrete-Time No-Collisions Constraint

本质来讲，这个constraint就是让所有的刚体的距离都在一定范围之外。对于每一个time-step都应该满足。这个sd( , )就是文中的signed distance，本质来就是如果两个刚体没有碰撞，那么这个距离就大于0，且相聚越远值越大。

变为penalties后为：

可见这样的话计算量极大，左右使用了一些方法删去了部分不可能碰撞的pair，降低了复杂度。更多关于如何高效计算signed distance和减低penalties计算复杂度方法的细节见paper。

ENSURING CONTINUOUS-TIME SAFETY

使用上面所介绍的这个No-Collisions Constraint就可以优化这个discretely-sampled trajectory，让其无碰撞。这些trajectory上的waypoints需要转变为一个continuous-time trajectory，直观的可以使用线性插值，但这种可能会导致continuous-time trajectory发生碰撞，如下图所示

为了解决这个问题，引入swept-out volume需要再将penalties做一个修正，swept-out volume如下图所示，是一个当前时刻的位置与下一个时刻位置的一个convex hull

然后将上一部分的signed distance变为如下的形式即可

这个计算起来也比较复杂，文中有详细描述了如何简化这个的计算。

NOTE: 作者在implement part也说明了如何设置例如开门任务的constraint

Reference

1. A fast and robust GJK implementation for collision detection of convex objects. 1999 关于swept-out volume

Safe and Coordinated Hierarchical Receding Horizon Control for Mobile Manipulators(HRHC)

Author: Jessica Leu er al

Journal/Conference: American Control Conference 2020

本文核心：本文针对的情况主要是time-varying dynamic environments

Introduction

开头有一段很好的关于literature的一个简单的review，其中提到了Reference中的几篇paper，值得根据这个继续阅读。

以前的这些方法的问题是他们一般是基于一个static environment，对于time-varying的环境并不适用。可以进一步通过加入prior knowledge来降低计算量，但显然这种基于prior knowledge的方法的使用范围还是有限的。

optimization methods受高维数数据影响更少，所以更适合用于这种coordinated motion planning.

本篇文章的核心目标是：

• 更高效。planning 所需时间更短
• 可让算法适用于更多的场景。

算法分为：A high level motion planning module用于解决motion planning问题（求解非凸的优化问题）& a low level safety controller运作在一个高频率下，保证当前的action是安全的。

Problem Formulation

主要介绍了这两部分：

• Formulation ofthe motion planning optimization problem 主要说明了一些notation和使用什么样的cost function
• Convex Feasible Set Algorithm（CFS），如何快速求解一个motion planning（non-convex problem）
• 以及如何设计constraint（Constraints formulation），保证碰撞安全的约束使用capsules（代表link）之间的距离来计算

Hierarchical Receding Horizon Control (HRHC)

在上层的motion planning部分主要使用CFS，额外设计了Soft constraints。即：引入松弛变量。

下层额外加入了Low-level safety controller，因为上层生成的无碰撞轨迹的生成时间相比于环境的变化时间较长，所以决策系统上一时刻生成的轨迹可能在此时并不适用，甚至会发生碰撞，所以需要一个更新更快的底层安全控制器来保证系统的安全性。实现起来本质是使用了一个更粗糙的碰撞检测。

Reference

1. S. M. LaValle, Planning algorithms. Cambridge university press, 2006.
2. Search-based planning for manipulation with motion primitives 2010
3. “Optimization techniques applied to multiple manipulators for path planning and torque minimization,” 2002
4. “Local motion planning for collaborative multi-robot manipulation of deformable
   objects,” 2015
5. “Efficient kinematic planning for mobile manipulators with nonholonomic constraints using optimal control,” 2017 VITAL
6. “Stomp: Stochastic trajectory optimization for motion planning,” 2018
7. “Synthesis and stabilization of complex behaviors through online trajectory optimization,” 2012 iterative LQR

Perceptive Model Predictive Control for Continuous Mobile Manipulation

Author: Johannes Pankert er al

Journal/Conference: IROS 2020

本文核心：本质上是一个给定预期end-effector trajectory（基于task space or Cartesian Space），产生每个关节控制量（速度）的controller；can control interaction-forces of a mobile robot without torque-controllable joints，

Introduction

主要针对，Continuous manipulation task，例如檫黑板，cleaning这些workspace超出一个固定机械手的workspace.

Model Predictive Control(MPC)

MPC是一种使用优化方法的controller，其考虑了约束的影响。

Sequential Linear Quadratic Model Predictive Control(SLQ method)

简介：本质上还是一个求解optimal Control的方法。对于系统的dynamic进行在当前state和input trajectory进行linearized。之后对于cost function做一个Quadratic approximate。之后求解出一个affine的控制策略

详细见Reference。

System Model

这里的state就是底盘的位姿（twist中的translation和rotation，rotation用qua）每个arm的转角θ。

Cost Function

主要包含了三部分

• Task space tracking error：目标end effector的pose和预期pose间的差距。这个error又分为：translational error（用position vector之间的差距来计算）& orientation error（用四元数计算）

end-effector pose的计算使用：Robcogen[Reference 1]，本质上就是求forward Kinematics

因为使用SLQ想要收敛的话，需要二阶导数项半正定，而一般forward Kinematics的二阶导未必半正定，所以使用Gauss-Newton来近似二阶导

• Soft Constraints: （不等式约束）使用Relaxed Barrier Functions（RBF）
• Collision Avoidance：本质上也是一种不等式约束，使用RBF函数。在内部函数里使用：球体来近似robot的link，并query Euclidean Signed Distance Field (ESDF)，并且额外引入cache gradients along with the signed distances，来加速计算（结果见paper的实验部分）。最终的约束如下式所示：FK为forward Kinematics。

生成Euclidean Signed Distance Field (ESDF)使用Voxblox[Reference 2]。

Querying cached distances and gradients from a euclidean signed distance field(ESDF) 可以有更高的执行效率。

ESDF的细节见[Refernce 3,4]

Task Space Admittance Control

Detail: TODO

可以实现力的控制！但需要力传感器和给定的力的大小。

Mechanical Stability

额外增加了一个SLQ问题的constraint（using Zero Moment Point (ZMP)）

Detail: ????

Reference

3、4是关于Euclidean Signed Distance Field (ESDF)的

1. RobCoGen: A code generator for efficient kinematics and dynamics of articulated robots, based on Domain Specific Languages 2016

2. “Voxblox: Incremental 3D Euclidean Signed Distance Fields for onboard MAV planning,” 2017

3. CHOMP: Gradient optimization techniques for efficient motion planning. 2009

4. STOMP: Stochastic trajectory optimization for motion planning. 2011

CHOMP: Gradient optimization techniques for efficient motion planning

Author: Nathan Ratliff er al

Journal/Conference: 2009

本文核心：continuous path refinement methods & standalone motion planner；使用covariant gradient来优化采样得到的trajectory；基于configuration space

Q:

1. covariant gradient descent
2. finite differencing operator/matrix
3. functional gradient？（泛函梯度）

Introduction

CHOMP的核心就是利用covariant gradient来优化采样得到的trajectory。基于sample-based methods改良而来。sample-based methods主要包含以下两个步骤：

• first find a feasible path（例如RRT：建树+A*搜索）
• optimize it to remove redundant or jerky motion（RRT*等）

第二个步骤实际上是一种trajectory optimization，之前比较流行的方法是：

• 使用一种 shortcut heuristic [Reference 1]
• 或者使用elastic bands or elastic strips planning involves modeling paths as mass-spring systems [Reference 2]

CHOMP(Covariant Hamiltonian Optimization for Motion Planning)的特点是：

• 不需要提前输入一个collision-free的path（以前的方法需要），可以将一个不可行的trajectory变为一个local optimal的解
• 使用covariant gradient update rules（local optimal guarantee）

THE CHOMP ALGORITHM

一个核心思想是：the proper use of geometrical relations, particularly as they apply to
inner products. 尤其在：differential geometry？

Covariant gradient descent

Goal：find smooth, collision-free, trajectory through the configuration space between two prespecified end points

Cost Term:

包含两项：obs包含了障碍物信息（描述了离障碍物的距离）；prior: measures dynamical quantities of the robot such as smoothness and acceleration. $\xi$代表trajectory

目标是在每次iteration都通过最小化一个描述trajectory smoothness的function的local approximation。我们的cost就是这个function。

其中：，A是一个对称正定矩阵

在第k步，用一阶泰勒展开来近似目标函数，即：

，其中

所以update rule就是：

即：

Understanding the Updata Rule

作者指出这个update rule是covariant gradient descent的一个特殊形式。（covariant gradient descent 见[**Reference 3,4 **]）

TODO

Obstacles and distance fields

首先要说明：

这里使用计算距离obstacle的方式是：**signed distance field d(x)**。这种方法对于计算static的环境是很有效的，其本质上就是需要提前计算空间中每一个点到最近障碍物的距离。d(x) x为空间中一点，如果x在障碍物内部，d(x)为-1，外部d(x)为1，边界为0。计算d(x)的方法为：利用EDT(Euclidean Distance Transform)。

同时将机器人的身体简化为一些列圆（或简单图形），这样可以方便的计算安全距离。

有了这些，就可以得到**workspace potential function c(x)**，其用来penalizes points of the robot for being near obstacles.

例如：

或更平滑的：

Defining an obstacle potential

现在已经有了workspace potential function c(x)，这个c(x)是对于robot上一点的描述，而为了对整个机器人产生一个cost，就需要结合机器人各个位置的c(x)。最直接的方式就是对于时间积分，但这样可能会产生让机器人在障碍物区域高速移动的趋势。

所以选择使用

最终的loss为：其中u为机器人身上的一点，B为机器人全部位置的点集。本质上来讲是对workspace的arc-length做积分。积分只与速度项和workspace position有关。

Smooth Projection for Joint Limits

一般处理joint limit的时候，有以下两种方法：本篇paper选用后者。

1. 额外加入一个potential term（作为罚项，防止joint value接近limit）

2. 当违背极限时，project到安全的区域。

这里的projection是关于A matrix的norm（A matrix定义在Covariant gradient descent这一小节中）。即：如果超过limit则截断至limit，然后乘上A的inverse。这个transform的作用是起到smooth的效果。

Experiments on Robotic Arm

稍微修改了Collision heuristic（即稍微修改了workspace potential function c(x)的计算方法），很核心的一个参数就是A矩阵的选取。

Reference

1. L. Kavraki and J. Latombe. Probabilistic roadmaps for robot path planning. Practical Motion Planning in Robotics: Current Approaches and Future Directions, 53, 1998.
2. O. Brock and O. Khatib. Elastic Strips: A Framework for Motion Generation in Human Environments. The International Journal of Robotics Research, 21(12):1031, 2002.
3. J. A. Bagnell and J. Schneider. Covariant policy search. In Proceedings ofthe International Joint Conference on Artificial Intelligence (IJCAI), August 2003.
4. M. Zlochin and Y. Baram. Manifold stochastic dynamics for bayesian learning. Neural Comput., 13(11):2549–2572, 2001.

STOMP: Stochastic trajectory optimization for motion planning

Sampling-Based Methods for Motion Planning with Constraints (survey)

Intro

motion planning需要考虑无碰撞的条件下，如何产生从start configuration 到 goal configuration的路径。

而constraints motion planning则需要在满足约束的情况下（如：保持end-effecort的方向一只朝上）完成motion planning。

Constrained sampling-based methods are based upon two core primitive operations:

1. sampling constraint-satisfying configurations
2. generating constraint-satisfying continuous motion

Motion Planning is a PSAPCE-hard problems, 复杂程度随空间维数的上升而上升。

Motion Planning的历史回顾

1. potential field methods 缺点：很难跳出local mimum
2. 另外一类就很接近search based methods。 这类方法很重要的一点在于A careful choice of resolution and heuristics is critical for efficient heuristic search
3. 基于优化的方法（或者也可也看为对于整个trajectory的一种优化）。存在的问题在于tuning the penalty functions to avoid local minima and avoid paths that go through thin obstacles (as obstacle avoidance is relaxed) is challenging for robots with many degrees offreedom in complex scenes.
4. Sampling-based algorithms。Sampling-based methods可以很方便的与Constraints相结合，constraints can be easily incorporated into the core ofa sampling-based algorithmwithout affecting its method for solution finding.

关于约束：

常见的就是Crave Tracking。可以用IK来求解，但其问题在于无法保证path continuity。

整体来说，geometric constraints increased the difficulty of the motion planning problem and required additional consideration for effective planning.

MOTION PLANNING AND CONSTRAINTS

NOTE: To dive deep, may need differential geometry（微分几何）相关的知识。

假设约束为完整约束，这个约束有K的等式，可以降低原来configuration space的维数（降低了m维），原有维度为n（n=m+k）。

这种情况下的约束也可以称为：几何约束（geometric Constraints）

约束形成了一个：The constraint function defines an m-dimensional implicit constrained configuration space within the ambient configuration space

所以motion planning with Constraints的这类问题可以写为：（manifold：流形）

Constraint Expression

最常见Constraints就是end-effector constraints. 一些表示方法比如：Task-Space Region (tsrs) formulation。Task-Space Region Chains (tsrcs)等。TSRCS是一种对于close-chain robot的扩展，也可以表示更多种类的机构。

已有许多方法将Constraints表示为F(q)的形式，例如：最通用的方法是不假定约束函数具有与机器人的运动学结构有关的任何属性，并假定该函数编码到约束流形表面的“距离”，

Constraint Composition

一种处理Constraints的方法就是Constraint Composition，将多个Constraints变为/编码为一个Constraints（相当于取并集），降低处理的难度。paper中列出了一些方法。即：Howmultiple constraints are encoded。

另外一种情况是，如果任务也是一种composition，且有多模态的性质。例如：间歇性接触（Intermittent contact）是操纵和有腿运动的重要组成部分，需要不断增加和消除约束。所以需要可以处理Constraints之间取交集的情况。这里也有一些解决方法，例如：利用图来处理different constraints之间的切换；也可以看为一种hierarchical的处理，先用离散度图结构判断处于哪个constraint modality，之后再用上面的geometric constrained planning即可。

SAMPLING-BASED MOTION PLANNING

The general idea behind sampling-based planning is to avoid computing the free space exactly, and to instead sample free configurations and connect them to construct a tree/graph that approximates the connectivity ofthe underlying free space.

一般是：probabilistic completeness的。即：if a solution exists, the probability of finding a path goes to 1 with the number of samples generated by the algorithm. 最常见的还是两类：PRM（graph、mutil-query）和RRT（tree、single-query），当然还有一些其他的tree-based methods，例如：EST。但这些方法都有一些相同的components，这里列出了部分：

1. Samplers。

最常见的就是uniform，但是也有一些新提出的sampler使用了不同的heuristics，例如：proposed to sample (approximately) in lower dimensional spaces to improve the odds of sampling in narrow passages, which is key to solving the motion planning problem.

2. Metrics & nearest-neighbor data structures。即：The choice of distance measure

3. Local planner

local planner用于产生一个相邻configurations间的motion。

4. Coverage estimates：还可以将高维的c-space投影至一个低维的空间（定义一个投影函数），之后看该空间的采样密度，来判断整个算法的收敛程度。

由Planner产生的path可能不是最优的，此时就需要一些额外的处理。例如：local的post-process。

asymptotically optimal: the solution path will converge to the globally optimal solution over time 。

值得注意的是：asymptotically optimal也可以扩展至考虑非完整约束的情况（kinodynamic planning）

SAMPLING-BASED MOTION PLANNING WITH CONSTRAINTS

Challenges

相比于之前列出的geometric planning，加入Constraints后会变得更加复杂，几个基本组件也需要满足一些新的要求：

1. Samplers: A planner needs to (1)sample configurations that satisfy the constraint function (solutions to F(q) = 0) (2)guide the search towards valid regions of the space.
2. Metrics & nearest-neighbor data structures: 正常情况下的两个configuration间的距离可以用C-space内的距离（例如欧氏距离来描述），而引入Constraints后，这时满足约束的configuration space就变为了一个原空间内的流形，而描述流行上的距离直接使用欧式距离可能并不是一个很好地解决方法，此时可以使用例如：Riemannian metric, as the constraint function usually defines a Riemannian manifold.
3. Local planner: 此时遇到的问题于2相似，都是由于在流行上进行直接差值比较困难
4. Coverage estimates: 目前（2018年前）并无考虑这个的Constraints motion planning problem

其中最主要的可能就是sampler和local planner了（不同的算法也主要在这上面有所不同），目前最常见的metric是：ambient configuration space‘s semi-metric。

Methodology Overview

列出的方法按复杂度由低到高的顺序列出：

1. Relaxation: 该问题的一个很大的困难就是：inability to find satisfying configurations，这个是由于满足约束的空间的lower dimension导致的。一种处理方法就是做如下的Relaxation

   

2. Projection: A projection operator takes a configuration and projects it onto the surface of the implicit manifold, iteratively retracting the point to a minimum of the constraint function and solving a linear system of equations at each iteration.

3. Tangent space: From a known satisfying configuration, a tangent space of the constraint function can be generated. The tangent space is constructed by finding the basis for the nullspace of the derivative of the constraint function. Satisfying configurations and valid local motions can be generated within the tangent space.

4. Atlas: Instead of recomputing tangent spaces at all points when an expansion is needed, the tangent spaces can be kept and composed together to create a piece-wise linear approximation of the manifold, which can then be readily used for sampling satisfying configurations or computing geodesics for local planning. This is called an atlas, in a slight abuse of terminology from differential geometry.

5. Reparameterization: For certain constraints, it is possible to compute a newparameterization of the robot’s configuration, allowing direct sampling of constraint-satisfying configurations. Using the reparameterized space, a new configuration space can be generated or a local motion computed and then mapped back into the robot’s previous configuration space.

TODO:

Randomized kinodynamic planning

Sampling-based algorithms for optimal motion planning