Motion Planning and Control of Mobile Manipulators Rescue Robotics Camp 2006 Motion Planning and Control of Mobile Manipulators Marilena Vendittelli Dipartimento di Informatica e Sistemistica Università di Roma “La Sapienza” Roma, Italy

dexterity of a standard manipulator a mobile manipulator merges dexterity of a standard manipulator increased (~ infinite) workspace capabilities of a mobile platform Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

notation robot configuration vector (platform + manipulator) robot forward kinematics platform velocity inputs manipulator velocity inputs dimension of the robot configuration space Dico da qui che si lavora a livello cinematico e perche’? number of degrees of freedom (number of velocity inputs) dimension of the task Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

classification holonomic MM nonholonomic MM statically redundant MM kinematically redundant MM Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

example 1: omnidirectional platform + RPP arm holonomic redundant Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

example 2: unicycle + RPP arm nonholonomic redundant Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

example 3: unicycle + gripper nonholonomic non-redundant Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

nonholonomic MMs (NMMs) include wheeled mobile platforms allowing for precise locomotion mechanisms with a reduced number of actuators the End-Effector (EE) does not always inherit the nonholonomic constraint of the base redundancy “frees” from nonholonomy, helps to stay away from singularities and allows obstacle avoidance Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/1 configuration-level kinematic inversion given an EE pose, generate robot configurations which realize it standard (includes static redundancy resolution) velocity-level kinematic inversion given an EE trajectory, generate velocity commands which allow to track the EE trajectory when starting on it solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward path planning (w/o obstacles) conf-to-conf: plan a feasible path for the NH platform (NHPP) and any path for the manipulator EE pose to EE pose: 2 approaches generate an EE trajectory joining the two poses and then (velocity-level) kinematic inversion generate two configurations corresponding to the two poses by (configuration-level) kinematic inversion and then the problem is conf-to-conf Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/2 motion planning (with obstacles) conf-to-conf: conventional randomized techniques accounting for the nonholonomy of the platform EE pose to EE pose: 2 approaches generate a collision-free EE trajectory joining the two poses (easy because formulated in the operational space where the obstacles are defined) and then motion planning on a given EE path (weak solution because it uselessly constrains the search) generate two confs corresponding to the two poses by (conf-level) kinematic inversion and then the problem is conf-to-conf (also constrains the search but in a less severe way) on a given EE path: special randomized technique where generated configurations are constrained to belong to self-motion manifolds Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/3 kinematic control given an EE trajectory, generate velocity commands which allow to track the EE trajectory also in the presence of initial displacements solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward plus a feedback correction term (recovers from initial errors and is also robust wrt linearization errors) dynamic control configuration space trajectory: use the NMM dynamic model (standard - but includes platform/manipulator interactions plus the nonholonolomy of the platform), use feedback linearization to reduce to second-order NMM kinematic model (platform: kinematic model + dynamic extension; manipulator: double integrators); then backstepping can be used to transform a kinematic to a dynamic controller EE trajectory: as above but use operational space formulation (includes inversion of the NMM jacobian) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

what’s in this talk derivation of a simple and general model for robotic systems made of a nonholonomic mobile platform carrying a manipulator by combining the manipulator differential kinematics with the admissible differential motion of the platform extension of classical redundancy resolution schemes, originally developed for standard manipulators, to kinematically redundant NMMs path planning and kinematic control of NMMs without obstacles motion planning along given end effector paths for NMMs in environments cluttered with obstacles Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

kinematic modeling the platform kinematic model is given by the driftless system , where the columns of the matrix span the platform admissible velocity space at each configuration the unicycle example generalized coordinates nonholonomic constraint the kinematic model with = driving, = steering velocity inputs Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

kinematic modeling the manipulator is unconstrained, i.e., the NMM differential kinematics is then Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

redundancy resolution the two concepts of static redundancy, i.e., kinematic redundancy, i.e., are relevant for a NMM (they collapse for holonomic MMs) when the objective is velocity-level kinematic inversion (i.e., generating velocity commands that guarantee the execution of a given task), kinematic redundancy definition is used Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Projected Gradient (PG) for a given , all inverse kinematic solutions can be expressed as , where is usually chosen so as to locally optimize a given criterion for fixed-base redundant manipulators: assuming that the manipulator has N joints, the mechanical joint limits may, for example, be avoided by minimizing the cost function where is the available range for joint and is its midpoint Esempi di h(q) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Projected Gradient (PG) for NMMs: the analysis of the time-derivative of shows that the command which realizes the maximum local improvement of is the generalized velocity of the platform is then feedback recupera anche linearizzazione J (dove viene linearizzato?) Includere 7 8 9 di paper robotica per spiegare u_h ?????? Scrivere q_ph come nell’articolo ma chiarire la storia della proiezione Non e’ chiara la generazione di u_h viene dalla derivata di h o dalla dimostrazione che quella scelta corrisponde a minimizzare una funzione di costo quadratica (vedi handbook 11.32)? Include time-derivative of h(q) Algoritmicamente come si sceglie alfa? Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Projected Gradient (PG) tracking tasks for planning, for kinematic control, with a diagonal matrix regulation tasks in both cases, the closed-loop system is described by feedback recupera anche linearizzazione J (dove viene linearizzato?) Includere 7 8 9 di paper robotica per spiegare u_h ?????? Scrivere q_ph come nell’articolo ma chiarire la storia della proiezione Non e’ chiara la generazione di u_h viene dalla derivata di h o dalla dimostrazione che quella scelta corrisponde a minimizzare una funzione di costo quadratica (vedi handbook 11.32)? Include time-derivative of h(q) Algoritmicamente come si sceglie alfa? yielding a linear, decoupled, and exponentially stable behavior for each error component of the task Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Reduced Gradient (RG) an alternative, computationally lighter strategy is to directly use commands for task execution and to perform optimization w.r.t. the reduced set of “extra” commands if the robot Jacobian matrix is full rank, it is always possible to find a permutation matrix such that with nonsingular this induce a reordering of the velocity input vector Clarify what is meant by reduction Add dimension of ua and ub with and the differential kinematics becomes Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Reduced Gradient (RG) the choice satisfies the kinematic task, while realizes the maximum local improvement of [note: is a reduction of to the subspace of commands that automatically satisfy the task constraint] when and the manipulator is not in a singular configuration one can choose and so that Clarify what is meant by reduction Add dimension of ua and ub Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

example 1: unicycle + 2R planar manipulator 5 generalized coordinates 4 velocity commands the 2x4 Jacobian of the EE planar positioning task is never singular if (base offset) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

trajectory tracking + planar pointing the end-effector must follow a given circular trajectory while the first link points towards a fixed target point the single degree of redundancy left is used to maximize the manipulability measure Add manipulability definition Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

trajectory tracking + planar pointing RG inversion scheme Platform center trajectory EE reference trajectory First link pointing direction Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

comparison of performance maximizing manipulability Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

example 2: unicycle + 3R spatial robot 6 generalized coordinates 5 velocity commands the 3x5 Jacobian of the EE spatial positioning task is never singular if and the manipulator arm is not stretched or folded along the vertical direction Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

trajectory tracking + spatial pointing a task involving spatial tracking and pointing: the end-effector must follow a circular trajectory the two degrees of redundancy are used to force the third link to point towards a fixed point , by minimizing a suitable Soecify h(q) Paolo? Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

trajectory tracking + spatial pointing RG inversion scheme Target EE ref. trajectory “EE point of view” Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

comparison of performance minimizing the pointing error norm Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

Motion Planning for MM along Given End-effector Paths (the MPEP problem) objective in a known environment, plan collision-free motions for a kinematically redundant mobile manipulator whose end-effector path is assigned Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

motivation in many applications the end-effector must trace a given path while the manipulator moves among obstacles e.g., camera inspection, object transfer,. . . it is necessary to generate joint motions that keep the end-effector on the path while avoiding collisions between the robot bodies and the obstacles (MPEP: Motion Planning along End-effector Paths) kinematic redundancy should help in avoiding obstacles, but existing solutions based on optimal redundancy resolution are unsatisfactory a promising approach is based on a randomized motion planner which takes into account the nonholonomy of the mobile base and the natural partition of dof’s Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

the MPEP problem given the end-effector path , find a path , in configuration space, such that and no collision occurs between the robot body and the obstacles the path is feasible w.r.t. the nonholonomic constraints manipulator joint limits and self-collisions are avoided as well a solution may exist or not depending on the connectivity of the free configuration space region that is compatible with the given end-effector constraint the initial configuration q(0) may or may not be assigned Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

a continuous path in configuration space is derived from a solution by we seek a solution in the form of a sequence of collision-free configurations a continuous path in configuration space is derived from a solution by interpolating the sequence by local paths for the mobile platform, local feasible paths are automatically produced by the planners for the manipulator, simple linear interpolation is used End-effector tracking accuracy may be improved by increasing the path sampling parameter s in any case, interpolation schemes accounting for the end-effector path constraint may be easily designed, e.g., based on pseudoinverse control Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

generation of random configurations idea generate random collision-free samples of self-motion manifolds tools random generation of a sample of Mi: • q ! (qb, qr), with qb 2 IRm, qr 2 IRn−m (base and redundant variables) • random generation of qr, inverse kinematics computation of 2. collision checking on (qb, qr) as well as along the local connecting path Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

how do we choose the base and the redundant variables? it depends on the particular mobile manipulator; for illustration, consider a unicycle, controlled by pseudovelocities v, , carrying a spatial 3R arm here chose : redundant variables and : base variables Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

generation of a random sample of Mi , the self-motion manifold corresponding to with (optional) biases the generated distribution: is used as a starting point for the platform (limited joint displacement) is enforced by INV KIN this allows the incremental construction of a solution: when a belonging to Mi is available, it can be used as to generate a belonging to Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

generation of random pseudovelocities admissible pseudovelocity set 1. completely random pseudovelocities: , are generated according to a uniform probability distribution in 2. constant-energy pseudovelocities: , must satisfy v2 +c !2 = 2 : energy level 3. optimal pseudovelocities: , are chosen in a finite set of candidates (e.g., random forward-right, forward-left, backward-right, backward-left motions) so as to optimize a criterion index: Idist = kqdes−qnewk: the distance between the new configuration and some desired configuration Icomp: the task compatibility of , which quantifies the capability of moving along the given end-effector path starting from Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

greedy planner a depth-first search starting from the initial end-effector pose given , an initial collision-free configuration is randomly generated on the self-motion manifold starting from , a sequence of random collision-free configurations . on the self-motion manifolds M1,M2,M3, . . . is generated; each is biased by the previous to enforce the incremental buildup of a feasible path if the last self-motion manifold Ms is not reached, a new is generated and the process restarted very effective in dealing with easy problems must generate a new to backtrack may prove inefficient in complex problems Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

RRT-like planners generate multiple samples for each self-motion manifold by adapting the concept of Rapidly-exploring Random Tree (RRT) the extension procedure takes place in the platform configuration space; the integer associated to each node identifies the self-motion manifold to which the node belongs Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

basic RRT-like 1. given , an initial configuration is randomly generated on the selfmotion manifold 2. the tree rooted at is expanded by (a) generating a random configuration (b) identifying in the node whose is closest to , and the associated end-effector pose (c) computing a new node as follows: • generate random pseudovelocities , and apply them from to obtain the corresponding • generate from by kinematic inversion of (with as bias) 3. if a node belonging to the final self-motion manifold of has not been generated after a maximum number of expansion steps, a new is randomly generated and the process is restarted to retain a RRT-like behavior (tree expands toward ), one may generate pseudovelocities which optimize , with Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

the breadth-first nature of RRT LIKE can be modified variations the breadth-first nature of RRT LIKE can be modified RRT GREEDY alternates depth-first phases (as in GREEDY) with RRT LIKE phases RRT BIDIR expands two trees, one rooted at the start and the other at the goal self-motion manifolds; when the trees meet on an intermediate manifold, a self-motion is generated to join them Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

planning experiments pseudovelocity generation (RRT LIKE) completely random optimization of Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

starting from a low-compatibility configuration optimization of optimization of Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

GREEDY performs effectively in relatively simple problems on the average GREEDY performs effectively in relatively simple problems RRT LIKE and RRT GREEDY become much more effective in complex planning instances RRT BIDIR is convenient when the search space is large and there is enough room for reconfiguration Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

manipulability optimization Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

room crossing Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

narrow passage (1) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

narrow passage (2) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

self-motion Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/1 configuration-level kinematic inversion standard (includes static redundancy resolution) velocity-level kinematic inversion given an EE trajectory, generate velocity commands which allow to track the EE trajectory when starting on it solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward path planning (w/o obstacles) conf-to-conf: plan a feasible path for the NH platform (NHPP) and any path for the manipulator EE pose to EE pose: 2 approaches generate an EE trajectory joining the two poses and then (velocity-level) kinematic inversion generate two configurations corresponding to the two poses by (configuration-level) kinematic inversion and then the problem is conf-to-conf Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/2 motion planning (with obstacles) conf-to-conf: conventional randomized techniques accounting for the nonholonomy of the platform EE pose to EE pose: 2 approaches generate a collision-free EE trajectory joining the two poses (easy because formulated in the operational space where the obstacles are defined) and then motion planning on a given EE path (weak solution because it uselessly constrains the search) generate two confs corresponding to the two poses by (conf-level) kinematic inversion and then the problem is conf-to-conf (also constrains the search but in a less severe way) on a given EE path: special randomized technique where generated configurations are constrained to belong to self-motion manifolds Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

NMMs: basic planning and control problems/3 kinematic control given an EE trajectory, generate velocity commands which allow to track the EE trajectory also in the presence of initial displacements solve (includes kinematic redundancy resolution) the NMM differential kinematic map (which accounts for the platform nonholonomy) using the nominal EE velocity as feedforward plus a feedback correction term (recovers from initial errors and is also robust wrt linearization errors) dynamic control configuration space trajectory: use the NMM dynamic model (standard - but includes platform/manipulator interactions plus the nonholonolomy of the platform), use feedback linearization to reduce to second-order NMM kinematic model (platform: kinematic model + dynamic extension; manipulator: double integrators); then backstepping can be used to transform a kinematic to a dynamic controller EE trajectory: as above but use operational space formulation (includes inversion of the NMM jacobian) Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators

bibliography L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manipulators, Springer, 2000. Y. Nakamura, Advanced Robotics: Redundancy and Optimization, Addison-Wesley, 1991. H.Seraji,``A unified approach to motion control of mobile manipulators,'’ Int. J. of Robotics Research, vol. 17, no. 2, pp. 107-118, 1998. Y. Yamamoto and X. Yun, “Unified analysis on mobility and manipulability of mobile manipulators,” in 1999 IEEE Int. Conf. on Robotics and Automation, 1999, pp. 1200–1206. F. G. Pin, K. A. Morgansen, F. A. Tulloch, C. J. Hacker, and K. B. Gower, ``Motion planning for mobile manipulators with a non-holonomic constraint using the FSP method,'’ J. of Robotic Systems, vol. 13, no. 11, pp. 723--736, 1996. J.-Y. Fourquet, B. Bayle, and M. Renaud, “Manipulability of wheeled mobile manipulators: Application to motion generation,” Int. J. Of Robotics Research, vol. 22, no. 7-8, pp. 565–581, 2003. A. De Luca, G. Oriolo, P. Robuffo Giordano, "Kinematic modeling and redundancy resolution for nonholonomic mobile manipulators," 2006 IEEE International Conference on Robotics and Automation, Orlando, FL, 2006. S. M. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” Technical Report No. 98-11, Computer Science Dept., Iowa State University., 1998. G. Oriolo, C. Mongillo, "Motion planning for mobile manipulators along given end-effector paths," 2005 IEEE International Conference on Robotics and Automation, Barcelona, SP, pp. 2166-2172, 2005. G. Oriolo, M. Vendittelli, L. Freda, and G. Troso, “The srt method: Randomized strategies for exploration,” 2004 IEEE Int. Conf. On Robotics and Automation, pp. 4688–4694, 2004. Rescue Robotics Camp 2006 M. Vendittelli: Motion planning and control of mobile manipulators