Motion Planning and Control of Mobile Manipulators

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

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

3. notation robot configuration vector (platform + manipulator) robot forward kinematics platform velocity inputs manipulator velocity inputs dimension of the robot configuration space number of degrees of freedom (number of velocity inputs) dimension of the task M. Vendittelli: Motion planning and control of mobile manipulators

4. classification holonomic MM nonholonomic MM staticallyredundant MM kinematicallyredundant MM M. Vendittelli: Motion planning and control of mobile manipulators

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

6. example 2: unicycle + RPP arm nonholonomic redundant M. Vendittelli: Motion planning and control of mobile manipulators

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

8. 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 M. Vendittelli: Motion planning and control of mobile manipulators

9. 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 M. Vendittelli: Motion planning and control of mobile manipulators

10. 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 M. Vendittelli: Motion planning and control of mobile manipulators

11. 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) M. Vendittelli: Motion planning and control of mobile manipulators

12. 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 M. Vendittelli: Motion planning and control of mobile manipulators

13. 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 M. Vendittelli: Motion planning and control of mobile manipulators

14. kinematic modeling • the manipulator is unconstrained, i.e., • the NMMdifferential kinematics is then M. Vendittelli: Motion planning and control of mobile manipulators

15. 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 M. Vendittelli: Motion planning and control of mobile manipulators

16. 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 M. Vendittelli: Motion planning and control of mobile manipulators

17. 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 M. Vendittelli: Motion planning and control of mobile manipulators

18. 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 yielding a linear, decoupled, and exponentially stable behavior for each error component of the task M. Vendittelli: Motion planning and control of mobile manipulators

19. 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 with and • the differential kinematics becomes M. Vendittelli: Motion planning and control of mobile manipulators

20. Reduced Gradient (RG) • the choice satisfies the kinematic task, whilerealizes 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 M. Vendittelli: Motion planning and control of mobile manipulators

21. 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) M. Vendittelli: Motion planning and control of mobile manipulators

22. 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 M. Vendittelli: Motion planning and control of mobile manipulators

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

24. comparison of performance maximizing manipulability M. Vendittelli: Motion planning and control of mobile manipulators

25. 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 M. Vendittelli: Motion planning and control of mobile manipulators

26. 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 M. Vendittelli: Motion planning and control of mobile manipulators

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

28. comparison of performance minimizing the pointing error norm M. Vendittelli: Motion planning and control of mobile manipulators

29. 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 M. Vendittelli: Motion planning and control of mobile manipulators

30. 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 M. Vendittelli: Motion planning and control of mobile manipulators

31. 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 M. Vendittelli: Motion planning and control of mobile manipulators

32. 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 M. Vendittelli: Motion planning and control of mobile manipulators

33. 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 M. Vendittelli: Motion planning and control of mobile manipulators

34. 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 M. Vendittelli: Motion planning and control of mobile manipulators

35. 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 M. Vendittelli: Motion planning and control of mobile manipulators

36. 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 M. Vendittelli: Motion planning and control of mobile manipulators

37. 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 M. Vendittelli: Motion planning and control of mobile manipulators

38. 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 M. Vendittelli: Motion planning and control of mobile manipulators

39. 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 M. Vendittelli: Motion planning and control of mobile manipulators

40. 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 M. Vendittelli: Motion planning and control of mobile manipulators

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

42. starting from a low-compatibility configuration optimization of optimization of M. Vendittelli: Motion planning and control of mobile manipulators

43. 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 M. Vendittelli: Motion planning and control of mobile manipulators

44. manipulability optimization M. Vendittelli: Motion planning and control of mobile manipulators

45. room crossing M. Vendittelli: Motion planning and control of mobile manipulators

46. narrow passage (1) M. Vendittelli: Motion planning and control of mobile manipulators

47. narrow passage (2) M. Vendittelli: Motion planning and control of mobile manipulators

48. self-motion M. Vendittelli: Motion planning and control of mobile manipulators

49. 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 M. Vendittelli: Motion planning and control of mobile manipulators

50. 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 M. Vendittelli: Motion planning and control of mobile manipulators