Continuous Motion Plans for Robotic Systems with Changing Dynamic Behavior

The main objective of this paper is to address motion planning for systems in which the dynamic equations describing the evolution of the system change in different regions of the state space. We adopt the control theory point of view and focus on the planning of open loop trajectories that can be used as nominal inputs for control. Systems with changing dynamic behavior are characterized by: (a) equality and inequality constraints that partition the state space into regions (discrete states); and (b) trajectories that are governed by diierent dynamic equations as the system traverses diierent regions in the state space. The motion plan therefore consists of the sequence of regions (discrete states) as well as continuous trajectory (evolution of the continuous state) within each of the regions. Since the task may require that the system trajectories and the inputs are suuciently smooth, we formulate the motion planning problem as an optimal control problem and achieve the smoothness by specifying an appropriate cost function. We present a formal framework for describing systems with changing dynamic behavior borrowing from the literature on hybrid systems. We formulate the optimal control problem for such systems, develop a novel technique for simplifying this problem when the sequence of discrete states is known, and suggest a numerical method for dealing with inequality constraints. The approach is illustrated with two examples. We rst consider the coordination between mobile manipulators carrying an object while avoiding obstacles. We show that the obstacle avoidance translates to inequality constraints on the state and the input. In this task no changes in the dynamic equations occur since no physical interaction between the manipulators and the obstacles takes place. In our second example, we study multi-ngered manipulation. In this case, the state space is partitioned into regions corresponding to different grasp conngurations. We compute the motion plan for a task that requires a switch between two grasp conngurations and obtain the optimal trajectory for the system as well as the optimal time when the switch should occur.