Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same
A robot control system determines which of a number of discretizations to use to generate discretized representations of robot swept volumes and to generate discretized representations of the environment in which the robot will operate. Obstacle voxels (or boxes) representing the environment and obstacles therein are streamed into the processor and stored in on-chip environment memory. At runtime, the robot control system may dynamically switch between multiple motion planning graphs stored in off-chip or on-chip memory. The dynamically switching between multiple motion planning graphs at runtime enables the robot to perform motion planning at a relatively low cost as characteristics of the robot itself change.
1. A method of operation of a processor-based robot control system that employs a plurality of planning graphs stored to a nontransitory storage during a pre-run time period, each planning graph respectively comprising a plurality of nodes connected by a plurality of edges, each node which represents, implicitly or explicitly, variables that characterize a respective state of the first robot in a configuration space of the first robot, and each edge which represents a transition between a respective pair of the states of the first robot, where the respective pair of states is represented by a respective ones of a pair of nodes that are coupled by a respective edge in the respective planning graph, the method comprising:
for a first planning graph of the plurality of planning graphs,
accessing the first planning graph that was stored to the nontransitory storage during the pre-run time period;
for each of a plurality of edges of the first planning graph, performing, by the robot control system, collision checking for collisions between a discretized representation of a swept volume associated with the edge and a discretized representation of any obstacles in an environment in which the robot will operate;
updating, by the robot control system, the first planning graph based on the collision checking;
performing, by the robot control system, an optimization of the updated first planning graph to identify one or more optimized results, if any, from the updated first planning graph;
determining, by the robot control system, whether the one or more optimized results, if any, from the updated first planning graph meets a satisfaction condition;
in response to determining that the optimized result does not meet the satisfaction condition:
accessing the second planning graph that was stored to the nontransitory storage during the pre-run time period;
for each of a plurality of edges of the second planning graph, performing, by the robot control system, collision checking for collisions between a discretized representation of a swept volume associated with the edge and a discretized representation of any obstacles in an environment in which the robot will operate,
updating, by the robot control system, the second planning graph based on the collision checking;
performing, by the robot control system, an optimization of the updated second planning graph to identify one or more optimized results, if any, from the updated second planning graph; and
controlling the first robot using one of the updated planning graphs during a run time period.
2. The method of claim 1 , further comprising:
determining, by the robot control system, whether the one or more optimized results, if any, from the updated second planning graph, if any, meets a satisfaction condition.
3. The method of claim 2 wherein, in response to determining that the one or more optimized results, if any, from the updated second planning graph meets the satisfaction condition, controlling the first robot using one of the updated planning graphs by:
applying, by the robot control system, a transition identified by one of the one or more optimized results from the updated second planning graph to the robot.
4. The method of claim 2 wherein, in response to determining that the one or more optimized results, if any, from the updated second planning graph does not meet the satisfaction condition:
for each of a plurality of edges of a third planning graph, performing, by the robot control system, collision checking for collisions between a discretized representation of a swept volume associated with the edge and a discretized representation of any obstacles in an environment in which the robot will operate,
updating, by the robot control system, the third planning graph based on the collision checking; and
performing, by the robot control system, an optimization of the updated third planning graph to identify one or more optimized results, if any, from the updated third planning graph.
5. The method of claim 4 wherein, in response to determining that the one or more optimized results, if any, from the updated third planning graph meets the satisfaction condition, controlling the first robot using one of the updated planning graphs by:
applying, by the robot control system, a transition identified by one of the one or more optimized results from the updated third planning graph to the robot.
6. The method of claim 1 wherein the first planning graph is associated with a first set of physical dimensions of at least a portion of the first robot at a first time, and the second planning graph is associated with a second set of physical dimensions of that portion of the first robot at a second time, the first set of physical dimensions being different from the second set of physical dimensions, and updating the second planning graph based on the collision checking includes updating the second planning graph associated with the second set of physical dimensions of the portion of the first robot.