# Linear Time-Varying MPC for Nonprehensile Object Manipulation with a Nonholonomic Mobile Robot

## Abstract

This paper proposes a technique to manipulate an object with a nonholonomic mobile robot by pushing, which is a nonprehensile manipulation motion primitive. Such a primitive involves unilateral constraints associated with the friction between the robot and the manipulated object. Violating this constraint produces the slippage of the object during the manipulation, preventing the correct achievement of the task. A linear time-varying model predictive control is designed to include the unilateral constraint within the control action properly. The approach is verified in a dynamic simulation environment through a Pioneer 3-DX wheeled robot executing the pushing manipulation of a package.

## I Introduction

In a robotic nonprehensile manipulation task, the object is subject only to unilateral constraints imposed by both the robot manipulating it and the environment. A complicated manipulation task can be split into many simpler subtasks, usually called manipulation primitives [25]. Among these primitives, the pushing operation is a simple solution, also adopted by humans, in those situations where the size of the manipulated object prevents an easy grasp by a gripper, or it is too heavy to be dexterously handled. Manipulation by pushing is intuitively simple, but it sets interesting control problems originated by the presence of the friction forces, which are complex to accurately model and causing an unpredictability of the object’s motion [17].

This paper tackles the problem of a nonholonomic mobile robot pushing an object in the environment. The use of a mobile robot for pushing manipulation is justified to overcome the physical limits imposed by a static manipulator’s workspace, or when the object is too large and/or too heavy to be grasped by a mobile manipulator with a gripper. Practical applications can be primarily found in warehouses and industries for handling of goods [26]. The pursued approach is the design of a linear time-varying (LTV) model predictive control (MPC) [3] which explicitly includes the pushing constraints. Violating these constraints means that the forces exerted by the robot on the object do not belong to the friction cones at the contact points. This induces the slipping of the object, reduces the precision of the manipulation task, and worsens the overall performance.

Building upon [10], we propose the use of a classic mobile robot to perform a pushing operation (see Fig. 1), by considering the robot’s nonholonomic constraints in the controller formulation explicitly, and introducing a motion constraint that considers the pushing dynamics to maintain a stiff contact between the robot and the pushed object.

## Ii Related work

A thorough literature review reveals that object manipulation with a mobile robot is typically achieved with proper tools [28], like grippers, or with multi-robot systems caging the object [31]. This latter approach takes inspiration from the natural world. Small animals, like ants, collaborate to transport heavy and large loads: several works try to mimic the behavior of ants to achieve collaborative transportation for groups of mobile robots [19, 20]. However, this problem is often solved considering approaches in which force closure is achieved [22], planning the motion of the robots in such a way that they are opportunely displaced around the object [31]. As an example, robots can be controlled to create a formation around the objects, in such a way that, locally exchanging information, they can transport it as designed in [4]. Along the same lines, the presence of a large number of robots, that can be attached to the object in such a way that they can exchange a force with the object itself, is considered in [9]. The center of mass of the object is approximated as the centroid of the positions of the robots. Geometrical properties of the object are also estimated in [27] based on the robots’ relative positions.

The approaches mentioned above resemble the most common solution exploited in robotics for solving the problem of moving an object: the pick-and-place method, where the object is grasped stiffly and is then moved to the desired location. While pick-and-place is a common and effective solution in several cases, it cannot always be applied. This is particularly true when the size of the object is too large, when its shape is unknown a priori, when it is excessively heavy, when a firm grasp can damage its surface, or when the external environment places some constraints on the use of a multi-robot system. Nonprehensile manipulation approaches can thus be exploited in these cases. Specifically, these approaches include methods in which the robot imposes the object’s motion through unilateral constraints only, such as in the case of pushing. The advantage is the possibility of using only one robot for the operation and the possibility of breaking a contact and create new ones during the same task [17, 25]. Nevertheless, nonprehensile manipulation requires taking into account the robot, the object, and the environmental dynamics, which is often a nontrivial task in the presence of the friction, as for pushing.

Recent implementations of nonprehensile manipulation with robots saw the use of flexible elements like ropes and cables. A robot equipped with a flexible cable is shown in [12], where a planning method is proposed to exploit the cable for moving the object. Objects with general shape are instead addressed in [18], where two mobile robots are connected through a cable, and they cooperatively pull a heavy object. However, such physical interconnection between the two robots may significantly limit the freedom of motion.

To avoid these issues, mobile robots can directly perform nonprehensile manipulation by directly pushing the object [13]. However, it is necessary to guarantee the possibility for the mobile robot to change the pushing direction. This means that the robot must freely move in the environment, without hitting obstacles, to change its relative position to the object. Uncertainties in control and motion execution are addressed in [15] employing an appropriate motion planning strategy, that considers an increased size of the pushed object, to include repositioning maneuvers of the pushing robot. A reinforcement learning framework is proposed in [14] to define the motion pattern for two robots pushing a box. However, a very simplistic scenario is considered where dynamics are neglected. By measuring the instantaneous direction, a robot or a group of robots is guided by an artificial potential field in [11] to push an object. Also in this case, dynamic effects, such as friction, are not considered, making the proposed method unsuitable for complex situations, such as in the presence of non-uniform friction. A fuzzy controller is instead designed in [6] to control two robots pushing an object with known geometrical properties. Slipping of the mobile robot’s wheels during a pushing operation is avoided in [1] through a nonlinear MPC design. However, the friction between the manipulated object and the robot is neglected, causing possible slippage of the object during the manipulation task. Finally, the complexity of determining the optimal sequence of actions to manipulate an object by pushing is solved offline in [10] through machine learning. A convex hybrid MPC program is then solved online to achieve planar manipulation.

Differently from [10], the proposed design explicitly includes the nonholonomic nature of the most common mobile robots available in the market with the dynamic model formulation. Besides, a motion constraint is designed to avoid the slippage of the contact with pushed object during the manipulation, made necessary by the robot nonholonomy.

## Iii Problem statement

Consider a wheeled mobile robot moving in a bi-dimensional environment, where a polygonal planar object has to be manipulated. Let and be the global reference frame and the body frame attached to the center of the robot’s axle, respectively. Let the pose of the mobile robot at time be represented by the vector , where represent the position of in , and is the rotation of with respect to . A visualization is provided in Fig. 2. In a similar way, let represent the pose of the object, as shown in Fig. 3.

We provide a solution to the following problem.

###### Problem

Control the motion of the mobile robot to manipulate the object through pushing maneuvers, in such a way that the trajectory tracks the desired one with the desired accuracy, starting from the initial pose .

In the following, we will assume the mobile robot to behave as a unicycle. The choice is motivated by the simplicity of notation introduced by such a model, and by the fact that several real-world mobile robots (as differential-drive robots) can be represented according to this formulation [21]. We assume that all the considered contacts are rigid, that the robot wheels do not slip, and that the forces exchanged in the interaction follow Coulomb’s model of friction. Moreover, we decompose each contact force in two components, aligned with the edges of the friction cone[23]. The angle between each component and the contact normal is

(1) |

where is the friction coefficient associated with the interacting surfaces. A visualization of the used decomposition is provided in Figure 3. The motion of the controlled system is assumed quasistatic (i.e, it is slow enough that inertial forces are negligible). Moreover, we assume the mobile robot to be equipped with a planar end-effector (i.e., a planar contact surface), such that the surface used to interact with the object is consistent and homogeneous. During the interaction, the end-effector is supposed to be parallel to one of the sides of the polygonal object. This type of interaction is typically referred to as line contact, modeled as if the only contact points were the extreme points of the line [30].

## Iv Modeling

In this section, we introduce the mathematical model of the system motion and the constraints applied within the MPC controller for planar manipulation tasks. First, we describe a second-order model of the mobile robot and its error dynamic for the desired trajectory, then the mathematical model of the pushed object motion is introduced.

### Iv-a Robot Model

Defining as the linear and angular velocities of the robot, respectively (see Fig. 2), the state of the robot is defined as , given by

(2) |

For ease of notation, in the following, dependence on time will be omitted, when not strictly necessary.

Define now as the inputs for the robot, given as the linear and angular acceleration, respectively. Hence, the model of the robot motion can be written as

(3) |

Solution of the Problem stated in Section III passes through the generation of the desired trajectory for the robot to realize the pushing maneuvers^{1}

Let be the error vector with respect to the desired reference frame centered in , and oriented as , that is defined as

### Iv-B Pushed Object Model

Consider, as discussed in Section III, a polygonal object pushed by the robot with line contact on one of its sides. The contact forces are modeled using the components along the friction cone as shown in Fig. 3. We denote with and the right and left contact force components, respectively, for each contact point [23]. We define the vector as

(6) |

The total external wrench , expressed in and whose torque is applied around the object’s center of mass, exerted by the robot to the object can be described by

(7) |

where is the so called grasp matrix. For a square object of side length , the grasp matrix is

(8) |

where is given in (1), and it can be obtained through a geometrical analysis of the contact forces.

Under the assumption of quasistatic interaction, the object motion can be described using the limit surface [7], a geometric representation of the relationship between the applied force on an object and its instantaneous velocity. Inspired by [10], an ellipsoidal approximation of the limit surface is used, due to its simplicity and invertibility properties. A convex quadratic formulation of the ellipsoidal limit surface is given by , where is the matrix representing the ellipsoidal approximation of the limit surface. A procedure for computing such an approximation, that requires the knowledge of the object’s shape and mass as well as the friction coefficient of the support surface, can be found in [16]. Through the principle of maximal dissipation [7], the object instantaneous velocity is perpendicular to the limit surface for a given wrench, which implies:

(9) |

## V MPC Controller Formulation

To correctly solve the Problem defined in Section III, a controller must be designed such as the error vector in (4) is steered to zero without violating the friction constraints given by the contact between the robot and the object. This avoids the slippage of the object during the pushing manipulation. As a matter of fact, zeroing the error vector only does not imply that the object follows the desired trajectory . The controller makes use of a LTV MPC formulation [3] to solve the nonlinear control problem in real-time through the solution of a motion constrained optimization problem. First, a LTV approximation of the model is presented. The model considers the presence of the velocity and acceleration of the desired trajectory in the form of the measured disturbances , a vector of known but unmodifyable model inputs. The MPC formulation and the applied constraints are finally addressed.

### V-a LTV Model Approximation

As discussed in [2], the MPC formulation requires a discrete-time linear (or linearized) model to construct the optimization problem. Therefore, the model (5) is linearized and discretized. The linearization is performed around a series of predicted states obtained through numerical integration of (5). In particular, the nonlinear error dynamics (5) can be approximated by the following LTV system

(10) |

where is the model input vector, is the vector of measured disturbances and is the predicted state. More specifically, we define

(11) |

and

(12) |

which represents the fact that matrices are obtained performing the linearization around , both evaluated at time . The discrete-time equivalent model of (10), defined with sampling time , can then be obtained following the procedure given in [5]. Denoting with the discrete time variable, we get

(13) |

with

(14) | ||||

(15) | ||||

(16) |

### V-B LTV MPC Formulation

The idea behind the MPC formulation is to optimize the future behaviour across a finite prediction horizon of steps. At every discrete-time instant , for a given state estimate , the optimal control input is computed solving the following constrained quadratic programming (QP)

(17a) | ||||

s.t. | (17b) | |||

(17c) | ||||

(17d) | ||||

(17e) |

where

(18) |

is the QP decision variable containing the input vector (that collects the linear and angular acceleration of the robots) and (that collects the forces imposed on the pushed object) for . As will be detailed in Section V-C, such a definition of the decision variable allows us to consider the physical limitations of the robot inside the QP problem, even though the contact forces are not considered in the robot model. Besides, the cost function in (17) is defined as the following quadratic function {dmath} J(z_k,e[k]) = ∑_i=0^p-1{[e[k+i]^T Q e[k+i]]+[u[k+i]^TR_u u[k+i]]+[Δu[k+i]^T R_Δu Δu[k+i]]}+e[k+p]^T P e[k+p]. The diagonal matrices provide the weights associated with each state variable, while contain the weights on the amplitude of the input and the amplitude of its rate of change respectively. The terminal weight is introduced to improve stability, as discussed in [8]. These matrices are all positive semidefinite. Inequality (17b) expresses a pushing interaction constraint, which will be described in details in Section V-C. Expression (17c) imposes upper and lower limits on the elements of the QP decision variable , while (17e) sets limits on its rate of change. These constraints are imposed to guarantee the feasibility of the solution, taking into account the physical limitations of the robot actuators.

### V-C Pushing Constraints for Object Slippage Avoidance

Since the robot is subject to nonholonomic constraints, it cannot change its orientation instantaneously. The direction of the force applied to the pushed object is thus constrained as well. Besides, as previously discussed, the pushing force must be restricted within the friction cone to avoid object slippage during manipulation. Hence, we will now introduce a constraint for the robot motion, such that the contact between the robot and the object does not break. This allows us to guarantee that the movement of the robot produces valid pushing forces, that lie within the friction cone. As a consequence, the input for the robot does not generate any relative motion between the object and the robot itself.

The concept above is implemented imposing the following constraints

(19) |

where is the position of the object in the robot frame , is the rotation matrix between and . The left-hand side of (19) represents the velocity that the object would have if the robot-object system were moving as a rigid body (i.e., no relative motion). The right-hand side expresses the motion of the object due to contact forces, as explained in Section IV-B.

In order to include (19) inside the optimization problem (17), some adjustments are required. In particular, to ensure that the contact forces lie inside the friction cone, each component of is bounded to be greater than zero. The equality constraint in (19) is thus converted into a set of two double inequalities, of the form , and linearized, at each time step , around the point . Matrix in (17b) is finally defined as the Jacobian matrix of the left-hand side of the inequalities, computed with respect to variable , while vector is a zero vector.

## Vi Implementation and Experiments

In this section, we discuss the implementation of the pushing system and the results of three different manipulation tests, which are representative of different operative conditions. The robot used during the simulation is a Pioneer 3-DX, a differential drive mobile robot with two actuated wheels and a castor wheel. The robot is equipped with a pushing bumper attached on the front. The robot receives velocity commands in the form through ROS [24]. At each time step , with period , the following procedure is performed. The controller first sends the velocity command to the robot, then collects the data required to predict the future behaviour and generate the linearized models. The solution of the quadratic problem discussed in section V-B is then computed and used to generate the velocity command for the future step.

Two case studies have been carried out in simulations, performed on a laptop with an Intel Core i7-4510U using the CoppeliaSym physics simulator. The validated controller is written in MATLAB, while ROS handles the communication with the simulator. The gains are experimentally tuned to , , , and . The results of the simulations are discussed below and are reported in the accompanying video. The video also reports the results of preliminary experiments, performed with a real robotic system in a laboratory environment.

### Vi-a Tracking along a straight line

The first case study we propose is the tracking of a straight line starting with an offset. Several simulations have been performed, with the robot starting its movement with initial error state , with varying . A representative run of the simulations, performed with , is discussed hereafter. The pushed object is placed in contact with the robot in a centered position. Figure 4 depicts the planar movement of the robot and the object, measured for a representative run of the simulations. The yellow line represents the desired trajectory while the blue line and red line depicts the movement of the robot and the object, respectively. The figure clearly shows that an initial position offset can be corrected using the proposed controller and constraints.

Figure 5 shows a comparison of the y components of the object position with respect to , while being pushed, with and without the presence of the constraint in the controller. The application of the constraint significantly reduces the amplitude of the movement of the pushed object. The same conclusion can be extracted from Figure 6, that shows the positions of the object and robot controlled without the constraint (19). Moreover, Figure 6 shows that, during the manipulation, the movement of the robot causes an interruption of the pushing line contact, also reflected in the spike visualized in Figure 5, that results in a loss of control over the movement of the object and ultimately in a loss of quality of the manipulation. The proposed controller and constraints maintain the line contact, with a final average object position error less than m, while the absence of the constraint on the motion leads to an average error greater than m.

### Vi-B Complete manipulation task

The second case study we explore is a complete manipulation. To complete the task, the robot transports the object to a desired configuration performing a series of pushing actions. Once a pushing maneuver is completed, the robot performs a repositioning maneuver to change the pushing side before starting the next action. During the maneuver the robot steps back from the object, goes around the object along a circular trajectory and then approaches slowly the object until the contact is established. Several simulations have been performed, considering different trajectories composed of straight and curve segments. The trajectory traveled by the robot and the object during a representative run of the simulations is depicted in Figure 7. In this task the robot transports the object from the initial position towards the desired configuration performing three pushing actions on the object. The results indicate that, with the use of the proposed controller, it is possible to track a curved line to transport the package without significant accumulation of error, that implies that the robot, when an appropriate reference trajectory is provided, can manipulate the object across the environment. The final positon error is m while the final orientation error is deg.

## Vii Conclusion and Future Work

In this paper, we investigated the problem of manipulating an object in a bi-dimensional environment by pushing with a nonholonomic mobile robot. In particular, we designed a predictive controller for the mobile robot with an appropriate set of constraints to ensure the correct manipulation, providing stiff contact with the object. Numerical case studies are presented, while early-stage experiments are shown in the multimedia attachment.

Future work is focused on consolidating the experimental validation of the proposed approach, taking into account possible external disturbances. We would also like to include our previous work developed in [1] within the proposed framework. Besides, we would like to extend this work to the case of a multi-robot system. Differently from what presented in Section II, the multi-robot system should not resemble a pick-and-place operation, but each agent must perform nonprehensile manipulation through pushing.

### Footnotes

### References

- (2019) Wheel slip avoidance through a nonlinear model predictive control for object pushing with a mobile robot. In 10th IFAC Symposium on Intelligent Autonomous Vehicles, Cited by: §II, §VII.
- (2017) Predictive control for linear and hybrid systems. Cambridge University Press. External Links: Document Cited by: §V-A.
- (2007-06) Predictive active steering control for autonomous vehicle systems. Control Systems Technology, IEEE Transactions on 15, pp. 566 – 580. External Links: Document Cited by: §I, §V.
- (2008-05) Multi-robot manipulation via caging in environments with obstacles. In 2008 IEEE International Conference on Robotics and Automation, Vol. , pp. 1471–1476. External Links: Document, ISSN 1050-4729 Cited by: §II.
- (1997) Digital control of dynamic systems. 3rd edition, Addison-Wesley Longman Publishing Co., Inc., Boston, MA, USA. External Links: ISBN 0201820544 Cited by: §V-A.
- (2009) Fuzzy controller for cooperative object pushing with variable line contact. In Mechatronics, 2009. ICM 2009. IEEE International Conference on, pp. 1–6. Cited by: §II.
- (1991) Planar sliding with dry friction part 1. limit surface and moment function. Cited by: §IV-B.
- (2013) Nonlinear model predictive control: theory and algorithms. Springer Publishing Company, Incorporated. External Links: ISBN 1447126491, 9781447126492 Cited by: §V-B.
- (2015-05) Distributed centroid estimation and motion controllers for collective transport by multi-robot systems. In 2015 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 1282–1288. External Links: Document, ISSN 1050-4729 Cited by: §II.
- (2017) Reactive planar manipulation with convex hybrid mpc. 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 247–253. Cited by: §I, §II, §II, §IV-B.
- (2010-05) A dipole field for object delivery by pushing on a flat surface. In 2010 IEEE International Conference on Robotics and Automation, Vol. , pp. 5114–5119. External Links: Document, ISSN 1050-4729 Cited by: §II.
- (2017) Using a compliant, unactuated tail to manipulate objects. IEEE Robotics and Automation Letters 2 (1), pp. 223–230. Cited by: §II.
- (2010) Dynamic pushing strategies for dynamically stable mobile manipulators. In Robotics and Automation (ICRA), 2010 IEEE International Conference on, pp. 3745–3750. Cited by: §II.
- (2004-05) Simulation of multi-robot reinforcement learning for box-pushing problem. In Proceedings of the 12th IEEE Mediterranean Electrotechnical Conference (IEEE Cat. No.04CH37521), Vol. 2, pp. 603–606 Vol.2. External Links: Document, ISSN Cited by: §II.
- (2016-08) A robust pushing skill for object delivery between obstacles. In 2016 IEEE International Conference on Automation Science and Engineering (CASE), Vol. , pp. 1184–1189. External Links: Document, ISSN 2161-8089 Cited by: §II.
- (1991) Fixture planning with friction. Cited by: §IV-B.
- (1996) Stable pushing: mechanics, controllability, and planning. The International Journal of Robotics Research 15 (6), pp. 533–556. External Links: Document Cited by: §I, §II, footnote 1.
- (2005) Mechanics of cooperative nonprehensile pulling by multiple robots. In Intelligent Robots and Systems, 2005.(IROS 2005). 2005 IEEE/RSJ International Conference on, pp. 2004–2009. Cited by: §II.
- (2014) Cooperative transport in ants: a review of proximate mechanisms. Insectes Sociaux 61 (2), pp. 99–110. Cited by: §II.
- (2014-12) Cooperative transportation of a rod using mobile agents. In 2014 IEEE International Conference on Robotics and Biomimetics (ROBIO 2014), Vol. , pp. 1019–1026. External Links: Document, ISSN Cited by: §II.
- (2002) WMR control via dynamic feedback linearization: design, implementation, and experimental validation. IEEE Transactions on control systems technology 10 (6), pp. 835–852. Cited by: §III.
- (2016) Grasping. In Springer Handbook of Robotics, B. Siciliano and O. Khatib (Eds.), pp. 955–988. Cited by: §II.
- (2008) Grasping. In Springer Handbook of Robotics, B. Siciliano and O. Khatib (Eds.), pp. 671–700. External Links: ISBN 978-3-540-30301-5, Document, Link Cited by: §III, §IV-B.
- (2009) ROS: an open-source robot operating system. In ICRA Workshop on Open Source Software, Cited by: §VI.
- (2018) Nonprehensile dynamic manipulation: A survey. IEEE Robotics and Automation Letters 3 (3), pp. 1711–1718. Cited by: §I, §II.
- (2018-03) The PAN-Robots project: advanced automated guided vehicle systems for industrial logistics. IEEE Robotics Automation Magazine 25 (1), pp. 55–64. External Links: Document, ISSN 1070-9932 Cited by: §I.
- (2002) Object closure and manipulation by multiple cooperating mobile robots. In Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, Vol. 1, pp. 394–399. Cited by: §II.
- (2016) Force-amplifying n-robot transport system (force-ANTS) for cooperative planar manipulation without communication. The International Journal of Robotics Research 35 (13), pp. 1564–1586. Cited by: §II.
- (2017-05) Planning and control for dynamic, nonprehensile, and hybrid manipulation tasks. In 2017 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 4066–4073. External Links: Document, ISSN Cited by: footnote 1.
- (2016-12) Rigid body dynamic simulation with line and surface contact. In 2016 IEEE International Conference on Simulation, Modeling, and Programming for Autonomous Robots (SIMPAR), Vol. , pp. 9–15. External Links: Document, ISSN Cited by: §III.
- (2003) Motion planning of multiple mobile robots for cooperative manipulation and transportation. IEEE Transactions on Robotics and Automation 19 (2), pp. 223–237. Cited by: §II.