2.1. Quasi-static mechanical manipulation system

As illustrated in Figure 1, starting with (Campolo and Cardin, Reference Campolo and Cardin2023a, Reference Campolo and Cardin2023b), the key steps for analyzing a manipulation task involve (i) splitting of degrees of freedom into nondirectly controllable variables (e.g., environment) $ \boldsymbol{z}\in \mathcal{Z}\subset {\mathrm{\mathbb{R}}}^N $ and directly controllable variables $ \boldsymbol{u}\in \mathcal{U}\subset {\mathrm{\mathbb{R}}}^K $ . (ii) defining a manipulation potential $ W\left(\boldsymbol{z},\boldsymbol{u}\right) $ :

(2.1) $$ {\displaystyle \begin{array}{l}W:{\mathrm{\mathbb{R}}}^N\times {\mathrm{\mathbb{R}}}^K\to \mathrm{\mathbb{R}},\\ {}\hskip3em \left(\boldsymbol{z},\boldsymbol{u}\right)\mapsto \sum \limits_i\;{W}_i^{el}\left(\boldsymbol{z},\boldsymbol{u}\right)+U\left(\boldsymbol{z}\right)\end{array}} $$

Assuming there are $ i $ contact points in total, each contact point is symbolized as $ {\boldsymbol{c}}_i\left(\boldsymbol{z}\right) $ . The body $ \mathrm{\mathcal{B}} $ , which denotes the internal states $ \boldsymbol{z} $ , decides these contact points $ {\boldsymbol{c}}_i\left(\boldsymbol{z}\right) $ . The control inputs $ {\boldsymbol{u}}_i $ , which connect to the contact points via a virtual spring, capture these interactions. In general, the dimensionality of the control space is often smaller than the dimensionality of the uncontrollable state, since $ K $ depends on the number of actuators.

Figure 1. Robots manipulate an object under equilibrium. The object $ \boldsymbol{z} $ is an indirectly controllable variable. Robots contact with the object at contact points $ {\boldsymbol{c}}_i\left(\boldsymbol{z}\right) $ , and control inputs $ {\boldsymbol{u}}_i $ denote the desired position of the robot. Virtual springs represent impedance control.

The configuration of the system is then determined solely by its manipulation potential $ W\left(\boldsymbol{z},\boldsymbol{u}\right) $ . The potential of objects $ U\left(\boldsymbol{z}\right) $ includes the contact energy between objects and the gravity potential. The elastic potential $ {W}_i^{el}\left(\boldsymbol{z},\boldsymbol{u}\right) $ captures the robot–environment interaction in terms of non-linear yet smooth interactions elastic potentials, which is crucial, and corresponds to a regularisation of otherwise non-smooth mechanical interaction. In particular, mechanical contact is modeled using nonlinear springs that exhibit high stiffness when the robot end-effector penetrates objects in the environment and highly flexible otherwise, with smooth transitions in-between. Each spring energy can be defined as

(2.2) $$ {W}_i^{el}\left(\boldsymbol{z},\boldsymbol{u}\right)=\frac{1}{2}\parallel {\boldsymbol{u}}_i-{\boldsymbol{c}}_i\left(\mathbf{z}\right){\parallel}_{{\boldsymbol{K}}_i}^2 $$

where $ \parallel \boldsymbol{a}{\parallel}_{\boldsymbol{A}}^2:= {\boldsymbol{a}}^T\boldsymbol{Aa} $ denotes the Mahalanobis distance, and $ {\boldsymbol{K}}_i $ denotes the stiffness matrix for the virtual spring. Once a robot-environment potential function $ W\left(\boldsymbol{z},\boldsymbol{u}\right) $ is defined for a problem, as outlined in (Campolo and Cardin, Reference Campolo and Cardin2023a, Reference Campolo and Cardin2023b), quasi-static manipulation can be seen as an optimization problem taking place on the so-called Equilibrium Manifold (EM) $ {\mathcal{M}}^{eq} $ , defined as the subspace space of mechanical equilibria with equilibrium condition as

(2.3) $$ {\nabla}_{\boldsymbol{z}}W\left({\boldsymbol{z}}_m^{\ast },\boldsymbol{u}\right)=\mathbf{0}\in {\mathrm{\mathbb{R}}}^N $$

We define $ {\nabla}_qW\equiv {\left[{\partial}_{q_1}W,\dots, {\partial}_{q_a}W\right]}^T $ , where the nabla (column) operator is defined as $ {\nabla}_q={\left[{\partial}_{q_1},\dots, {\partial}_{q_a}\right]}^T $ . Meanwhile, define the shorthand notation $ {\nabla}_z^2\equiv \left({\nabla}_z{\nabla}_z^T\right)={\nabla}_z{\nabla}_z^T $ for Hessians and mixed-derivative operators. Meanwhile, $ {\boldsymbol{z}}_m^{\ast } $ represents the potentially multiple solutions, with $ m\ge 1 $ indicating the multiplicity of equilibria. In other words, for the same control input of the system, the uncontrollable state can have multiple situations. As such, the EM is defined when $ \boldsymbol{u} $ is seen as a parameter, that is,

(2.4) $$ {\mathcal{M}}^{eq}\left\{\left(\boldsymbol{z},\boldsymbol{u}\right)\in \mathcal{Z}\times \mathcal{U}|{\nabla}_{\boldsymbol{z}}W\left(\boldsymbol{z},\boldsymbol{u}\right)=\mathbf{0}\right\} $$

is a smooth embedded submanifold in the ambient space $ \mathcal{Z}\times \mathcal{U} $ . The state transitions are purely controlled by the robotic agent $ \boldsymbol{u} $ . Meanwhile, A point is strictly stable when its Hessian is positive definite, i.e., $ {\left.{\nabla}_{\boldsymbol{zz}}^2W\right|}_{\ast}\succ 0 $ . Meanwhile, following (Campolo and Cardin, Reference Campolo and Cardin2023a, Reference Campolo and Cardin2023b), the haptic distance $ S $ between any two points $ {\boldsymbol{u}}_0 $ to $ {\boldsymbol{u}}_1 $ on control space is defined as,

(2.5) $$ S\left[\boldsymbol{u}(c)\right]={\int}_{{\boldsymbol{u}}_0}^{{\boldsymbol{u}}_1}\sqrt{\frac{d{\boldsymbol{u}}^T}{dc}{\boldsymbol{G}}_m^2\left(\boldsymbol{u}\right)\frac{d\boldsymbol{u}}{dc}}\hskip0.1em dc, $$

where $ S\left[\boldsymbol{u}(c)\right] $ represents the robot’s cost when following a control policy $ \boldsymbol{u}(c) $ . The greater the force required by the robot during manipulation, the larger the value of $ S $ , and $ {\boldsymbol{G}}_m\left(\boldsymbol{u}\right) $ is the control Hessian (Campolo and Cardin, Reference Campolo and Cardin2023a, Reference Campolo and Cardin2023b) defined at equilibria $ \left({\boldsymbol{z}}_m^{\ast },\boldsymbol{u}\right) $ :

(2.6) $$ {\boldsymbol{G}}_m\left(\boldsymbol{u}\right)\hskip0.2em :\hskip-0.45em =\hskip0.2em {\nabla}_{\boldsymbol{uu}}^2W-{\nabla}_{\boldsymbol{uz}}^2W{\left({\nabla}_{\boldsymbol{zz}}^2W\right)}^{-1}{\nabla}_{\boldsymbol{zu}}^2W $$

2.2. Numerical continuation approach

In differential geometry, it is established that a manifold can be characterized by a set of local parametrizations known as charts, which are collectively organized within an atlas (Do Carmo, Reference Do Carmo2016). (Henderson, Reference Henderson2002) introduced a continuation method to explore a manifold which is implicitly defined by a series of overlapping neighborhoods. The concept involves covering a flat manifold, $ {\mathrm{\mathbb{R}}}^k $ , with spherical balls, or projecting from the tangent space onto a general manifold. Therefore, each chart is defined as $ {\mathcal{C}}_i\left({\boldsymbol{u}}_i\right) $ , including a center $ {\boldsymbol{u}}_i $ and a polygon $ {P}_i^m $ as an approximation, where $ m $ denotes the number of steps.

The relationships between these balls can be categorized based on their spatial interactions: they may be external to each other, one ball may be completely inside another, or they may intersect. These relationships are determined by comparing the distance between the centers of two balls with their radius $ R $ . To compute the intersection of a sphere that lies within a convex polyhedron defined by the intersection of half spaces, Henderson employs a finite convex polyhedral, $ {C}_{R_i}\left({u}_i\right) $ , to cover each spherical ball. Consequently, the boundary of this collection of balls is formed by the union of all the individual ball boundaries and the polygons. Efficient exploration of the entire space hinges on expansion along these boundaries. A practical approach involves extending from the exterior vertices of the polygons. Here, “exterior” refers to vertices where the distance from the vertex to the center exceeds the radius (Eq. 2.7). To manage this process, a waiting list for spheres is maintained, which helps in prioritizing expansion points.

(2.7) $$ {B}_{\mathrm{list}}^m=\left\{i|\exists \boldsymbol{v}\in \mathrm{vertex}\left({P}_i^m\right)\mathrm{s}.\mathrm{t}.|\boldsymbol{v}|>{R}_i\right\} $$

From this exterior vertex, a new ball is generated along with its corresponding polygon. Subsequently, a trimming function is applied to reduce the overlapping volume between nearby polyhedra.

(2.8a) $$ {P}_i^m=\{\begin{array}{ll}\mathrm{\varnothing}& \mathrm{i}\mathrm{f}\hskip0.32em \mathrm{f}\mathrm{o}\mathrm{r}\hskip0.32em \mathrm{a}\mathrm{n}\mathrm{y}\hskip0.32em j\in {J}_i^m,\mid {\boldsymbol{u}}_j-{\boldsymbol{u}}_i\mid <{R}_j-{R}_i,\\ {}{C}_{R_i}({\boldsymbol{u}}_i){\cap}_{j\in {J}_i^m}({HS}_{ij})& \mathrm{o}\mathrm{t}\mathrm{h}\mathrm{e}\mathrm{r}\mathrm{w}\mathrm{i}\mathrm{s}\mathrm{e}\end{array}\operatorname{} $$

(2.8b) $$ {HS}_{ij}=\left\{\boldsymbol{u}|\left(\boldsymbol{u}-{\boldsymbol{u}}_i\right)\cdot \left({\boldsymbol{u}}_j-{\boldsymbol{u}}_i\right)\le \frac{1}{2}\left({R}_i^2-{R}_j^2+{\left|{\boldsymbol{u}}_j-{\boldsymbol{u}}_i\right|}^2\right)\right\} $$

This entails identifying the intersection or cross set between a half-space ( $ {HS}_{ij} $ ) and the polyhedron. This step of the framework is called PolyTrim. For any two polyhedra, we trim them by Algorithm 1.

Algorithm 1 Trim two polyhedra

Subsequently, the classical numerical continuation approach to move on the constrained manifold is depicted in Figure 2. Each chart $ {C}_i $ is approximated by polygon $ {P}_i $ , with a sphere with radius $ {R}_i $ . During the (Henderson, Reference Henderson2002) process, two projection steps are performed on the manifold. One is to project the black circle in the tangent space at $ {\boldsymbol{u}}_j $ to manifold as the red circle. The other is to project the black circle onto the tangent space at $ {\boldsymbol{u}}_i $ as the pink circle. The former is used to cover the manifold, the latter is aimed at finding the half space $ {HS}_{ij} $ to execute the trimming process. The projection step requires iterations to find the solution on the manifold. Moreover, each step during trim and growth requires a projection operation, and this projection-based growth is widely used in other constrained motion planning techniques (Jaillet and Porta, Reference Jaillet and Porta2012; Kingston et al., Reference Kingston, Moll and Kavraki2019).

Figure 2. Chart $ {C}_i $ and $ {C}_j $ are approximated by polygon covering circle with centers $ {\boldsymbol{u}}_i $ and $ {\boldsymbol{u}}_j $ . Each chart $ {C}_i $ is expressed in the tangent space at is center $ {\boldsymbol{u}}_i $ . Black circle in the tangent space at $ {\boldsymbol{u}}_j $ is projected to the manifold as the red circle, then onto the tangent space at $ {\boldsymbol{u}}_i $ as the pink circle. The half space $ {HS}_{ij} $ pass through the intersection points between the green circle and the pink circle.

3.1. Exploring via adaptive ODE

The Equilibrium Manifold (EM) $ {\mathcal{M}}^{eq} $ is therefore implicitly defined and, given an initial solution $ \left({\boldsymbol{z}}_0,{\boldsymbol{u}}_0\right)\in {\mathcal{M}}^{eq} $ , a first-order approximation on the equilibrium manifold is given as,

(3.1) $$ \delta \boldsymbol{z}=-{\left({\nabla}_{\boldsymbol{zz}}^2W\right)}^{-1}{\nabla}_{\boldsymbol{uz}}^2 W\delta \boldsymbol{u} $$

Eq. 3.1 is depicted as a blue arrow in Figure 3(a), which illustrates the linear relationship with an infinitesimal change between $ \boldsymbol{z} $ and $ \boldsymbol{u} $ . The calculation of an implicit manifold from a given set of (nonlinear) equations invariably relies on standard iterative methods such as Newton–Raphson, as in the case of (Henderson, Reference Henderson2002; Jaillet and Porta, Reference Jaillet and Porta2012). Recently, it was shown that the classical iterative Newton–Raphson method can be transformed into an adaptive ODE (Schneebeli and Wihler, Reference Schneebeli and Wihler2011). Therefore, based on quasi-static assumptions, we propose evaluating the evolution $ t\to \boldsymbol{z}(t)\in {\mathrm{\mathbb{R}}}^N $ whenever the control parameters $ t\to \boldsymbol{u}(t)\in {\mathrm{\mathbb{R}}}^K $ are (quasi-statically) varied by (numerically) solving a set of adaptive ODE. The Newton-Raphson “infinitesimal” adjustments, when $ u=\mathrm{const} $ gives rise to the out-of-equilibrium dynamics, depicted as a red arrow in Figure 3(a).

(3.2) $$ \delta \boldsymbol{z}=-\alpha {\left({\nabla}_{\boldsymbol{z}\boldsymbol{z}}^2W\right)}^{-1}{\nabla}_{\boldsymbol{z}} W\delta t, $$

where $ \alpha $ represents step size in the ODE (Schneebeli and Wihler, Reference Schneebeli and Wihler2011), which influences the convergence of the ODE. If $ \alpha $ is too small, the ODE may fail to converge within the given time. Conversely, if $ \alpha $ is too large, the ODE might jump to a different branch of the EM.

Figure 3. The evolution of physic-informed adaptive ODE exploring manifold.

Clearly, if out of equilibrium (i.e. $ {W}_z=0 $ ), for a fixed u and by virtue of the positive-definiteness of the Hessian $ {W}_{zz}>0 $ , this term drives the variables toward the equilibrium manifold $ {\mathcal{M}}^{eq} $ .

From a given initial solution $ \left({\boldsymbol{z}}_0,{\boldsymbol{u}}_0\right)\in {\mathcal{M}}^{eq} $ , a new nearby solution $ \left({\boldsymbol{z}}_1,{\boldsymbol{u}}_1\right) $ for a new parameter $ {u}_1 $ at constant rate $ \dot{\boldsymbol{u}}=\left({\boldsymbol{u}}_1-{\boldsymbol{u}}_0\right)/T $ , can be evaluated by numerical integration of the adaptive ODE. Hence, the adaptive ODE becomes

(3.3) $$ \frac{d}{dt}\left[\begin{array}{c}\boldsymbol{z}\\ {}\boldsymbol{u}\\ {}\phi \end{array}\right]=\left[\begin{array}{c}-{\left({\nabla}_{\boldsymbol{z}\boldsymbol{z}}^2W\right)}^{-1}{\nabla}_{\boldsymbol{u}\boldsymbol{z}}^2W\dot{\boldsymbol{u}}-\alpha {\left({\nabla}_{\boldsymbol{z}\boldsymbol{z}}^2W\right)}^{-1}{\nabla}_{\boldsymbol{z}}W\\ {}\left({\boldsymbol{u}}_1-{\boldsymbol{u}}_0\right)/T\\ {}\sqrt{{\dot{\boldsymbol{u}}}^T{\boldsymbol{G}}_m^2\left(\boldsymbol{u}\right)\dot{\boldsymbol{u}}}\end{array}\right],\left[\begin{array}{c}\boldsymbol{z}(0)\\ {}\boldsymbol{u}(0)\\ {}\phi (0)\end{array}\right]=\left[\begin{array}{c}{\boldsymbol{z}}_0\\ {}{\boldsymbol{u}}_0\\ {}0\end{array}\right] $$

Meanwhile, we introduce the concept of a haptic obstacle,

(3.4) $$ \det \left({\nabla}_{zz}^2W\left({\boldsymbol{z}}^{\ast}\left(\boldsymbol{u}\right),\boldsymbol{u}\right)\right)\ge \lambda >0 $$

where $ \lambda $ is a threshold. The haptic constraint (Eq. 3.4) is imposed because the inverse of the Hessian is required, so $ -{\left({\nabla}_{\boldsymbol{zz}}^2W\right)}^{-1} $ should be sufficiently small to avoid significant changes of $ \boldsymbol{z} $ . Meanwhile, $ \lambda >0 $ indicates the Hessian is positive definite, suggesting the node is stable on the equilibrium manifold. The stable points are also referred to as nondegenerate critical points (Poston and Stewart, Reference Poston and Stewart2014). The adaptive ODE with haptic obstacles always starts from a stable point and avoids exploring unstable critical points, as the surrounding region of an unstable critical point is inherently unstable. To prevent numerical instabilities, the ODE can be terminated by haptic obstacle, ensuring stability and continuity in the manipulation process while avoiding singularities.

3.2. Numerical continuation in control space

In our framework, we can only control the robot $ \boldsymbol{u} $ , the directly controllable variables, rather than the entire implicit EM. Hence, we explore the control space via the numerical continuation approach. Each chart $ {\mathcal{C}}_i $ is represented as a node in a graph. These charts include information about $ {\mathcal{C}}_i\equiv \left({\boldsymbol{u}}_i,{\boldsymbol{z}}_i,{R}_i,{P}_i^m\right) $ , where $ i $ denotes the index of this chart, and $ m $ represents the number of exploration steps. It is important to note that only the polyhedral can be revised during exploration, as $ {P}_i^m $ is determined by both $ i $ and $ m $ .

Similarly to Henderson’s method, our waiting list stores the polyhedra to be explored. Each polyhedron contains several vertices and is associated with two lists of properties: $ isExteriorVertex $ and $ isDeadVertex $ , abbreviated as ExVtx and DdVtx. The ExVtx label is initially set to True for all vertices, but is reassessed after every trimming session. This label indicates whether a vertex is a frontier that needs to be explored, defined as

(3.5) $$ \mathrm{E}\mathrm{X}\mathrm{V}\mathrm{TX}(k)=\left\{\mathrm{TRUE}\ \mathrm{if}\hskip0.2em \parallel {v}_k\parallel >{R}_i\;\mathrm{else}\ \mathrm{FALSE},\forall {v}_k\in \mathrm{vertex}\left({P}_i^m\right)\right\} $$

where $ k $ is the index of vertex, $ i $ denotes the index of polygon, ExVtx includes all the vertex of a polygon.

The second label, DdVtx, is initialized for all the vertex as False, and it is evaluated during the growth phase to determine if a vertex leads to a dead end; this status is maintained even after trimming.

When selecting a polyhedral from the waiting list, we choose a vertex where ExVtx is true and DdVtx is false. Unlike Henderson’s method, which directly grows a new sphere, our strategy involves extending from the center $ {\mathbf{u}}_i $ to the vertex $ {v}_i $ using the adaptive ODE from Eq. 3.3. This ensures that all steps in the manipulation are quasi-static, enhancing the precision and stability of the robot’s movement. For each ODE, representing the growth step from the center to the vertex over a specified time range, termination can occur due to a haptic obstacle or upon reaching the target vertex. The label DdVtx is updated based on the outcome $ \boldsymbol{u}(T) $ from ODE (Eq. 3.3), defined as:

(3.6) $$ \mathrm{DDVTX}(k)=\left\{\mathrm{FALSE}\ \mathrm{if}\;\boldsymbol{u}(T)={v}_k\;\mathrm{else}\ \mathrm{TRUE}\hskip0.24em |{v}_k\in \mathrm{vertex}\left({P}_i^m\right)\right\} $$

This also implies that we can only update one vertex $ i $ for the list DdVtx within each growth. Meanwhile, the edge weight between two nodes is evaluated as $ {\mathrm{\mathcal{E}}}_{ij}={S}_{ij}=\phi (T) $ . As such, the continuation exploration can be visualized in Figure 3(b), where each growth begins from the center of a polyhedron and targets a frontier vertex. The entire ODE (Eq. 3.3) stays on EM, shown as the curve on EM. There is sometimes a risk that the ODE may leave the EM, resulting in loss of stability. In such cases, the corresponding vertex will be labeled as DdVtx. Conversely, the ODE may be completed successfully, indicating that the growth step is finished.

Algorithm 2 Extend a node in the Graph

Hence, the algorithm for Extend_Graph becomes Algorithm 2, the function starts by inputting a chart $ {\mathcal{C}}_i $ , where we extract the centre point $ \left({\boldsymbol{u}}_0,{\boldsymbol{z}}_0\right) $ and the polyhedron $ {P}_i^m $ (line 2). We also select a vertex labeled as ExVtx is True and DdVtx is False, indicating that this vertex is a frontier and is achievable, having not encountered the haptic obstacle (line 3). Subsequently, we execute the ODE (Eq. 3.3) (line 4) to determine whether it can integrate to this vertex (line 5). If it is true, indicating that the ODE was not terminated by a haptic obstacle, we add a new node to our graph (line 6), and also append the index $ m $ of this new chart to the waiting list (line 7). After adding a new node, we enter the trimming phase. We loop through all existing charts (lines 8 and 9), and assess whether they are sufficiently close (line 10). If they are, we compute the ODE from the new chart $ {C}_m $ to the nearby existing charts (lines 11 and 12). A new edge is then added to the graph based on the geodesic $ \phi $ (line 13). Subsequently, we trim both polyhedrons separately with the PolyTrim function via Eq. 2.8 (lines 14 and 15). The final step involves checking the new polyhedron. If this polyhedron no longer has any frontier vertices, it is removed from the waiting list. Conversely, if the growth is terminated by a haptic obstacle, the corresponding DdVtx label is set to True.

The charts are only trimmed when they are near. Different from Henderson’s method, we define the distance function as

(3.7) $$ \mathrm{dist}\left({C}_i,{C}_j\right)=\sqrt{\parallel {u}_i-{u}_j{\parallel}^2+l{\left(\mathit{\operatorname{mod}}\left({z}_i-{z}_j,2\pi \right)\right)}^2} $$

where $ l $ relates to the size of the object to make same dimension for translation and rotation (Campolo et al., Reference Campolo, Widjaja, Xu, Ang and Burdet2013). This formula reflects that the rotational periodicity of real-world components (e.g., motors) is $ 2\pi $ . Hence, we define the function Nearby_Charts as in Algorithm 3 where $ {R}_i+{R}_j $ defines the distance in the control space, $ {R}_z $ defines the distance in the state space. As the difference between $ {z}_i $ and $ {z}_j $ equals $ 2\pi $ , two nodes are still near each other.

Algorithm 3 Decide nearby charts

3.3. Overall algorithm

We start our exploration by placing a node on the EM and defining a radius $ {R}_i $ for the corresponding sphere to generate a polyhedron. We maintain a waiting list $ {B}_{\mathrm{list}}^m $ of these polyhedra (line 1). In each iteration of our process, as long as the waiting list is not empty (line 2), we select the first index from the waiting list (line 3) and identify its charts (line 4), if this chart does not have an unexplored frontier vertex, which means $ {\mathcal{C}}_i.\mathrm{EXVTX}\subseteq {\mathcal{C}}_i.\mathrm{DDVTX} $ , we remove it from the waiting list. Otherwise, we start to grow once using Algorithm 2. Finally, we sort the sequence of polyhedra in the waiting list to determine the exploration strategy, such as whether to explore the most distant polyhedron first or start from the nearest to the origin.

Algorithm 4 Haptic graph exploration

4.1. Inverted pendulum model

In a 2D plane, the system comprises a pendulum modeled as a superquadric with length $ {L}_0 $ and mass $ m $ . A virtual robot agent is connected to the pendulum via a nonlinear spring. The pendulum is hinged at one end to the origin of the world, and a body frame is attached to the center of mass (CoM). As depicted in Figure 4, the uncontrollable state variable of the system is the rotation angle $ \boldsymbol{z}=\theta \in {S}^1 $ , and the control vector is the position of the robot $ \boldsymbol{u}=[{u}_x,{u}_y{]}^T\in {\unicode{x211D}}^2 $ .

Figure 4. Inverted pendulum task, where the contact interaction is captured by non-linear stiffness.

Hence, the total manipulation potential $ W\left(\boldsymbol{z},\boldsymbol{u}\right) $ consists of contact energy between the robot and the pendulum, and gravity potential.

(4.1) $$ {\displaystyle \begin{array}{c}W\left(\boldsymbol{z},\boldsymbol{u}\right)={W}_{\mathrm{grav}}\left(\boldsymbol{z}\right)+{W}_{\mathrm{ctrl}}\left(\boldsymbol{z},\boldsymbol{u}\right),\\ {}=\frac{1}{2}{mgL}_0\sin \theta +\frac{1}{2}k\left({\left({u}_x-{L}_0\cos \theta \right)}^2+{\left({u}_y-{L}_0\sin \theta \right)}^2\right),\end{array}} $$

where $ k $ is the nonlinear spring stiffness, capturing the interaction between the robot and the pendulum.

(4.2) $$ k(d)={k}_{\mathrm{min}}+\frac{1-\tanh \left(d/{d}_0\right)}{2}{k}_{\mathrm{max}}. $$

The variable $ l $ serves as a signed distance for mechanical contact. When $ l<0 $ , it indicates that the robot $ \boldsymbol{u} $ has penetrated the pendulum, signifying that contact has occurred. This results in a large contact force due to the high stiffness parameter $ {k}_{\mathrm{max}} $ . Conversely, when there is no contact between the robot and the pendulum ( $ l>0 $ ), the resultant force should be zero, indicated by a significantly lower stiffness ( $ {k}_{\mathrm{max}}\gg {k}_{\mathrm{min}} $ ). The parameter $ {l}_0 $ is utilized to facilitate a smooth transition between contact and no-contact states, to guarantee a smooth and differentiable manifold.

4.2. Exploring and trimming in control space

We first show how the numerical continuation approach expands in the control space and how polytrim operates. We set $ {R}_i $ for each polyhedron to be the same as the red circles in Figure 5(a). We observe that a convex polyhedron (represented as a square in 2D space) contains a sphere (represented as a circle in 2D space). Then, polytrim identifies the half-space between two nearby polyhedra. The polyhedron is trimmed until the square fits within the circle, indicating no more frontier vertices.

Figure 5. The implicit manifold $ {\mathcal{M}}^{eq} $ is discretized via a Graph having as nodes a finite set of equilibria in $ {\mathcal{M}}^{eq} $ interconnected weighted edges defined via the $ {\boldsymbol{G}}^2 $ metric. Manipulation planning can therefore be recast into an optimal process on finite Graphs.

The final result is shown in Figure 5(b), we plot the entire “staircase” branch of the inverted pendulum task in the configuration space. Each polyhedron is depicted as a blue square since control space $ \mathcal{U}\in {\mathrm{\mathbb{R}}}^2 $ . The polyhedra in control space is moved down to better indicate the idea of control space with respect to configuration space. The implicit manifold is illustrated with orange dots in the background, while the red dots represent the nodes in the graph. We observe that all the red nodes remain on the EM, which validates the accuracy of our adaptive ODE.

We identify two boundaries of red nodes. The outer circle, representing a haptic obstacle, indicates instability and helps avoid loss of singularity. The inner circle, also a haptic obstacle, suggests that the robot is exerting excessive force on the pendulum, leading to instability. Furthermore, the connectivity in the implicit manifold indicates that rotating the pendulum full circle is equivalent to returning to the starting point, thus forming a loop closure for manipulation. We demonstrate that this task inherently exhibits a circular nature, as opposed to a helical one, and confirm that our $ \mathcal{Z} $ space belongs to $ {S}^1 $ instead of $ {\mathrm{\mathbb{R}}}^1 $ . Hence, our algorithm enables the robot to recognize that the inverted pendulum task is a circular motion, which is crucial for selecting the appropriate manipulation strategy. For instance, understanding that circular tasks (e.g., valve turning) require no translational motion, while helical tasks (e.g., bottle opening) necessitate it, ensures the robot can recognize these differences. Meanwhile, by applying Topological Data Analysis (TDA) (Joharinad and Jost, Reference Joharinad and Jost2023; Nathaniel Saul, Reference Nathaniel Saul2019), the graph has a genus of $ 1 $ , which supports our results. This means the object has one hole in its structure from its topological perspective.

Remark 1: In the real-world scenario, observed $ {\boldsymbol{z}}^{\ast } $ could be noisy but the control $ \boldsymbol{u} $ is always accurate. Therefore, this means the initial guess of our ODE is noisy. However, with the help of the attractor properties of Newton–Raphson and the description of the embedded manifold within our ambient space ensure that our ODE still functions correctly, ultimately attracting the node back to the manifold.

Remark 2: It is worth noting that our work fundamentally differs from classical inverted pendulum studies (Irfan et al., Reference Irfan, Zhao, Ullah, Mehmood and Fasih Uddin Butt2024), which primarily focus on control, aiming to stabilize the pendulum without considering contact dynamics. In contrast, our method incorporates contact interactions between the robot and the pendulum, with the goal of exploring the entire manipulation manifold while emphasizing its connectivity and topological properties. From a broader perspective, our work aligns more closely with motion planning approaches for pendulum-like objects such as door handles (Shaikh-Mohammed et al., Reference Shaikh-Mohammed, Alharbi and Alqahtani2023).

Remark 3: Conventional motion planning methods typically default to directly controlled variables, which may not fully capture the interaction between robot commands and manipulated objects. In contrast, our approach considers both indirectly and directly controllable variables. Additionally, constrained motion planning approaches rely on task-specific constraint functions. For example, in the case of manipulation of the inverted pendulum, a task-based constraint function (Kingston et al., Reference Kingston, Moll and Kavraki2018) on $ \boldsymbol{u} $ is required to generate a feasible control path to invert the pendulum, while our framework does not require any task-specific constraint function.