1 Introduction

Model-based design is the prevailing approach in engineering. The automotive and aerospace industries make extensive use of modeling and optimization tools to improve their products. As the available computation power increases, so too does the viability of using model-based optimization to solve control problems.

Practical systems often contain a combination of discrete and continuous state variables. Accurate modeling of such systems requires the incorporation of hybrid dynamics. Hybrid models often appear in the context of processes with switched dynamics. For example in mechanics, establishing or breaking contact changes the behavior of the system. Although it is possible to model the rapid changes in forces resulting from such phenomena, the resulting differential equations are stiff and cause trouble for numerical methods. An alternative approach is to instead allow discontinuities in the state trajectories. The framework of hybrid systems allows both switched dynamics and discontinuous state variables.

Applying optimization techniques to hybrid models has resulted in the solution of various practical problems. In locomotion planning, there are many examples where hybrid dynamic optimization methods have been used. For example, gait generation for bipedal and quadrupedal robots is addressed in [17]. In [12], the full dynamics of a humanoid is considered to find a walking pattern. Hybrid dynamic optimization is also used in other fields of engineering, e.g., to make discrete decisions in power plants [13].

A competing paradigm to the model-based approach is the model-free approach. Using a variety of model-free techniques, successful results for optimal control of robots interacting with the environment have been reported [21, 30, 32]. Nevertheless, model-free approaches provide us with little or no insight about fundamental limitations and difficulties.

This paper contributes by presenting a simple, yet realistic enough, example system to capture the main issues arising in modeling and optimal control of dynamics with varying contacts. We propose a ball and finger system resembling a trackball interacted with by a human finger. Unilateral and non-holonomic constraints play an important role in this system. In order to derive analytic equations, simple 3D geometries are considered. The model possesses the following features:

  • Analytic 3D models with contacts

  • Rolling of finger against ball

  • Dynamics and slippage

  • Suitable for simulation as well as optimization

There are elaborate mathematical models for studying contact stability [35, 36]. In contrast, the purpose of the model developed here is to capture contact transitions and to find optimal trajectories. The hybrid dynamic optimization required for this purpose has been performed utilizing a multiphase approach, i.e., a sequence of the discrete modes of the system is mapped to phases.

The rest of the paper is organized as follows. In Sect. 2, the mechanical model of the process is developed. Based on this model, we construct a hybrid system in Sect. 3, which is later on used for simulation as well as optimization. Section 4 discusses the trajectory-planning problem and the optimization approach to find optimal solutions. Various tools have been used for simulation and optimization. The tool chain is detailed in Sect. 5. We introduce instances of the trajectory-planning problems and give the results of the optimization and simulation in Sect. 6. Section 7 discusses various aspects of the modeling and optimization. We draw conclusions and propose future research in Sect. 8. Additionally, Appendices A and B provide the details of the mathematical expressions for the kinematics and the dynamics of the finger, respectively.

2 Mechanical modeling

In this section, we build the mechanical model of the process shown in Fig. 1. The ball is positioned in a socket joint which is not shown in the figure. To produce analytic equations, we consider an idealized model, which only admits contact between a sphere and a line segment. The following list summarizes the modeling assumptions:

  1. 1.

    The finger is modeled as a rigid body with three degrees of freedom (DOF).

  2. 2.

    The ball is a sphere with radius \(r\), which is held by a socket imposing a spherical joint constraint.

  3. 3.

    The Coulomb friction model is considered with static (\(\mu _{s}\)) and kinetic (\(\mu_{k}\)) friction constants.

  4. 4.

    There is viscous friction in the joints and between the socket and the ball, with friction constants \(\mu_{v}\) and \(D\), respectively.

  5. 5.

    There is no torsional friction.

  6. 6.

    The collision between the finger and the ball is inelastic, i.e., when the finger collides with the ball it will not bounce back. Accordingly, the coefficient of restitution is equal to zero.

  7. 7.

    The links are ideal line segments.

  8. 8.

    In case of contact, single point contact either at the end-effector or along the last link is allowed. This is guaranteed by the choice of the geometry.

  9. 9.

    The contact points are non-adhesive.

  10. 10.

    The centers of gravity (COG) are located at the middle of each link and inertia tensors with respect to these points are diagonal.

Fig. 1
figure 1

An illustration of the ball-and-finger system for \(\ell= r\)

Full size image

2.1 Geometry and kinematics

Considering Fig. 1, the geometry of the system is described here. The ball with radius \(r\) is assumed to be located at the origin \(o_{b} = [0 \ \ 0 \ \ 0 ] ^{T}\). The base of the finger is specified by the transformation consisting of a translation \(o_{f} = [2r \ \ 0 \ \ 0 ] ^{T}\) and a rotation −90 degrees around the \(y\)-axis. The kinematics of the finger is specified by the Denavit–Hartenberg (DH) parameters [10] according to Table 1. We use \(q \in\mathbb{R}^{3}\) to denote the finger joint angles and the length of every link is \(\ell\leq r\). Knowing the DH parameters and the base transformation, the forward kinematics and the geometric Jacobian with respect to the fingertip can be calculated. See Appendix A for details.

Table 1 DH parameters of the finger
Full size table

2.2 Collision

To detect collision between the ball and the finger, the closest distance between the ball and the last link is found. Define \(p_{m} := p_{e} + \alpha\hat{e}_{x_{3}}\) as the point on the line corresponding to the last link closest to the ball, where \(\hat {e}_{x_{3}}\) is the unit vector parallel to link 3, \(p_{e}\) is the position of the fingertip, and \(\alpha\) determines how far \(p_{m}\) lies from the fingertip. Then

$$\begin{aligned} (p_{m} - o_{b}) \cdot\hat{e}_{x_{3}} &= 0, \end{aligned}$$
(1)

which results in

$$\begin{aligned} \alpha= (o_{b} - p_{e}) \cdot\hat{e}_{x_{3}}. \end{aligned}$$
(2)

Define also the potential collision point, i.e., the point on link 3 closest to the ball

$$\begin{aligned} p_{c} := p_{e} + \mbox{sat}_{-a_{3}}^{0}( \alpha) \hat{e}_{x_{3}}, \end{aligned}$$
(3)

where \(\mbox{sat}_{L}^{U}(x)\) denotes the saturation function with \(L\) as the lower limit and \(U\) as the upper limit and \(a_{3} = \ell\) is the length of the third link. Hence, the signed shortest distance from the third link to the ball is

$$\begin{aligned} h(q):= \lVert r_{c} \rVert - r, \end{aligned}$$
(4)

where \(r_{c}:=p_{c} - o_{b}\) is the vector to the closest point on the finger to the ball.

If the ball and the finger are impenetrable, it is required that \(h(q) \geq0\). Therefore, the condition for collision can be stated as

$$\begin{aligned} h(q)\leq0. \end{aligned}$$
(5)

The velocity of the third link at the potential contact point denoted by \(v_{c}\) can be derived from the velocity of the end point \(v_{e}\) using the velocity relation of rigid motion [26] according to

$$\begin{aligned} v_{c} &:= v_{e} + \omega_{3} \times(p_{c} - p_{e}) \\ &\phantom{:}= v_{e} - \mbox{sat}_{-a_{3}}^{0}( \alpha) \hat{e}_{x_{3}} \times\omega_{3} \\ &\phantom{:}= \bigl(J_{p} - \mbox{sat}_{-a_{3}}^{0}( \alpha) S(\hat{e}_{x_{3}}) J_{o} \bigr)\dot{q} \\ &\phantom{:}= J_{c} \dot{q}, \end{aligned}$$
(6)

where \(\omega_{3}\) is the angular velocity of the last link and \(S(\cdot )\) denotes the skew-symmetric operator performing the cross-product. We have also defined

$$\begin{aligned} J_{c} := J_{p} - \mbox{sat}_{-a_{3}}^{0}( \alpha) S(\hat{e}_{x_{3}}) J_{o}, \end{aligned}$$
(7)

which is the Jacobian at the potential contact point. Note that \(v_{c}\) is the velocity of the link at the contact point and not the velocity of the contact point itself, i.e., \(v_{c} \neq\dot{r}_{c}\).

2.3 Constrained motion

When the finger is touching the ball, its motion is constrained to a manifold. Using (4), the finger is in contact with the ball when

$$\begin{aligned} h(q) = 0. \end{aligned}$$
(8)

This is equivalent to

$$\begin{aligned} \lVert r_{c} \rVert^{2} - r^{2} = 0. \end{aligned}$$
(9)

Differentiating (9) w.r.t. time results in

$$\begin{aligned} v_{c} \cdot r_{c} = 0. \end{aligned}$$
(10)

Hence, when in contact, the velocity of the third link at the contact point remains tangential to the ball.

Depending on whether there is an interaction force between the objects, the contact may be active or inactive. Let us denote the normal vector at the contact point outwards w.r.t. the ball by

$$\begin{aligned} n :=\frac{r_{c}}{ \lVert r_{c} \rVert}. \end{aligned}$$
(11)

The normal component of the interaction force at the contact is then given by

$$ \lambda_{n} :=\lambda\cdot n, $$
(12)

where \(\lambda\) denotes the contact force exerted on the finger. Using the Coulomb friction model [8], interaction forces between two bodies are possible if and only if there is a normal force between the contacting surfaces. Therefore, an active contact implies \(\lambda_{n} > 0\).

The relative velocity between the finger and the ball at the contact point is

$$\begin{aligned} \Delta v := \omega\times r_{c} - v_{c}, \end{aligned}$$
(13)

where \(\omega\) is the angular velocity of the ball. When two bodies are sticking, the relative velocity between them is zero, i.e.,

$$\begin{aligned} \Delta v = 0. \end{aligned}$$
(14)

This constraint is maintained by the static friction.

At this stage, it is useful to introduce \(f_{\perp}\) and \(f_{\parallel }\) for the perpendicular and tangential decomposition of an arbitrary vector \(f\) w.r.t. a plane with normal vector \(n\), according to

$$\begin{aligned} f &= f_{\parallel} + f_{\perp}, \end{aligned}$$
(15a)
$$\begin{aligned} f_{\perp} &= (v\cdot n) n = (n\otimes n) f = P_{n} f, \end{aligned}$$
(15b)
$$\begin{aligned} f_{\parallel} &= -n\times(n \times f) = f - f_{\perp} = (1-P_{n}) f = T_{n} f, \end{aligned}$$
(15c)

where ⊗ denotes the outer product, 1 is the identity matrix of proper dimension, and we have defined

$$\begin{aligned} P_{n} &:= (n\otimes n), \end{aligned}$$
(16)
$$\begin{aligned} T_{n} &:= 1 - P_{n}. \end{aligned}$$
(17)

Now, let us define

$$\begin{aligned} \sigma:= \mu_{s} \lambda_{n} - \lVert\lambda_{\parallel}\rVert, \end{aligned}$$
(18)

where \(\lambda_{\parallel}\) is the force resulting from friction. Hence, according to the Coulomb model, the condition for stiction is

$$\begin{aligned} \lVert\lambda_{\parallel}\rVert < \mu_{s} \lambda_{n} \Leftrightarrow\sigma> 0. \end{aligned}$$
(19)

When the finger is slipping against the ball, the relative velocity \(\Delta v\) is not zero anymore and

$$\begin{aligned} \lambda_{\parallel}= \mu_{k} \lambda_{n} \frac{\Delta v}{ \lVert\Delta v \rVert}. \end{aligned}$$
(20)

Note that in this case \(\Delta v\) is tangential to the ball surface, since using the definition (13) and (10) it is possible to show

$$\begin{aligned} \Delta v \cdot r_{c}= (\omega\times r_{c})\cdot r_{c} - v_{c} \cdot r_{c} = 0. \end{aligned}$$
(21)

The contact is inactive when \(\lambda_{n} \leq0\).

Depending on the contact point between the finger and ball, two types of contact may arise: point and sphere or line and sphere. If the contact is not slipping, for the former type of contact, (14) can be integrated to result in a geometrical constraint, while the latter type yields a rolling without slipping constraint which is non-holonomic.

2.4 Dynamics

In this section, we derive the equations of motion for the ball and finger system. The kinetic and potential energy of the finger can be described by

$$\begin{aligned} &\mathscr{T}_{f} = \sum_{i=1}^{3} \frac{1}{2} m_{i} \dot{q}^{T} \bigl(J_{P}^{i} \bigr)^{T} J_{P}^{i}\dot{q} + \sum _{i=1}^{3}\frac{1}{2} \dot{q}^{T} \bigl(J_{O}^{i} \bigr)^{T} R_{i} I_{i} R_{i}^{T} J_{O}^{i} \dot{q}, \end{aligned}$$
(22a)
$$\begin{aligned} &\mathscr{U}_{f} = \sum_{i=1}^{3} m_{i} g_{0}^{T} p_{i}, \end{aligned}$$
(22b)

respectively. Here, \(m_{i}\) and \(I_{i}\) denote the mass and inertia matrix of link \(i\), respectively, \(J_{P}^{i}\) and \(J_{O}^{i}\) denote partial Jacobians up to link \(i\), \(R_{i}\) is the rotation matrix of link \(i\) with respect to the world coordinate, \(g_{0} = [ 0 \ \ 0 \ \ -g] ^{T}\) is the gravity acceleration vector, and \(p_{i}\) denotes the position vector to the center of the gravity of link \(i\). The Lagrangian of the finger is \(\mathscr{L}_{f} := \mathscr{T}_{f} - \mathscr{U}_{f}\). Accordingly, we derive the equations of motion [7]

$$\begin{aligned} \frac{\mathrm{d}\phantom{x}}{\mathrm{d}t} \biggl(\frac{\partial\mathscr {L}_{f}}{\partial\dot{q}} \biggr) + \frac{\partial\mathscr{L}_{f}}{\partial q} = Q^{e} + Q_{f}^{c}. \end{aligned}$$
(23)

The generalized external forces and the generalized forces due to the kinematic constraints are denoted by \(Q^{e}\) and \(Q_{f}^{c}\), respectively. Substituting \(Q^{e}\) with the actuation torques, \(\tau\), minus the viscous friction, we obtain

$$\begin{aligned} M_{f}(q) \ddot{q} + C(q,\dot{q})\dot{q} + \mu_{v} \dot{q} + g(q) = \tau+ Q_{f}^{c}, \end{aligned}$$
(24)

where \(M_{f}(q)\) is the mass matrix of the finger, \(C(q,\dot{q})\dot{q}\) denotes the contribution of the centrifugal and Coriolis forces, \(\mu _{v}\) is the viscous friction coefficient, and \(g(q)\) is the gravitational force. The expressions for these functions are given in Appendix B.

For the ball, using the Newton–Euler equations, we find

$$ I_{b} \dot{\omega} + \omega\times I_{b} \omega+ D \omega= Q_{f}^{c}, $$
(25)

where \(I_{b} = \frac{2}{5}mr^{2} 1\) is the inertia tensor for the ball and \(D\) denotes the friction constant with the socket.

In the rest of this section, we consider different regions depending on the active constraints imposed by the contact. The effect of collision, i.e., switching between no contact and contact, is also considered. Finally, we describe the overall system.

2.4.1 Free motion

In case of no contact, we have \(Q_{f}^{c} = 0\). Hence,

$$\begin{aligned} &M_{f}(q) \ddot{q} + C(q,\dot{q})\dot{q} + \mu_{v} \dot{q} + g(q) = \tau, \end{aligned}$$
(26a)
$$\begin{aligned} &I_{b} \dot{\omega} + \omega\times I_{b} \omega+ D \omega= 0. \end{aligned}$$
(26b)

2.4.2 Sticking

The condition of no slipping (14) can be rewritten as

$$\begin{aligned} G \begin{bmatrix} \dot{q} \\ \omega \end{bmatrix} = 0, \end{aligned}$$
(27)

where we have defined \(G := [J_{c}\, S(r_{c}) ] \in\mathbb {R}^{3\times6}\). The generalized forces due to the constraints according to the Lagrange–d’Alembert theorem [7] can be derived as

$$\begin{aligned} \begin{bmatrix} Q^{c}_{f} \\ Q^{c}_{b} \end{bmatrix} = G^{T} \lambda, \end{aligned}$$
(28)

where \(\lambda\) are the Lagrange multipliers corresponding to the interaction forces. Therefore, (24) and (25) can be rewritten as

$$\begin{aligned} &M_{f}(q) \ddot{q} + C(q,\dot{q})\dot{q} + \mu_{v} \dot{q} + g(q) = \tau+J^{T}_{c} \lambda, \end{aligned}$$
(29a)
$$\begin{aligned} &I_{b} \dot{\omega} + \omega\times I_{b} \omega+ D \omega= - r_{c} \times\lambda. \end{aligned}$$
(29b)

The interaction forces \(\lambda\) may be solved for in order to reduce the DAE into an ODE. They could be used to evaluate whether in a certain state sticking is possible without switching to new dynamics. The calculation may also be used to find out the direction of kinetic friction at the onset of slipping. Let us define the block diagonal matrix

$$ M := \operatorname{blkdiag} \bigl(M_{f}(q),I_{b} \bigr). $$
(30)

We can rewrite (29a)–(29b) as

$$ M \begin{bmatrix} \ddot{q} \\\dot{\omega} \end{bmatrix} + f(q,\dot{q},\omega, \tau) = G^{T} \lambda, $$
(31)

where we have defined

$$\begin{aligned} f(q,\dot{q},\omega,\tau):= \begin{pmatrix}C(q,\dot{q})\dot{q} + \mu_{v}\dot{q} + g(q) -\tau\\ \omega\times I_{b} \omega+ D \omega \end{pmatrix} . \end{aligned}$$
(32)

The Lagrange multipliers \(\lambda\) can be calculated by first taking the derivative of the constraints (27). Afterward, we multiply (31) by \(GM^{-1}\) from the left and use the relation obtained from the derivative of the constraints. Let us define \(\varGamma:= G M^{-1} G^{T}\). Assuming \(\varGamma^{-1}\) exists, \(\lambda= \lambda(t,q,\dot{q},\omega)\) can be calculated according to

$$\begin{aligned} \lambda&= \varGamma^{-1} \left(G M^{-1} f(q,\dot{q},\omega, \tau) - \sum_{k=1}^{n} \biggl( \frac{\partial G}{\partial q_{k}}\dot{q}_{k} \biggr) \begin{bmatrix} \dot{q} \\\omega \end{bmatrix} \right) \\ &=\varGamma^{-1} \bigl(G M^{-1} f(q,\dot{q},\omega,\tau) + \omega\times\dot{r}_{c} - \dot{J}_{c} \dot{q} \bigr). \end{aligned}$$
(33)

Note that the matrix \(M\) is always invertible with

$$ M^{-1} = \operatorname{blkdiag} \bigl(M_{f}^{-1}(q),I_{b}^{-1} \bigr). $$
(34)

2.4.3 Slipping

In the case of slipping, the constraint only defines the normal part of the interaction force, i.e., \(\lambda_{n}\). By rewriting (10), we find

$$\begin{aligned} r_{c} \cdot J_{c} \dot{q} = 0. \end{aligned}$$
(35)

We normalize (35) by dividing by \(\lVert r_{c} \rVert\), to get

$$\begin{aligned} n^{T} J_{c} \dot{q} = 0. \end{aligned}$$
(36)

Now, from the Lagrange–d’Alembert theorem, it is clear that the contribution of the constraint force is \(Q_{f}^{c} = J_{c}^{T} n \lambda_{n}\). Including the kinetic friction forces specified by (20), we conclude

$$\begin{aligned} &M_{f}(q) \ddot{q} + C(q,\dot{q})\dot{q} + \mu_{v} \dot{q} + g(q) = \tau+ J_{c}^{T}( \lambda_{\parallel}+ n \lambda_{n}), \end{aligned}$$
(37a)
$$\begin{aligned} &I_{b} \dot{\omega} + \omega\times I_{b} \omega+ D \omega= - r_{c} \times\lambda_{\parallel}. \end{aligned}$$
(37b)

It is also possible to reformulate (37a)–(37b) using the total interaction force \(\lambda\) such that we obtain the same relations as (29a)–(29b). In this case, (20) can be written as

$$\begin{aligned} \lambda\cdot\Delta v &= \lVert\Delta v \rVert \mu _{k} (\lambda \cdot n), \end{aligned}$$
(38a)
$$\begin{aligned} (n \times\Delta v) \cdot\lambda&= 0, \end{aligned}$$
(38b)

which additionally has no risk of division by zero as opposed to (20).

2.5 Impacts

Collisions give rise to rapid changes of the states. By allowing discontinuities in the state trajectory, it is possible to model the effect of impacts right after the collision without dealing with the actual process. In this case, new values for post-impact quantities must be calculated. Since these values can have jumps, we use superscripts + and − to denote the right and left limits of a quantity, respectively. Next, we consider various impact laws. Since after collision the finger is neither allowed to penetrate the ball nor to bounce off directly (coefficient of restitution equal to zero), all the impact models ensure \((J_{c} \dot{q}^{+})_{\perp}= n \cdot J_{c} \dot{q}^{+} = 0\).

2.5.1 Newton’s impact law

According to Newton’s impact law [28], after impact the tangential velocity of the finger remains intact and only its normal velocity is vanished because of the assumed inelasticity of the collision. Hence,

$$\begin{aligned} J_{c}\dot{q}^{+} = (J_{c}\dot{q})^{-}_{\parallel}= T_{n} J_{c}\dot{q}^{-}. \end{aligned}$$
(39)

Accordingly, we can solve for the post-impact joint velocities. In this case, the impulse is perpendicular to the ball and cannot change the velocity of the ball.

2.5.2 Sticking

When sticking occurs, the force impulse \(\iota\) resulting from the impact makes the post-collision relative velocity \(\Delta v^{+} = 0\). From the equations of motion (29a)–(29b) and (13), we conclude

$$\begin{aligned} & M_{f}(q) \bigl(\dot{q}^{+} - \dot{q}^{-} \bigr) = J^{T}_{c} \iota, \end{aligned}$$
(40a)
$$\begin{aligned} & I_{b} \bigl(\omega^{+}-\omega^{-} \bigr) = - r_{c} \times\iota, \end{aligned}$$
(40b)
$$\begin{aligned} &\omega^{+} \times r_{c} - J_{c}\dot{q}^{+}= 0, \end{aligned}$$
(40c)

which can be solved for \(\dot{q}^{+}\), \(\omega^{+}\), and \(\iota\). By combining (40a)–(40c), we find

$$\begin{aligned} \bigl(J_{c} M_{f}^{-1} J_{c}^{T} - S(r_{c}) I_{b}^{-1} S(r_{c}) \bigr) \iota &= \omega^{-} \times r_{c} - J_{c} \dot{q}^{-} \\ &= \Delta v^{-}. \end{aligned}$$
(41)

This shows that the impulse \(\iota\) is a function of the contact point \(r_{c}\), the configuration of the finger \(q\), and the pre-impact relative velocity \(\Delta v^{-}\), but neither the absolute velocity of the finger nor the ball.

2.5.3 Slipping

In the case of slipping, an impact law can be formulated as minimizing the post-impact energy, based on the maximum dissipation principle [16]. The optimization problem to be solved is

$$\begin{aligned} \text{minimize} & \frac{1}{2} \omega^{+T} I_{b} \omega^{+} + \frac{1}{2} \dot{q}^{+T} M_{f}(q) \dot{q}^{+}, \end{aligned}$$
(42a)
$$\begin{aligned} \text{with respect to} & \iota, \omega^{+}, \dot{q}^{+}, \\ \text{subject to} & M_{f}(q) \bigl(\dot{q}^{+} - \dot{q}^{-} \bigr) = J^{T}_{c} \iota, \end{aligned}$$
(42b)
$$\begin{aligned} & I_{b} \bigl(\omega^{+}-\omega^{-} \bigr) = - r_{c} \times\iota, \end{aligned}$$
(42c)
$$\begin{aligned} & n \cdot J_{c}\dot{q}^{+}= 0, \end{aligned}$$
(42d)
$$\begin{aligned} & \lVert\iota_{\parallel} \rVert \leq\mu_{k} \iota \cdot n . \end{aligned}$$
(42e)

A heuristic way is to minimize the post-impact relative velocity. This strategy allows a quick transition to sticking. The corresponding optimization problem is

$$\begin{aligned} \text{minimize} & \bigl\lVert \Delta v^{+} \bigr\rVert , \end{aligned}$$
(43a)
$$\begin{aligned} \text{with respect to} & \mu, \iota, \omega^{+}, \dot{q}^{+}, \\ \text{subject to} & M_{f}(q) \bigl(\dot{q}^{+} - \dot{q}^{-} \bigr) = J^{T}_{c} \iota, \end{aligned}$$
(43b)
$$\begin{aligned} & I_{b} \bigl(\omega^{+}-\omega^{-} \bigr) = - r_{c} \times\iota, \end{aligned}$$
(43c)
$$\begin{aligned} & n \cdot J_{c}\dot{q}^{+}= 0, \end{aligned}$$
(43d)
$$\begin{aligned} & \iota_{\parallel} = \mu\iota_{n} \frac{\Delta v^{-}_{\parallel}}{ \lVert\Delta v^{-}_{\parallel}\rVert}, \end{aligned}$$
(43e)
$$\begin{aligned} & 0< \mu\leq\mu_{k}, \end{aligned}$$
(43f)

where we have additionally assumed that the direction of the tangential part of the impulse is aligned with the pre-impact relative velocity. Problem (43a)–(43f) can be solved analytically [15]. Subsequently, the equations can be used in an optimal control problem.

2.6 Overall system

The complete system has three discrete modes, corresponding to free motion, sticking, and slipping. Let \(\lambda\) denote the interaction force and \(x^{T} := ( q^{T},\, dq^{T} ,\, \omega^{T} )\), where \(\dot {q} = dq\). If for the moment we do not consider the transition between modes, we can unify all the modes with the first-order differential equation

$$\begin{aligned} &\begin{pmatrix}1 & 0 \\0 & M(q) \end{pmatrix} \dot{x} + \begin{pmatrix}0 & -1 & 0 \\ 0 &C(q,dq) +\mu_{v} & 0\\ 0 & 0 & S(\omega) I_{b} + D \end{pmatrix} x + \begin{pmatrix}0 \\ g(q) \\0 \end{pmatrix} \\ &\quad = \begin{pmatrix}0\\ \tau\\0 \end{pmatrix} + \begin{pmatrix}0 \\ J_{c}^{T}(q)\\-S(r_{c}(q)) \end{pmatrix} \lambda. \end{aligned}$$
(44)

The determination of \(\lambda\) is separate for each mode as follows:

  • Free motion,

    $$\begin{aligned} \lambda= 0. \end{aligned}$$
    (45)

  • Sticking (valid when \(\sigma> 0\)),

    $$\begin{aligned} J_{c} \dot{q} + r_{c} \times\omega= 0. \end{aligned}$$
    (46)

  • Slipping (valid when \(\Delta v \neq0\)),

    $$\begin{aligned} r_{c} \cdot J_{c} \dot{q} &= 0 , \end{aligned}$$
    (47a)
    $$\begin{aligned} \lambda\cdot\Delta v &= \lVert\Delta v \rVert \mu_{k} \lambda_{n}, \end{aligned}$$
    (47b)
    $$\begin{aligned} (n \times\Delta v) \cdot\lambda&= 0. \end{aligned}$$
    (47c)

Hence, we have a differential-algebraic equation (DAE) describing the dynamics in each mode, which is low index in the case of free motion and of index 2 in the case of sticking and slipping.

3 System modeling

The aim of this section is to build a hybrid dynamical system based on the mechanical system introduced in Sect. 2.6. To find the solution to the overall system, we must also consider an impact law and the so called Signorini’s condition or corner law for \(\lambda_{n}\) (see [8]):

$$\begin{aligned} \begin{aligned} &h\cdot\lambda_{n} = 0, \\ &h\geq0, \quad \lambda_{n} \geq0. \end{aligned} \end{aligned}$$
(48)

Firstly, we discuss the relation between these complementarity conditions and mode transitions. In the latter subsection, the complete hybrid system is presented.

3.1 Mode transitions

In view of the complementarity conditions (48), we can partition a state trajectory into three different regions:

$$\begin{aligned} &\lambda_{n} = 0 \wedge h > 0, \end{aligned}$$
(49a)
$$\begin{aligned} &\lambda_{n} > 0 \wedge h = 0, \end{aligned}$$
(49b)
$$\begin{aligned} &\lambda_{n} = 0 \wedge h = 0. \end{aligned}$$
(49c)

The first partition is a region where the dynamics are unconstrained. The second partition is related to active contacts where constrained dynamics are applicable. The third partition defines states, where there is a contact but the constraint is inactive. This set can be reached from either the unconstrained dynamics or the constrained dynamics.

Using the complementarity conditions, it is possible to determine whether certain dynamics can be assumed in a given region. However, for simulation of the dynamics forward, (48) does not provide explicit transition conditions between unconstrained and constrained modes. In continuous time a complete causal description of the transition condition is not generally possible. Thus, by allowing a small violation of the validity regions, the transition time can be approximated arbitrarily close to the actual ones.

Starting from the free motion where \(\lambda_{n} = 0\), using the unconstrained dynamics when \(h=0\) and either \(\dot{h} < 0\) or \(\dot{h}= 0 \wedge\ddot{h} < 0\) leads to penetration. Therefore, to guarantee impenetrability new initial velocities must be calculated (in the case of inelastic collision, \(\dot{h}=0\)). Accordingly, the transition from the free motion is triggered when

$$\begin{aligned} h < 0 \vee(h = 0 \wedge\dot{h} < 0) \vee(h = \dot{h} = 0 \wedge\ddot{h} < 0). \end{aligned}$$
(50)

Depending on the post-impact conditions, the state trajectory can either switch to the constrained dynamics or continue evolving according to the unconstrained dynamics. On the other hand, starting from either of the constrained dynamics where \(h = 0\), the transition to the free motion can be triggered when \(\lambda_{n} < 0\).

The modes and the transitions are illustrated in Fig. 2, following the notation in [22]. Each mode is represented by a circle and can have its own set of dynamical equations. The conditions for the validity of a mode is given at the lowest part of the circle. The transition conditions are shown close to the tail of the arrows and the reset maps close to the arrowheads. Note that \(h < 0\) and \(\lambda_{n} < 0\) in the transition conditions should be interpreted as when the boundaries at zero are crossed. In the case of dynamic simulation, this means that the solver has to pinpoint when the transition condition becomes satisfied with equality and then perform the transition.

Fig. 2
figure 2

Hybrid system representation of the complementarity condition (48)

Full size image

To account for switching to slipping and sticking and between them, we should also consider their region of validity. Both sticking and slipping constitute constrained modes where \(h = 0\). The transition to them is, however, distinguished by the relative post-impact velocity \(\Delta v^{+}\). If the static friction is not able to maintain the sticking constraint, i.e., according to (19) when \(\sigma \leq0\), the system makes a transition to slipping. In the opposite direction, the transition depends on the relative velocity. However, (20) is undefined for zero relative velocity. This implies that there is no ideal transition condition between the slipping and sticking modes, although it is possible to calculate a continuous solution for the velocities. This can be resolved by conditioning the transition on \(\lVert\Delta v \rVert \leq\epsilon\). The switching then happens arbitrarily close to the actual transition by tuning the value of \(\epsilon\). To avoid switching directly back we should also make sure that \(\mathrm{d} \lVert\Delta v \rVert/\,\mathrm{d}t< 0\).

Note that switching on \(\lVert\Delta v \rVert\) greater than zero implies that the transition to the sticking mode is not possible. Similarly in the opposite direction, by the end of the sticking mode, the relative velocity is still equal to zero and a transition to slipping mode is not possible. A solution to these problems is to update \(\dot{q}\) and \(\omega\) such that the required condition on \(\Delta v^{+}\) is satisfied.

Since equalities cannot be detected exactly during simulation, in the implementation of the simulation models the equality conditions have to be replaced with inequality conditions on the residual of the errors. We consider a constant \(\epsilon> 0\) defining the numerical tolerance, e.g., \(\Delta v = 0\) is implemented as \(\lVert\Delta v \rVert \leq\epsilon\). Note that for numerical reasons, we also allow \(h(q)\) to drift in the in-contact modes.

3.2 Hybrid model

The transitions between the modes of (44)–(47c) happen according to the discussion of Sect. 3.1. To avoid extra complexity, the contact types (point and sphere or line and sphere) are not distinguished by additional modes. The final model is a hybrid, variable-index DAE system. Figure 3 illustrates the hybrid model. In the figure, post-impact relative velocities are used in the conditions for the transition from the free motion mode. This is, however, just for saving space. It can be assumed that (50) is the transition condition to an “In-contact” state where the impact law is applied. Consequently, depending on \(\lVert\Delta^{+} \rVert\) the transition to either sticking or slipping happens.

Fig. 3
figure 3

The hybrid system representing the ball and finger system. Only the subsystem shown in blue is used for finding optimal trajectories, limiting the possible solutions to free motion and sticking (Color figure online)

Full size image

4 Optimization

In this section, we discuss how the hybrid DAE in Fig. 3 can be used to find trajectories for the system using optimal control. In the optimization, the slipping mode and its transitions are not considered, i.e., we only consider free motion and sticking, which correspond to the parts in blue in Fig. 3. For many applications, this choice is motivated since slippage implies energy loss, which is preferably avoided. However, this hybrid DAE is ill-suited for straightforward application, because of the combination of nonlinear and hybrid dynamics. To deal with the hybrid dynamics, we use a mixture of multiple phases and smooth approximations to obtain a system model that is better suited for numerical optimization. The resulting equations are nonlinear and sufficiently differentiable to apply Newton-based methods.

4.1 Optimal control problem

To investigate optimal trajectories, we use the optimal control problem formulation

$$\begin{aligned} \text{minimize} & \phi \bigl(c(t_{f}) \bigr), \end{aligned}$$
(51a)
$$\begin{aligned} \text{with respect to} & x, y, u, t_{f}, x(0), \\ \text{subject to} & \mbox{dynamics of Fig.~3 in blue}, \end{aligned}$$
(51b)
$$\begin{aligned} & \dot{c} = L(x, u), \end{aligned}$$
(51c)
$$\begin{aligned} & \gamma \bigl(x(0), x(t_{f}) \bigr) = 0, \quad c(0) = 0, \end{aligned}$$
(51d)
$$\begin{aligned} & g(\dot{x}, x, y, u) \leq0, \end{aligned}$$
(51e)

where

$$ x = (q, dq,\omega), \qquad y = (h, \lambda, \lambda_{n}, \sigma), \qquad u = \tau $$
(52)

are the differential, algebraic, and control variables, respectively, \(t_{f}\) denotes the final time, \(c\) is additional state variables that model the objective defined by \(\phi\) and \(L\), which we describe further in Sect. 4.4, \(g\) provides path inequality constraints, which are described in Sect. 4.2, and \(\gamma \) are point constraints used to enforce, for example, initial and periodicity conditions. The initial state \(x(0)\) can also be a (partially) free optimization variable.

This general formulation can be used to, e.g., find optimal solutions to the following problems:

  • Reach the ball;

  • Reorient the ball;

  • Rotate the ball with a reference velocity;

  • Rotate the ball along an axis as fast as possible;

  • Find a periodic solution to keep the ball rotating.

4.2 From hybrid dynamics to multiple phases

The optimization problem (51a)–(51e) is not compatible with conventional methods for numerically solving optimal control problems as a result of the hybrid dynamics. To accurately treat the switching dynamics discussed in Sect. 3 in a differentiable way without making smooth approximations—which would lead to unacceptable inaccuracy—we introduce multiple phases as described in, e.g., [2, 6]. The time horizon \([0, t_{f}]\) is thus divided into \(N\) phases

$$ \bigl[t_{0}^{(1)}, t_{f}^{(1)} \bigr],\quad \dots,\quad \bigl[t_{0}^{(N)}, t_{f}^{(N)} \bigr] $$
(53)

such that

$$ t_{0}^{(1)} = 0, \qquad t_{f}^{(N)} = t_{f}, \qquad t_{0}^{(i+1)} = t_{f}^{(i)}, \quad i = 1, \dots, N-1. $$
(54)

Within each phase only a single, prescribed mode is active, and the mode is only allowed to switch at the phase boundaries.

For ease of notation, let

$$ z^{(i)} := \begin{bmatrix} \dot{x}^{(i)} & x^{(i)} & \dot{c}^{(i)} & c^{(i)} & y^{(i)} & u^{(i)} & {t} \end{bmatrix} ^{T}. $$
(55)

The dynamics within phase \(i\) are then described by the non-hybrid DAE

$$ F^{(i)} \bigl(z^{(i)} \bigr) = 0 $$
(56)

that corresponds to the mode that has been prescribed to the phase, that is, (45) or (46), as well as (44) and (51c).

The validity conditions are enforced by the path constraint \(g^{(i)}\). State and control inequality constraints can also be incorporated into \(g^{(i)}\). Event functions \(e^{(i)}\) are used to enforce mode transition conditions. The system variables in the respective phases are connected by linkage constraints \(\psi\) corresponding to the reset maps of Fig. 3.

The hybrid dynamic optimization problem (51a)–(51e) is thus transformed into the non-hybrid, multiple phase problem

$$\begin{aligned} \text{minimize} & \phi \bigl(c^{(N)}(t_{f}) \bigr), \end{aligned}$$
(57a)
$$\begin{aligned} \text{w.r.t.} & x^{(i)}, c^{(i)}, y^{(i)}, u^{(i)}, t_{f}^{(i)}, x_{0}^{(i)}, \\ \text{subject to} & F^{(i)} \bigl(z^{(i)} \bigr) = 0, \end{aligned}$$
(57b)
$$\begin{aligned} & e_{L}^{(i)} \leq e^{(i)} \bigl({z^{(i)} \bigl(t_{0}^{(i)} \bigr)}, z^{(i)} \bigl(t_{f}^{(i)} \bigr) \bigr) \leq e_{U}^{(i)}, \end{aligned}$$
(57c)
$$\begin{aligned} & \psi_{L} \leq\psi \bigl(z^{(1)} \bigl(t_{0}^{(1)} \bigr), z^{(1)} \bigl(t_{f}^{(1)} \bigr), \\ & \vdots \\ & \phantom{\psi_{L} \leq\psi(}z^{(N)} \bigl(t_{0}^{(N)} \bigr), z^{(N)} \bigl(t_{f}^{(N)} \bigr) \bigr)\leq \psi_{U}, \end{aligned}$$
(57d)
$$\begin{aligned} & g^{(i)} \bigl(z^{(i)} \bigr) \leq0, \\ & i = 1, \dots, N, \end{aligned}$$
(57e)

where \(e_{L}^{(i)}\), \(e_{U}^{(i)}\), \(\psi_{L}\), \(\psi_{U}\) are proper constants. Any remaining point constraints given by (51d) have also been incorporated into either the event constraints (57c) or the linkage constraints (57d).

4.3 Smooth saturation

Although (57a)–(57e) is not hybrid, it is still not suitable for dynamic optimization because of the non-differentiable equations resulting from the saturation function in (3). While this can be handled by introducing further phase changes for whenever a junction point of the saturation function is crossed—i.e., when the potential contact point shifts between the fingertip to the side of the finger—this approach is computationally expensive and also makes it more difficult to determine the optimal phase sequence. We thus instead choose to approximate the saturation function by noting that

$$\begin{aligned} \mbox{sat}_{L}^{U}(x) = \frac{1}{2} \bigl(L + U + \lvert x-L \rvert - \lvert x-U \rvert \bigr) \end{aligned}$$
(58)

and then using the approximation

$$ \lvert x \rvert \approx\sqrt{x^{2} + \epsilon^{2}}, $$
(59)

where \(\epsilon\) is a small number, which we set equal to 0.01. Figure 4 compares the smooth approximation with the ideal saturation, as well as the hyperbolic tangent function with an appropriate shifting and scaling. The hyperbolic tangent is another function commonly used to model saturation behavior [25]. Note that a significantly better approximation by scaling the hyperbolic tangent function is not possible since it requires changing either the slope or the saturation limits.

Fig. 4
figure 4

Approximation of the saturation function with the lower limit of −1 and the upper limit of 0. The curves have point symmetry around \((-0.5,-0.5)\). Hence, only the first quadrant is shown. The green curve approximates the saturation function by scaling and shifting tanh. We observe that for \(\epsilon= 0.01\), the smooth approximation of the saturation function is hardly distinguishable from the saturation function (Color figure online)

Full size image

4.4 Objective

Out of the various archetype problems mentioned in Sect. 4.1, we focus on the final one of finding a periodic trajectory, viz., \(x(0) = x(t_{f})\), that achieves a certain rotation of the ball. We use the objective function

$$\begin{aligned} \phi \bigl(c(t_{f}) \bigr) &:= \frac{c_{1}(t_{f})}{c_{2}(t_{f})}, \end{aligned}$$
(60)

where \(c_{1}\) and \(c_{2}\) denote some cost states. Accordingly, \(\phi (c(t_{f}))\) is the ratio between the total accumulated costs during the time horizon \([0, t_{f}]\). We will consider two different objective functions, hence two different ways of defining \(c_{1}\) and \(c_{2}\). The first is cost of transport (COT), which can be expressed by the two cost states

$$ \dot{c}_{1}^{\mathrm{COT}} := \lVert u \rVert^{2}, $$
(61)

which quantifies the cost of control action, and a measure of the amount of rotation in a desired direction

$$ \dot{c}_{2}^{\mathrm{COT}} := -w^{\mathrm{COT}} \bigl\lVert \underline{\omega}^{\mathrm{ref}} \bigr\rVert ^{2} + \omega\cdot \hat{ \omega}^{\mathrm{ref}}, $$
(62)

where \(\hat{\omega}^{\mathrm{ref}}\) is a unit vector determining the desired direction of angular velocity,

$$ \underline{\omega}^{\mathrm{ref}} := \omega- \bigl(\omega\cdot \hat{ \omega}^{\mathrm{ref}} \bigr) {\hat{\omega}^{\mathrm{ref}}} $$
(63)

is the component of the angular velocity perpendicular to the desired direction, and \(w^{\mathrm{COT}}\) is used to weight the two quantities. The second objective is a more conventional quadratic penalty on the input weighted by \(w^{\mathrm{QC}}\) and on the deviation from a reference rotational velocity, \(\omega^{\mathrm{ref}}\), expressed by the cost states

$$ \dot{c}_{1}^{\mathrm{QC}} := w^{\mathrm{QC}} \lVert u \rVert^{2} + \bigl\lVert \omega- \omega ^{\mathrm{ref}} \bigr\rVert ^{2}, $$
(64)

and a state for calculating the total period

$$ \dot{c}_{2}^{\mathrm{QC}} := 1. $$
(65)

The cost of transport defined here is motivated by the energy efficiency of the rotation and is similar to the energy index used for comparing the efficiency of various transport systems [31]. The average rotational speed is thus determined by the optimization. On the other hand, the quadratic objective function is intended for achieving a certain average rotational speed while allowing some trade-off for the average power. The fact that the quadratic cost function favors angular velocities close to the constant reference velocity leads to short periods. This is understood by noting that the speed of the ball cannot be influenced in the free motion and drops rapidly.

5 Implementation

The equations of Appendices A and B were derived with the use of Maple. The multiphase problem (57a)–(57e) was implemented in PSOPT [3]. The Legendre–Gauss pseudospectral collocation of PSOPT was used to transcribe the problem into a twice continuously differentiable nonlinear program (NLP) with 30 collocation points for each phase. The NLP was solved with IPOPT [34] and the linear solver MA27 [18].

Simulation of the system including slipping was performed using Modelica [14] and Dymola [11]. Dymola does not currently support hybrid systems with variable index, although there has been recent work to resolve this [24]. One possible way to manually resolve this is to perform index reduction in the high-index modes, but this is challenging for the considered system because of the impossibility of statically selecting suitable state variables. Hence, we instead adopted a more cumbersome approach where three instances of the system were simulated simultaneously, one low-index for free motion and two high-index (with dynamic state selection [23]) for modes in contact. A correct system model could thus be created by appropriate communication between the three instances during mode switching.

The trajectories from both optimization and simulation were finally imported into MATLAB for visualization. For 3D visualization of the finger and its movements, Peter Corke’s robotic toolbox [9] was used.

6 Results

This section presents results for the problem of finding a periodic solution for rotating the ball. We first present optimal trajectories obtained by neglecting slipping, and then briefly show the solutions obtained by simulating the system with the open-loop optimal inputs while taking into account slippage. For the numerical experiments, we use \(r = \ell= 1~\mbox{m}\), \(m = 20~\mbox{g}\), and \(I_{b} = \operatorname{diag}(1.0677,2.3177,2.0833 )~\mbox{g}\,\mbox{s}^{-2}\) for all the links.

6.1 Optimal control

We considered the two objectives discussed in Sect. 4.4. We divided each period into three phases: free motion, sticking, and finally free motion again with the constraint \(x^{(3)}(t_{f}) = x^{(1)}(0)\). The initial state as well as the terminal and switching times \(t_{f}^{(i)}\) were free. We also imposed the initial constraint \(h(0) \geq0.3r\) for robustness, i.e., the finger should go sufficiently far away from the ball before making a new contact. The weights \(w^{\mathrm{COT}}\) and \(w^{\mathrm{QC}}\) were optionally set to 10 and 1, respectively. As the desired direction of angular velocity \(\hat{\omega}^{\mathrm{ref}}\), we chose \(1/\sqrt{2}(1, -1, 0)^{T}\) for the cost of transport and we used \(\omega ^{\mathrm{ref}} = (0, 0, 2.2)^{T}\) as the angular velocity reference for the quadratic objective. We impose an arbitrary torque limit of 1 N m.

A simple initial guess was sufficient to numerically solve the problem. The initial guess consisted of a constant non-zero \(q \equiv q^{0}\) and \(t_{f}^{(i)} = i\), with all the remaining variables being 0.

We present the results of three scenarios. In the first two scenarios, we solved the problem of optimizing the COT. The difference between the scenarios is that in the first one we included no constraint on the joint angles, while in the second one the constraint \(q_{i} \geq0\) for \(i \in\{2,3\}\) was enforced in order to comply with the limitations of the human finger. Figure 5 illustrates the optimal path obtained for the robotic finger. In Fig. 6, we see the resulting optimal angular velocities of the ball and switching times. The discontinuities at the impact point are visible.

Fig. 5
figure 5

Visualization of the ball-and-finger system. The blue solid curve shows the optimal path of \(p_{c}\) for the COT when there is no joint limit. The red dashed curve corresponds the optimal path for the fingertip (Color figure online)

Full size image
Fig. 6
figure 6

The optimal angular velocity of the ball \(\omega\) for the COT. The vertical lines show the switching time for the phases

Full size image

The minimum distance \(h\) between the finger and the ball is shown in Fig. 7. Moreover, the distance between the contact point and the fingertip is plotted. As we see, the contact point is located somewhere on the link and shifts slightly during the rotation. The required torques and the interaction force are given in Figs. 8 and 9, respectively.

Fig. 7
figure 7

The minimum distance to the ball, \(h\), and the distance of the contact point to the fingertip. The finger is in contact with the ball in the interval between the vertical lines. The side of the finger is used for rotating the ball and the contact point shifts slightly during rotation

Full size image
Fig. 8
figure 8

The optimal control signal for minimizing the COT: The vertical lines show the switching time for the phases. Toward the end of the contact phase, the bounds on the control signals become active

Full size image
Fig. 9
figure 9

Interaction forces for the COT: The tangential and normal components of the interaction force are shown. In this case, there is always some margin to slipping

Full size image

Figure 10 illustrates the human finger in its initial position. In this case, the fingertip always remains the closest point to ball. The path obtained from the movement of the closest point is shown in blue. The paths of the closest points of the human finger and the robotic finger are compared in Fig. 11.

Fig. 10
figure 10

Visualization of the ball and finger system. The blue curve shows the optimal path of \(p_{c}\) for the COT, complying with the human joint limits. The contact point is always the fingertip (Color figure online)

Full size image
Fig. 11
figure 11

The paths of \(p_{c}\) obtained using the COT optimization criterion. The red and blue curves correspond to human and robotic fingers, respectively. The parts of the paths in contact with the ball are dashed (Color figure online)

Full size image

Assuming the COT, the robotic finger was able rotate the ball in the desired direction more efficiently. It used the side of the finger, while the human finger contacted with the fingertip only.

Figure 12 shows the finger in its initial position and the path obtained for rotating the ball around the \(z\)-axis using the quadratic objective function. The angular velocity obtained is illustrated in Fig. 13. As we can see, the average angular velocity matches the desired \(\omega_{\mathrm {ref}}\). The period of the solution compared to Fig. 6 is shorter. The minimum distance as a function of time and the distance of the contact point to the fingertip are shown in Fig. 14. In this solution, only the fingertip is used for rotating the ball. Finally, the torques and interaction forces are depicted in Figs. 15 and 16, respectively. As we see, the constraint on the friction cone is always active during contact. Hence, the motion is always on the verge of slipping.

Fig. 12
figure 12

Visualization of the ball and finger system. The blue curve shows the optimal path of \(p_{c}\) for the quadratic cost. The objective is to rotate the ball around the \(z\)-axis (Color figure online)

Full size image
Fig. 13
figure 13

The optimal angular velocity of the ball \(\omega\) for the quadratic cost. The vertical lines show the switching time for the phases

Full size image
Fig. 14
figure 14

The minimum distance to the ball, \(h\), and the distance of the contact point to the fingertip. The finger is in contact with the ball in the interval between the vertical lines. Only the fingertip is used for rotating the ball

Full size image
Fig. 15
figure 15

The optimal control signal for minimizing the quadratic cost: The vertical lines show the switching time for the phases. Note that the bounds on the control signal are only active when making contact or breaking it

Full size image
Fig. 16
figure 16

Interaction forces for the quadratic cost: The tangential and normal components of the interaction force are shown. In this case, there is no margin to slipping

Full size image

6.2 Open-loop simulation

We now use the open-loop optimal torques for the COT criterion without non-negativity bounds on the joints from Fig. 8 to simulate the full system in Fig. 3 in Dymola. The resulting angular velocities are shown in Fig. 17 and compared with the optimal angular velocities. We see that open-loop angular velocities match well at the beginning, but drift from the optimum as time passes. There is a short window of time immediately following collision in which slipping occurs, which is almost always the case when employing Newton’s impact law. The dynamics quickly switch to sticking because of the high interaction forces. There is a jump in the post-impact velocities in the optimization solution, while using the Newton’s impact law, the velocities are continuous in the simulation result.

Fig. 17
figure 17

The open-loop simulation of the complete system in Fig. 3 using the control signals in Fig. 8. The angular velocities of the ball (solid blue curves) are compared with the optimization result (red dashed curves). The vertical lines show the switching times obtained from the optimization. The time regions corresponding to the slipping and sticking modes of the simulated result are shaded in purple and gray, respectively. We see that the contact times are slightly different. However, there is no difference in the time that the finger leaves the ball (Color figure online)

Full size image

7 Discussion

The ball-and-finger system is modeled with three modes, each described with a different set of equations. In contrast to some of the results for finding optimal trajectories reported in the literature (see, for example, [20, 30]), in our approach the connection between different modes happens through phases. Thus, it is not required to compromise the optimal solution by biasing the cost function toward making contact. On the other hand, we have fixed the order of the phases. This can be relaxed by trying all the combinations. For more complex systems, a hierarchical planning scheme can be used in order to determine the phases [15].

There are several possible approaches to treating hybrid dynamics in dynamic optimization other than the multiphase framework. A natural alternative in the considered case is the incorporation of the complementarity constraints [1] in the optimization problem. However, it is not always easy to rewrite the non-smooth behavior using complementarity conditions, e.g., switching between slipping and sticking when the friction coefficients are not the same. Other possible approaches are mixed-integer nonlinear programming based on branch-and-bound [4] or sum-up-rounding [19] or widely applicable derivative-free methods such as genetic algorithms [33]. The various approaches have their own strengths and weaknesses. The strength of the multiphase framework is the efficiency of the required computations, which may enable online solution of these problems. A significant drawback is that the phase sequence needs to be determined a priori.

In optimization, it is not always suitable to neglect the slippage completely. It can be ignored when the interaction forces are high. However, when the slipping margin \(\sigma\) is small, it becomes more important to consider the effects of slipping. If the slipping mode is neglected, it is also important to ensure that the collision itself does not give rise to slipping. When using Newton’s impact law, this is possible by constraining the relative tangential velocity at the collision point to be zero. This will guarantee a transition to the sticking mode upon collision, though it might not be the optimal solution. Another solution is to constrain the resulting impulse \(\iota\) so that the friction is sufficient to support the sticking mode. Similar to (19), this can be formulated as

$$\begin{aligned} \lVert\iota_{\parallel} \rVert \leq\mu_{s} \iota\cdot n, \end{aligned}$$
(66)

and enforced in the optimization using event constraints. In our example with the quadratic objective function in Sect. 6.1, adding this constraint did not have a significant effect on the solution. For the COT objective function, the velocity of the finger normal to the ball was prior to the collision increased to ensure enough friction while the rest of the variables were similar. This suggests that introducing limits on the normal impulse might also be required to avoid bouncing back in practice.

Another option for handling impacts is to add flexible components such as a non-rigid ball or a padded arm. This leads to a smooth approximation of discontinuities, which does not require the exact contact times to be identified. For example, the normal force can be considered proportional to the depth of penetration of \(p_{c}\), i.e., \(\lambda_{n} = - k \min(h(q),0) n\) could be employed. This is, however, not suitable when the contact stiffness is high, since a computationally intractable amount of discretization points may be required to be able to capture rapid changes in the forces. Note that, irrespective of this assumption, it is possible to constrain the amount of normal impulse in the optimization such that the desired behavior is achieved.

Contrary to assumption 6, real contacts have some degree of elasticity. However, given the fact that both robotic and human fingers have relatively high impedance, the normal impact at the collision time is absorbed and the finger would not bounce back. Therefore, an inelastic model is able to describe the overall behavior well for practical collision velocities.

Open-loop control is generally not satisfactory, even if slippage can be neglected. Feedback is needed to handle model uncertainties, such as an inaccurate collision model, as well as discretization errors. Different controllers depending on the mode of the system may be employed. Specifically, in free motion, a position controller with feedforward torques can be used. As soon as the finger comes into contact with the ball, the controller can be switched to a hybrid position/force controller. To maintain the sticking condition, the normal force should be regulated while the velocity in the tangential direction is being tracked.

In simulation, inconsistent solutions due to the Painlevé paradox [8] may arise when the kinetic friction constant between the ball and the finger is large. In this case, without a proper reinitialization of the states, it is not possible to find consistent solutions with the complementarity conditions (48). However, this will not be an issue in the optimization because of the infeasibility of such solutions.

The optimization results presented in Sect. 6 were obtained using simple initial guesses. However, warm starting the optimization from previous runs with simplified cost functions can often be beneficial. This way it is possible to direct the solution gradually to the region of interest.

The path constraint related to the validity condition \(h \geq0\) is difficult to treat numerically. In phase 1, the natural formulation is \(h^{(1)} \geq0\) and \(h^{(1)}(t_{f}) = 0\). However, such a formulation violates constraint qualifications, leading to unbounded dual variables [29]. Ideally, this would be handled by only enforcing \(h^{(1)} \geq0\) in all but the last collocation point of phase 1, which instead is determined by the event constraint \(h^{(1)}(t_{f}^{(1)}) = 0\), but this is not readily supported by PSOPT. Hence, we work around this issue by replacing the event constraint by \(0 \leq h^{(1)}(t_{f}) \leq\epsilon:= 10^{-4}\). Further problems related to this are encountered in phase 3. In phase 2, \(\dot{h} = 0\) is a solution invariant, which is, however, not preserved during discretization. Numerical drift-off can thus lead to \(h^{(2)}(t_{f}) < 0\) (enforcing \(h^{(2)} \geq0\) would once again lead to violation of constraint qualifications). To circumvent this, we replace the validity condition of phase 3 by \(h^{(3)} \geq-\epsilon\).

The proposed framework is not robust with respect to avoiding contact in the free motion mode, as it allows for free motion arbitrarily close to the ball. A simple remedy for the considered examples is to also enforce \(\dot{h} \leq-\epsilon\) in the first phase and \(\dot{h} \geq\epsilon\) in the third phase, which may, however, sacrifice optimality. Future work is to design a more sophisticated solution by considering robust approaches to optimization and control such as those in [5, 27].

8 Conclusion

In this article, we propose a ball-and-finger system as a realistic example to study common robotic tasks involving interaction and varying dynamics. The result of modeling is a hybrid, variable-index differential-algebraic equation system. To find the optimal trajectories for performing a task, we used an integrated approach, i.e., there is no separate process for finding the required path. A multiphase formulation was used to handle changes in the dynamics in each mode. The benefit of this approach is that there is no need to compromise the desired objective function in order to establish contact.

As the result of the optimization, we find not only the trajectory for the motion but also the interaction forces. This is an important feature considering the fact that robots are often employed for manipulation tasks, which involves interaction with other objects. The extension of our model with several fingers opens up the opportunity to perform automatic motion planning for dexterous manipulation, e.g., for assembly tasks.

From the modeling, three modes of behavior from the system are expected, which may require three different strategies for control. Suitable feedback signals are the minimum distance to the ball, the margin to slipping, and the relative velocity at the contact point. In the contact situation, controlling the force in the normal direction to the ball is important in order to maintain the contact as well as to make the system more robust with respect to the variations of the impact law.