Optimal shape and motion planning for dynamic planar manipulation

  • 240 Accesses


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.

This is a preview of subscription content, log in to check access.

Access options

Buy single article

Instant unlimited access to the full article PDF.

US$ 39.95

Price includes VAT for USA

Subscribe to journal

Immediate online access to all issues from 2019. Subscription will auto renew annually.

US$ 99

This is the net price. Taxes to be calculated in checkout.

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13
Fig. 14


  1. Becker, A., & Bretl, T. (2012). Approximate steering of a plate-ball system under bounded model perturbation using ensemble control. In IEEE/RSJ international conference on intelligent robots and systems (IROS) (pp. 5353–5359).

  2. Brokowski, M., Peshkin, M., & Goldberg, K. (1995). Optimal curved fences for part alignment on a belt. Transactions-America Society of Mechanical Engineers Journal of Mechanical Design, 117, 27–35.

  3. Caine, M. (1994). The design of shape interactions using motion constraints. In IEEE international conference on robotics and automation (ICRA) (pp. 366–371).

  4. Chen, P. T. (2010). Simulation and optimization of a two-wheeled, ball-flinging robot. Masters thesis, University of California, San Diego.

  5. Cristescu, A., Cristescu, B., & Laurenia, A. (2014). Generalization of multispeed gear pitch curves design. Applied Mechanics and Materials, 659, 559–564.

  6. Gill, P. E., Murray, W., & Saunders, M. A. (2005). SNOPT: An SQP algorithm for large-scale constrained optimization. SIAM Review, 47(1), 99–131.

  7. Goss, V. G. A. (2013). Application of analytical geometry to the form of gear teeth. Resonance, 18(9), 817–831.

  8. Ha, S., Coros, S., Alspach, A., Kim, J., & Yamane, K. (2017). Joint optimization of robot design and motion parameters using the implicit function theorem. In Robotics: Science and systems.

  9. Hargraves, C. R., & Paris, S. W. (1987). Direct trajectory optimization using nonlinear programming and collocation. Journal of Guidance, Control, and Dynamics, 10(4), 338–342.

  10. Lippiello, V., Ruggiero, F., & Siciliano, B. (2016). The effect of shapes in input-state linearization for stabilization of nonprehensile planar rolling dynamic manipulation. IEEE Robotics and Automation Letters, 1(1), 492–499.

  11. Litvin, F. L., Fuentes-Aznar, A., Gonzalez-Perez, I., & Hayasaka, K. (2009). Noncircular gears: Design and generation. New York: Cambridge University Press.

  12. Liu, J. Y., & Chen, Y. C. (2008). A design for the pitch curve of noncircular gears with function generation. In Proceedings of the international multiconference of engineers and computer scientists (Vol. 2).

  13. Lynch, K. M., & Mason, M. T. (1999). Dynamic nonprehensile manipulation: Controllability, planning, and experiments. The International Journal of Robotics Research, 18(1), 64–92.

  14. Lynch, K. M., Shiroma, N., Arai, H., & Tanie, K. (1998). The roles of shape and motion in dynamic manipulation: The butterfly example. In IEEE international conference on robotics and automation (ICRA) (pp. 1958–1963).

  15. Montana, D. J. (1988). The kinematics of contact and grasp. The International Journal of Robotics Research, 7(3), 17–32.

  16. Posa, M., Cantu, C., & Tedrake, R. (2014). A direct method for trajectory optimization of rigid bodies through contact. The International Journal of Robotics Research, 33(1), 69–81.

  17. Reist, P., & D’Andrea, R. (2009). Bouncing an unconstrained ball in three dimensions with a blind juggling robot. In IEEE international conference on robotics and automation (ICRA) (pp. 1774–1781).

  18. Reist, P., & D’Andrea, R. (2011). Design of the pendulum juggler. In IEEE international conference on robotics and automation (ICRA) (pp. 5154–5159).

  19. Rodgers, E. (1946). Brachistochrone and tautochrone curves for rolling bodies. American Journal of Physics, 14(4), 249–252.

  20. Rodriguez, A. (2013). Shape for contact. Ph.D. thesis, CMU-RI-TR-13-21, Carnegie Mellon University.

  21. Rodriguez, A., & Mason, M. T. (2012). Grasp invariance. The International Journal of Robotics Research, 31(2), 237–249.

  22. Rodriguez, A., & Mason, M. T. (2013). Effector form design for 1DOF planar actuation. In IEEE international conference on robotics and automation (ICRA) (pp. 349–356).

  23. Ryu, J. C., Ruggiero, F., & Lynch, K. M. (2012). Control of nonprehensile rolling manipulation: Balancing a disk on a disk. In IEEE international conference on robotics and automation (ICRA) (pp. 3232–3237).

  24. Sussmann, H. J., & Willems, J. C. (1997). 300 years of optimal control: From the brachystochrone to the maximum principle. IEEE Control Systems Magazine, 17(3), 32–44.

  25. Yang, D. C., & Tong, S. H. (1998). Generation of identical noncircular pitch curves. Journal of mechanical design, 120, 337–341.

  26. Zarbski, I., & Saaciski, T. (2008). Designing of non-circular gears. Archive of Mechanical Engineering, 55(3), 275–292.

Download references

Author information

Correspondence to Orion Taylor.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

This is one of several papers published in Autonomous Robots comprising the “Special Issue on Robotics Science and Systems”.

Electronic supplementary material

Below is the link to the electronic supplementary material.

Supplementary material 1 (mp4 20699 KB)

Supplementary material 1 (mp4 20699 KB)


A Collocation details

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}$$


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

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}$$
$$\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}$$
$$\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}$$

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}$$
$$\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}$$

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}$$
$$\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}$$
$$\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}$$

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}$$

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}$$
$$\begin{aligned} V'&=[\xi '_0,\xi '_1,\ldots \xi '_j,\ldots \xi '_M]^T \end{aligned}$$
$$\begin{aligned} V''&=[\xi ''_0,\xi ''_1,\ldots \xi ''_j,\ldots \xi ''_M]^T \end{aligned}$$

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}$$
$$\begin{aligned} D^1_{ij}&= \dot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) (N-3) \end{aligned}$$
$$\begin{aligned} D^2_{ij}&= \ddot{\varPhi }\left( \frac{N-3}{M}j - i + 2\right) (N-3)^2 \end{aligned}$$

These matrices are constant for a given choice of (MN), 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}$$

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.

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}$$
$$\begin{aligned} Y(s)&= \sum _{i=1}^{N}\alpha _{yi} \varPhi (s-i-k) \end{aligned}$$

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 XY 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.

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}$$
$$\begin{aligned} \bar{s}_h&= [\bar{s}_{h1},\ldots \bar{s}_{hi},\ldots \bar{s}_{hN}]^T\end{aligned}$$
$$\begin{aligned} \bar{\theta }_b&= [\bar{\theta }_{b1},\ldots \bar{\theta }_{bi},\ldots \bar{\theta }_{bN}]^T\end{aligned}$$
$$\begin{aligned} \bar{s}_b&= [\bar{s}_{b1},\ldots \bar{s}_{bi},\ldots \bar{s}_{bN}]^T\end{aligned}$$
$$\begin{aligned} \bar{p}_{bx}&= [\bar{p}_{bx1},\ldots \bar{p}_{bxi},\ldots \bar{p}_{bxN}]^T\end{aligned}$$
$$\begin{aligned} \bar{p}_{by}&= [\bar{p}_{by1},\ldots \bar{p}_{byi},\ldots \bar{p}_{byN}]^T\end{aligned}$$
$$\begin{aligned} \bar{c}_{hx}&= [\bar{c}_{ch1},\ldots \bar{c}_{hxi},\ldots \bar{c}_{hxL}]^T\end{aligned}$$
$$\begin{aligned} \bar{c}_{hy}&= [\bar{c}_{hy1},\ldots \bar{c}_{hyi},\ldots \bar{c}_{hyL}]^T \end{aligned}$$

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}$$
$$\begin{aligned} \xi '&= [\xi '_0,\xi '_1,\ldots ,\xi '_j,\ldots \xi '_M]^T\end{aligned}$$
$$\begin{aligned} \xi ''&= [\xi ''_0,\xi ''_1,\ldots ,\xi ''_j,\ldots \xi ''_M]^T\end{aligned}$$
$$\begin{aligned} \bar{\xi }&= [\bar{\xi }_1,\ldots ,\bar{\xi }_i,\ldots \bar{\xi }_N]^T \end{aligned}$$

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}$$

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}$$
$$\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}$$

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}$$
$$\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}$$

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}$$

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}$$

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}$$

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}$$

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}$$

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}$$

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}$$


$$\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}$$

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}$$

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}$$
$$\begin{aligned} \nu&= C \end{aligned}$$

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

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

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}$$

Rights and permissions

Reprints and Permissions

About this article

Verify currency and authenticity via CrossMark

Cite this article

Taylor, O., Rodriguez, A. Optimal shape and motion planning for dynamic planar manipulation. Auton Robot 43, 327–344 (2019).

Download citation


  • Manipulation
  • Optimization
  • Robot end effector design