Motion planning with sequential convex optimization

Question

I recently read a paper titled Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization by John Schulman, Jonathan Ho, Alex Lee, Ibrahim Awwal, Henry Bradlow and Pieter Abbeel.

The authors mention that the end-effector final pose constraint can be readily incorporated in the planning scheme which is based on solving an unconstrained optimization with the equality and inequality constraints added in penalty function.

Let $F_{targ} \in $ SE(3) be the desired pose and $F_{cur}(\theta)$ be the current pose, then the pose error is given as $F^{-1}_{targ}F_{cur}(\theta)$. However, I am wondering that if we plan motion in the joint space, then how can this error be incorporated in the objective function as a penalty term?,since $F_{cur}(\theta)$ is a highly nonlinear, $nonconvex$ forward kinematics map, are we linearizing the forward kinematics map to make it convex and add it in the penalty formulation?

Answer 1

This isn't my area of expertise, but it looks like your guess in your question is correct. From the paper:

Let $F_{\mbox{targ}}$ denote the target pose of the gripper, and let $F_{\mbox{cur}}(\theta)$ be the current pose. Then $F^{-1}_{\mbox{targ}} F_{\mbox{cur}}(\theta)$ gives the pose error, measured in the frame of the target pose. This pose error can be represented as the six dimensional error vector $h(\theta) = (t_x, t_y, t_z, r_x, r_y, r_z)$ (26).

Earlier in the paper, it is stated, "

Sequential convex optimization solves a non-convex optimization problem by repeatedly constructing a convex subproblem — an approximation to the problem around the current iterate x. The subproblem is used to generate a step ∆x that makes progress on the original problem.

And, earlier still,

Robotic motion planning problems can be formulated as non-convex optimization problems, i.e., minimize an objective subject to inequality and equality constraints:

$$ \begin{array}{ll} \mbox{minimize } f(x) & (1) \\ \mbox{subject to} & (2) \\ g_i(x) ≤ 0, i = 1, 2, . . . , n_{\mbox{ineq}} & (3) \\ h_i(x) = 0, i = 1, 2, . . . , n_{\mbox{eq}} & (4) \end{array} $$ where $f$, $g_i$, $h_i$, are scalar functions.

[...later...]

The nonlinear constraints are incorporated into the problem as penalties, while the linear constraints are directly imposed in the convex subproblems.

So, in Algorithm 1, line 15 gives where the $h(\theta)$ error vector is incorporated:

$$ x \leftarrow \mbox{arg min}\tilde{f}(x) + \mu \Sigma_{i=1}^{n_{\mbox{ineq}}} \left|\tilde{g}_i(x)\right|^+ + \mu \Sigma_{i=1}^{n_{\mbox{eq}}} \left|\tilde{h}_i(x)\right| $$

I wish there were more detail or a worked example; I don't see where "TrueImprove" or "ModelImprove" is given, I don't see how the initial convex approximations are calculated, etc.

Answer 2

are we linearizing the forward kinematics map to make it convex and add it in the penalty formulation?

No. The reason is that "convex optimization" is not the solution it is the problem description like "minimize the function f(x) with constraints a,b and c". The same mistake made Emo Todorov in his paper A convex, smooth and invertible contact model for trajectory optimization. The trajectory optimization problem is described as logic calculus but that shows only that we have a np-hard problem which is not solvable on current hardware.