Optimal shape and motion planning for dynamic planar manipulation

Abstract

This paper presents a framework for optimizing both the shape and the motion of a planar rigid end-effector to satisfy a desired manipulation task. Both shape and motion play key roles in determining contact interaction, and while both are commonly seen as design/control freedoms, their synergies are rarely formally explored. In this paper we study quasistatic problems like cam design, or dynamic problems like ball throwing, where both the shape and motion of the surfaces at contact are relevant. We frame this design problem as a nonlinear optimization program, where shape and motion are decision variables represented as splines. The task is represented as a series of constraints, along with a fitness cost, which force the solution to be compatible with the dynamics of frictional hard contact while satisfying the task. We illustrate the approach with the example problem of moving a disk along a desired path or trajectory, and we verify it by applying it to three classical design problems: the rolling brachistochrone, the design of teeth of involute gears, and the pitch curve of rolling cams. We conclude with a case study involving the optimization and real implementation of the shape and motion of a dynamic throwing arm.

Appendices

A Collocation details

1.1 A.1 Motion variables

Let’s consider a motion variable $\xi (t)$ represented using N evenly spaced basis function, in a nonlinear program that makes use of $M+1$ evenly spaced collocation points. If the time length of the trajectory is $\tau$, then the jth collocation point corresponds to time $t_j=\frac{\tau }{M}j$. As described in Sect. 5, $\xi (t)$ equals:

$$\begin{aligned} \xi (t) =\sum _{i=1}^{N} \alpha _i \varPhi \left( L(t) - i + 2\right) \end{aligned}$$

(23)

where

$$\begin{aligned} L(t)&= (N-3)\frac{t}{\tau } \end{aligned}$$

(24)

$$\begin{aligned} \dot{L}(t)&= \frac{N-3}{\tau } \end{aligned}$$

(25)

$$\begin{aligned} \ddot{L}(t)&= 0 \end{aligned}$$

(26)

For the sake of completeness, we list the definition of $\varPhi (x)$ and its derivatives:

$$\begin{aligned} \varPhi (x)&=\left\{ \begin{array}{rrcl}0 |&{}-\infty &{}\le x\le &{}0\\ \frac{1}{6}x^3+x^2+2x+\frac{4}{3}|&{}-2&{}\le x\le &{} -1\\ -\frac{1}{2}x^3-x^2+\frac{2}{3}|&{}-1&{}\le x \le &{} 0\\ \frac{1}{2}x^3-x^2+\frac{2}{3}|&{}0&{}\le x \le &{} 1\\ -\frac{1}{6}x^3+x^2-2x+\frac{4}{3}|&{}1&{}\le x\le &{} 2\\ 0 |&{}2&{}\le x\le &{}\infty \end{array} \right. \end{aligned}$$

(27)

$$\begin{aligned} \dot{\varPhi }(x)&=\left\{ \begin{array}{rrcl}0 |&{}-\infty &{}\le x\le &{}0\\ \frac{1}{2}x^2+2x+2|&{}-2&{}\le x\le &{} -1\\ -\frac{3}{2}x^2-2x|&{}-1&{}\le x \le &{} 0\\ \frac{3}{2}x^2-2x|&{}0&{}\le x \le &{} 1\\ -\frac{1}{2}x^2+2x-2|&{}1&{}\le x\le &{} 2\\ 0 |&{}2&{}\le x\le &{}\infty \end{array} \right. \end{aligned}$$

(28)

$$\begin{aligned} \ddot{\varPhi }(x)&=\left\{ \begin{array}{rrcl}0 |&{}-\infty &{}\le x\le &{}0\\ x+2|&{}-2&{}\le x\le &{} -1\\ -3x-2|&{}-1&{}\le x \le &{} 0\\ 3x-2|&{}0&{}\le x \le &{} 1\\ -x+2|&{}1&{}\le x\le &{} 2\\ 0 |&{}2&{}\le x\le &{}\infty \end{array} \right. \end{aligned}$$

(29)

As a quick reminder, this choice of $\varPhi (x)$ is special because:

• It is twice differentiable

• It vanishes outside the interval $[-\,2,2]$

• It is constructed out of 3rd degree polynomials (which is the minimum degree required for it to be twice differentiable)

• It has evenly spaced knot points.

Differentiating, we find expressions for $\dot{\xi }(t)$ and $\ddot{\xi }(t)$:

$$\begin{aligned} \dot{\xi }(t)&= \sum _{i=1}^{N} \alpha _i \dot{\varPhi }\left( L(t) - i + 2\right) \left( \frac{N-3}{\tau }\right) \end{aligned}$$

(30)

$$\begin{aligned} \ddot{\xi }(t)&= \sum _{i=1}^{N} \alpha _i \ddot{\varPhi }\left( L(t) - i + 2\right) \left( \frac{N-3}{\tau }\right) ^2 \end{aligned}$$

(31)

In the optimization, $\xi (t)$ and its derivatives are only evaluated at the collocation points $t_j=\frac{\tau }{M}j$. Substituting this in for t, we find that:

$$\begin{aligned} \xi _j&=\xi (t_j) =\sum _{i=1}^{N} \alpha _i \varPhi \left( \frac{N-3}{M}j - i + 2\right) \end{aligned}$$

(32)

$$\begin{aligned} \xi '_j&=\dot{\xi }(t_j)= \sum _{i=1}^{N} \alpha _i \dot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) \left( \frac{N-3}{\tau }\right) \end{aligned}$$

(33)

$$\begin{aligned} \xi ''_j&=\ddot{\xi }(t_j)= \sum _{i=1}^{N} \alpha _i \ddot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) \left( \frac{N-3}{\tau }\right) ^2 \end{aligned}$$

(34)

It is important to remember that the actual decision variables in the optimization are the weights of the basis functions $\alpha _1\ldots \alpha _N$ used to describe each of the continuous decision variables $\xi (x)$, and the inverse of the time length of the trajectory $\frac{1}{\tau }$. Thus, it would be useful to be able to quickly map $\frac{1}{\tau }$ and the vector of basis function weights,

$$\begin{aligned} A=[\alpha _1,\ldots \alpha _i,\ldots \alpha _N]^T \end{aligned}$$

(35)

to the vectors encoding the values of $\xi$ and its derivatives evaluated at the collocation points:

$$\begin{aligned} V&=[\xi _0,\xi _1,\ldots \xi _j,\ldots \xi _M]^T \end{aligned}$$

(36)

$$\begin{aligned} V'&=[\xi '_0,\xi '_1,\ldots \xi '_j,\ldots \xi '_M]^T \end{aligned}$$

(37)

$$\begin{aligned} V''&=[\xi ''_0,\xi ''_1,\ldots \xi ''_j,\ldots \xi ''_M]^T \end{aligned}$$

(38)

To do this, we define transformation matrices $D^0$, $D^1$, $D^2$:

$$\begin{aligned} D^0_{ij}&= \varPhi \left( \frac{N-3}{M}j - i + 2\right) \end{aligned}$$

(39)

$$\begin{aligned} D^1_{ij}&= \dot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) (N-3) \end{aligned}$$

(40)

$$\begin{aligned} D^2_{ij}&= \ddot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) (N-3)^2 \end{aligned}$$

(41)

These matrices are constant for a given choice of (M, N), meaning that they only need to be evaluated once at the start of the optimization. The matrices are also sparse, since $\varPhi (x)$,$\dot{\varPhi }(x)$,$\ddot{\varPhi }(x)$ vanish outside of the interval $[-\,2,2]$. It follows that:

$$\begin{aligned} V=D^0A, \quad V'=\frac{1}{\tau } D^1A,\quad V''=\frac{1}{\tau ^2}D^2A \end{aligned}$$

(42)

We construct transformation matrices for each motion variable, since each motion variable can be represented with a different number of basis functions. This framework gives us the flexibility to tune the resolution of each spline independently, while managing the necessary re-scaling operations to ensure that each motion variable is evaluated across the same time interval.

1.2 A.2 Shape variables

We represent shape as a parametric curve: $\left( X(s),Y(s)\right)$. Like the motion variables, X(s) and Y(s) are described as a sum of basis functions,

$$\begin{aligned} X(s)&= \sum _{i=1}^{N}\alpha _{xi} \varPhi (s-i-k) \end{aligned}$$

(43)

$$\begin{aligned} Y(s)&= \sum _{i=1}^{N}\alpha _{yi} \varPhi (s-i-k) \end{aligned}$$

(44)

where the basis function $\varPhi$ is described in Sect. A.1. Unlike the motion variables, which can have varying resolutions, we use the same number of basis functions to describe the X and Y splines of a given shape. Since the contact parameter s(t) is one of the motion variables in the optimization, it follows that $\varPhi (s(t_j)-i-k)$ is not constant during the optimization. Thus, there is no benefit in constructing a set of transformation matrices for X and Y as we did in Sect. A.1. Instead, we must re-evaluate $\varPhi ,\dot{\varPhi }$ using (27) and (28) each time we compute X, Y and their derivatives.

B Toy problem implementation

In this section, we will describe the specifics details of converting the toy problem into a nonlinear program.

1.1 B.1 Variable definitions

For the sake of simplicity, we will assume that each motion variable uses the same number of basis functions. In this case, if the motion variables use N basis functions and the shape variables use L basis functions, then the decision variables in the optimization are the vectors of basis function weights for each continuous decision variable:

$$\begin{aligned} \bar{\theta }_h&= [\bar{\theta }_{h1},\ldots \bar{\theta }_{hi},\ldots \bar{\theta }_{hN}]^T \end{aligned}$$

(45)

$$\begin{aligned} \bar{s}_h&= [\bar{s}_{h1},\ldots \bar{s}_{hi},\ldots \bar{s}_{hN}]^T\end{aligned}$$

(46)

$$\begin{aligned} \bar{\theta }_b&= [\bar{\theta }_{b1},\ldots \bar{\theta }_{bi},\ldots \bar{\theta }_{bN}]^T\end{aligned}$$

(47)

$$\begin{aligned} \bar{s}_b&= [\bar{s}_{b1},\ldots \bar{s}_{bi},\ldots \bar{s}_{bN}]^T\end{aligned}$$

(48)

$$\begin{aligned} \bar{p}_{bx}&= [\bar{p}_{bx1},\ldots \bar{p}_{bxi},\ldots \bar{p}_{bxN}]^T\end{aligned}$$

(49)

$$\begin{aligned} \bar{p}_{by}&= [\bar{p}_{by1},\ldots \bar{p}_{byi},\ldots \bar{p}_{byN}]^T\end{aligned}$$

(50)

$$\begin{aligned} \bar{c}_{hx}&= [\bar{c}_{ch1},\ldots \bar{c}_{hxi},\ldots \bar{c}_{hxL}]^T\end{aligned}$$

(51)

$$\begin{aligned} \bar{c}_{hy}&= [\bar{c}_{hy1},\ldots \bar{c}_{hyi},\ldots \bar{c}_{hyL}]^T \end{aligned}$$

(52)

We include an additional decision variable $\nu =\frac{1}{\tau }$, where $\tau$ is the time length of the trajectory. For the toy problem, we assume that the ball is a circle of radius r, and the hand is constrained to rotate about the origin, meaning that the decision variables $\bar{p}_{hx},\bar{p}_{hy},\bar{c}_{bx},\bar{c}_{by}$ are absent from the optimization.

If $\xi _j,\xi '_j,\xi ''_j$ denote the value of a continuous decision variable and its derivatives evaluated at the jth collocation point, then for M $+$ 1 collocation points this can be represented in vector form as:

$$\begin{aligned} \xi&= [\xi _0,\xi _1,\ldots ,\xi _j,\ldots \xi _M]^T\end{aligned}$$

(53)

$$\begin{aligned} \xi '&= [\xi '_0,\xi '_1,\ldots ,\xi '_j,\ldots \xi '_M]^T\end{aligned}$$

(54)

$$\begin{aligned} \xi ''&= [\xi ''_0,\xi ''_1,\ldots ,\xi ''_j,\ldots \xi ''_M]^T\end{aligned}$$

(55)

$$\begin{aligned} \bar{\xi }&= [\bar{\xi }_1,\ldots ,\bar{\xi }_i,\ldots \bar{\xi }_N]^T \end{aligned}$$

(56)

As shown in Sect. A.1, we see that if $\xi (t)$ is a motion variable, then we can use the transformation matrices $D^0,D^1,D^2$ to quickly map $\bar{\xi }$ and $\nu$ to $\xi _j,\xi '_j,\xi ''_j$:

$$\begin{aligned} \xi =D^0\bar{\xi }, \quad \xi '=\nu D^1\bar{\xi },\quad \xi ''=\nu ^2 D^2\bar{\xi } \end{aligned}$$

(57)

To evaluate the hand shape and its tangency vector at the jth collocation point, we use the definitions:

$$\begin{aligned}&\left[ \begin{array}{c} c_{hxj}\\ c_{hyj} \end{array}\right] = \sum _{i=1}^L\left[ \begin{array}{c} \bar{c}_{hxi}\\ \bar{c}_{hyi} \end{array}\right] \varPhi (s_{hj}-i-k) \end{aligned}$$

(58)

$$\begin{aligned}&\left[ \begin{array}{c} c'_{hxj}\\ c'_{hyj} \end{array}\right] = \sum _{i=1}^L\left[ \begin{array}{c} \bar{c}_{hxi}\\ \bar{c}_{hyi} \end{array}\right] \dot{\varPhi }(s_{hj}-i-k) \end{aligned}$$

(59)

Since the ball is a circle of radius r, the ball shape and its tangecny vector at the jth collocation point are given by:

$$\begin{aligned}&\left[ \begin{array}{c} c_{bxj}\\ c_{byj} \end{array}\right] = r\left[ \begin{array}{c} \cos (s_{bj})\\ \sin (s_{bj}) \end{array}\right] \end{aligned}$$

(60)

$$\begin{aligned}&\left[ \begin{array}{c} c'_{bxj}\\ c'_{byj} \end{array}\right] = r\left[ \begin{array}{c} -\sin (s_{bj})\\ \cos (s_{bj}) \end{array}\right] \end{aligned}$$

(61)

1.2 B.2 Function definitions

We define R as a function that maps angle $\theta$ to the rotation matrix corresponding to a counterclockwise rotation by $\theta$:

$$\begin{aligned} R(\theta )= \left[ \begin{array}{cc} \cos (\theta ),&{}-\sin (\theta )\\ \cos (\theta )&{}\sin (\theta ), \end{array}\right] \end{aligned}$$

(62)

We define $\angle$ as the function that maps the vector $[x,y]^T$ to its polar angle:

$$\begin{aligned} \angle [x,y]^T=Im\left( \ln (x+yi)\right) \end{aligned}$$

(63)

1.3 B.3 Constraints

At the jth collocation point, we apply the kinematic and dynamic constraints of motion, as described in Sect. 4.1.

Contact constraint

$$\begin{aligned} R(\theta _{hj}) \left[ \begin{array}{c} c_{hxj}\\ c_{hyj} \end{array}\right] - R(\theta _{bj}) \left[ \begin{array}{c} c_{bxj}\\ c_{byj} \end{array}\right] -\left[ \begin{array}{c} p_{bxj}\\ p_{byj} \end{array}\right] =0 \end{aligned}$$

(64)

Tangency constraint

$$\begin{aligned}&\theta _{hj}+ \angle R\left( \frac{\pi }{2}\right) \left[ \begin{array}{c} c'_{hxj}\\ c'_{hyj} \end{array}\right] - \theta _{bi} - \angle R\left( \frac{\pi }{2}\right) \left[ \begin{array}{c} c'_{bxj}\\ c'_{byj} \end{array}\right] \nonumber \\&\quad + (2k_j+1)\pi = 0 \end{aligned}$$

(65)

Here, $k_j$ is the integer used to deal with the fact that the original tangency constraint is true mod $2\pi$. It is chosen at each constraint evaluation to minimize the constraint error.

Rolling constraint

$$\begin{aligned} s'_{hj}\sqrt{\left( c'_{hxj}\right) ^2+\left( c'_{hyj}\right) ^2} +s'_{bj}\sqrt{\left( c'_{bxj}\right) ^2+\left( c'_{byj}\right) ^2}=0\nonumber \\ \end{aligned}$$

(66)

Inertia constraint

$$\begin{aligned} \left( R(\theta _{bj})\left[ \begin{array}{c} c_{bxj}\\ c_{byj} \end{array}\right] \right) \times m \left( \left[ \begin{array}{c} p''_{bxj}\\ p''_{byj} \end{array}\right] + \left[ \begin{array}{c} 0\\ g \end{array}\right] \right) - I \theta ''_{bj}=0\nonumber \\ \end{aligned}$$

(67)

Friction cone constraint

$$\begin{aligned} \left( \left( \mu R\left( \frac{\pi }{2}\right) \pm \left[ \begin{array}{cc} 1&{}0\\ 0&{}1 \end{array}\right] \right) \frac{[c'_{hxj},c'_{hyj}]^T}{\sqrt{\left( c'_{hxj}\right) ^2+\left( c'_{hyj}\right) ^2}}\right) \cdot \mathbf {f_{hj}} \ge 0\nonumber \\ \end{aligned}$$

(68)

where

$$\begin{aligned} \mathbf {f_{hj}}=R(-\theta _{hj}) m\left( \left[ \begin{array}{c} p''_{bxj}\\ p''_{byj} \end{array}\right] + \left[ \begin{array}{c} 0\\ g \end{array}\right] \right) \end{aligned}$$

(69)

Task constraints

To constrain the ball to move along a specific path, we use the constraint:

$$\begin{aligned} G(p_{bxj},p_{byj})=0 \end{aligned}$$

(70)

If we want to control both the spatial path, and the speed of the ball as it travels, we instead fix the decision variables for the position of the ball and the length of the trajectory:

$$\begin{aligned}{}[\bar{p}_{bxi},\bar{p}_{byi}]&= [f_{px}(i),f_{py}(i)] \end{aligned}$$

(71)

$$\begin{aligned} \nu&= C \end{aligned}$$

(72)

We can fix the motion of the hand using the constraints:

$$\begin{aligned} \bar{\theta }_{hi}&= f_\theta (i) \end{aligned}$$

(73)

$$\begin{aligned} \nu&= C \end{aligned}$$

(74)

If instead we’d like to fix the shape of the hand, we use the constraint:

$$\begin{aligned}{}[\bar{c}_{hxi},\bar{c}_{hyi}]&= [f_{cx}(i),f_{cy}(i)] \end{aligned}$$

(75)