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

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

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

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

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

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

7. example 3: unicycle + gripper
nonholonomic
non-redundant
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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)
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

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

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
Rescue Robotics Camp 2006
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
Esempi di h(q)
Rescue Robotics Camp 2006
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
feedback recupera anche linearizzazione J (dove viene linearizzato?)
Includere 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

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
feedback recupera anche linearizzazione J (dove viene linearizzato?)
Includere 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

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

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

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)
Rescue Robotics Camp 2006
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
Add manipulability definition
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

24. comparison of performance maximizing manipulability
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Soecify h(q)
Paolo?
Rescue Robotics Camp 2006
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”
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

28. comparison of performance minimizing the pointing error norm
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

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

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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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 = : 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

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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

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

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

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

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

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

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

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

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

48. self-motion Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
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
Rescue Robotics Camp 2006
M. Vendittelli: Motion planning and control of mobile manipulators

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

52. 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 , 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 , 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 , 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 , 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