System and Method for Controlling Wheeled Mobile Manipulators

A method for controlling a robot for performing a task, wherein the robot is a mobile manipulator including a nonholonomic base having wheels for moving on flat surfaces and a manipulator arm connected to the wheeled base and having degrees of freedom for performing the task, wherein the method comprises performing the following steps using a processor coupled with stored instructions:

• receiving a task in the form of a sequence of waypoints from a task planner, wherein the mobile manipulator, if located at the sequence of waypoints, is collision-free;
• determining a safe trajectory of a coordinated motion of the wheeled base and the manipulator arm of the robot to perform the task using a joint motion model of the wheeled base and the manipulator arm of the robot including external forces acting on the robot when the robot moves according to the safe trajectory while ignoring dynamics of internal forces generated by actuators of the robot that cause the external forces, wherein determining the safe trajectory further comprises applying stability constraints on the external forces ensuring that external forces maintain dynamic stability of the robot;
• determining the internal forces for actuators of the robot to track the safe trajectory subject to feasibility constraints on the generation of the internal forces by the actuators, such that the safe trajectory is adjusted to satisfy the feasibility constraints to produce feasible trajectory;
• generating a feasible trajectory by adjusting the safe trajectory to satisfy feasibility constraints, wherein adjusting the safe trajectory comprises determining internal forces to be generated by the actuators of the robot when following the safe trajectory which satisfy the feasibility constraints;
• and
• controlling the actuators of the robot to generate the internal forces to be generated by the actuators of the robot causing the robot to follow the feasible trajectory.