Motion Planning and Control of Robots in Shared Workspaces Using Staging Poses

Abstract

The structures and algorithms described herein employ staging poses to facilitate motion robots operating within a shared workspace or workcell, efficiently moving the robot to one or more targets to perform their respective tasks while preventing or at least reducing the risk of collision. Motion planning can be performed during runtime and includes identifying one or more staging poses for the robot to advantageously position or configure the robot whose path is blocked or expected to be blocked by one or more other robots, monitoring the other robots, and moving the robot towards the target in response to the path being unblocked or cleared. Staging poses may be identified using various heuristics, and the robot may be efficiently positioned or configured to complete its task once its path is unblocked or cleared.

Description

[Technical field]

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS This patent application claims priority to U.S. Patent Application No. 63 / 318,933, filed March 11, 2022, the entire disclosure of which is incorporated herein by reference for all purposes.

[0002] <Technical field> The present disclosure relates generally to robot motion planning and control, and more particularly to systems and methods that perform collision detection via processor circuitry to generate motion plans that efficiently drive robots and the like in a shared workspace. [Background technology]

[0003] <Description of Related Art> Motion planning is a fundamental problem in robot control and robotics. Motion planning specifies a path that a robot can follow to transition from a start or current state to a goal state, typically to complete a task without colliding with any obstacles in the operational environment or with a reduced likelihood of colliding with any obstacles in the operational environment. Challenges to motion planning include the ability to execute the motion plan at a very fast rate even as characteristics of the environment change. For example, one or more characteristics, such as the position and / or orientation of one or more obstacles in the environment, may change over time during the runtime of the robot. Challenges also include generating motion plans that enable the robot to complete a task efficiently, for example, in terms of duration to complete the task and / or in terms of energy consumed to complete the task. Challenges further include executing the motion plan with relatively low-cost equipment, with relatively low energy consumption, and with a limited amount of storage (e.g., memory circuits on a processor chip circuit).

[0004] One problem in robotics, for example, is the operation of two or more robots in a shared workspace (work space / workspace is commonly called a workcell or environment) where the robots or the robotic appendages (or appendages) of the robots may interfere with each other while performing a task.

[0005] One approach to operating multiple robots in a common workspace can be called the task-level approach. The task-level approach can use teach-and-repeat training. Engineers can ensure that the robots do not collide by defining a common portion of the workspace and programming each individual robot so that there is only one robot in the shared workspace at any given time. For example, when the first robot begins to move into the workspace, the first robot sets a flag. A controller (e.g., a programmed logic controller (PLC)) reads the flag and prevents other robots from moving into the shared workspace until the first robot de-asserts the flag as it exits the workspace. This approach is intuitive and easy to understand, implement, and troubleshoot. However, this approach necessarily results in low work throughput, since the use of task-level de-confliction usually leads to at least one of the robots being idle for a significant amount of time, even if the idle robot is technically capable of performing useful work in the shared workspace.

[0006] Another approach for operating multiple robots in a shared workspace employs offline planning (e.g., during configuration time before runtime) to achieve higher work throughput than the task-level conflict removal based approach described above. To do so, the system may attempt to solve a planning problem in the combined joint space of all robots or robotic appendages. For example, if two 6-degree-of-freedom (DOF) appendages are in the workspace, a 12-DOF planning problem must be solved. This approach allows for higher performance, but planning can be very time consuming. The 12-DOF problem appears to be too large for traditional motion planning algorithms to solve using currently available architectures. DISCLOSURE OF THEINVENTION [Problem to be solved by the invention]

[0007] One strategy to address these issues is to optimize the motion of a first robot / robot appendage and then manually optimize the motion of a second robot / robot appendage. This can involve iteratively simulating the motion to ensure that the robots / robot appendages do not collide with each other, which can require hours of computation time. In addition, if modifications to the workspace result in a change in the trajectory of one of the robots / robot appendages, the entire workflow must be revalidated. [Means for solving the problem]

[0008] <Summary> The structures and algorithms described herein advantageously facilitate the operation of two or more robots operating within a shared workspace or workcell using staging poses to prevent or at least reduce the risk of the robots or robotic appendages of the robots colliding with one another while maneuvering to efficiently move one or more robots, respectively, to one or more targets to perform their respective tasks within the shared workspace.

[0009] The structures and algorithms described herein enable highly flexible robots to avoid collisions and continue to operate efficiently in a changing shared environment. The efficient planning methods can be accelerated with or without hardware acceleration to generate collision-free motion plans in milliseconds. Ultra-fast "real-time" motion planning allows robot paths to be determined at run-time during task execution without the need for training or time-intensive path optimization. This may advantageously enable the coordination of multiple robots in a shared workspace in an efficient manner.

[0010] The structures and algorithms described herein, at least in some implementations, can result in efficient, collision-free robot movement for a robot in a workspace shared by multiple robots. Collision-free motion can occur for all parts of the robot (e.g., base, robot appendages, end-of-arm tools, end effectors), even when operating at high speeds.

[0011] The structures and algorithms described herein may advantageously reduce the programming effort for multi-robot workspaces, for example, by performing autonomous planning during runtime of one or more robots. In at least some implementations, there is no need for an operator to program safety zones, time synchronization, or joint space trajectories. Input may be limited to a description of the task to be performed, or a target pose, or a goal and kinematic model of the robot. Input may further include representations of fixed objects, or objects (e.g., people) with optionally unpredictable trajectories.

[0012] The structures and algorithms described herein may advantageously identify (e.g., select, generate) one or more staging poses for a given robot to dynamically execute a motion plan that includes advantageously positioning or configuring the given robot where its path is or will be blocked, e.g., by one or more other robots, monitoring the other robots, and moving the given robot toward a goal in response to the path becoming unblocked or cleared. Staging poses may be identified using various heuristics to efficiently position or configure the given robot to complete its task once its path is unblocked or cleared.

[0013] The structures and algorithms described herein, in at least some implementations, can enable robots to share information, for example, over a non-proprietary communication channel (e.g., an Ethernet connection), which may advantageously facilitate the integration of robots in a shared workspace, even robots from different manufacturers.

[0014] The structures and algorithms described herein, in at least some implementations, may operate without cameras or other perception sensors. In at least some implementations, coordination between the robots relies on kinematic models of the robots, the ability of the robots to communicate their respective motion plans, and a geometric model of the shared workspace. In other implementations, visual or other perception may optionally be used, for example, to avoid humans or other dynamic obstacles, such as other robots, that may enter or occupy portions of the shared workspace.

[0015] A wide variety of algorithms are used to solve motion planning problems. Each of these algorithms typically needs to be able to determine whether a given pose of the robot or a movement from one pose to another will result in a collision, either with the robot itself or with an obstacle in the environment. The collision evaluation or check may be performed "in software" using a processor executing processor-executable instructions from a stored set of processor-executable instructions to execute the algorithm. The collision evaluation or check may be performed "in hardware" using a set of dedicated hardware circuits (e.g., collision checking circuits implemented in a field programmable gate array (FPGA), application specific integrated circuit (ASIC)). Such a circuit may, for example, represent a volume (or volume) swept by the robot / robot appendage or a part thereof during each motion or transition between two states (i.e., a swept volume). The circuit may, for example, generate a Boolean evaluation indicating whether a motion will collide with any obstacles, at least some of which represent a volume swept in performing a motion or transition by other robots operating in a shared workspace. [Brief description of the drawings]

[0016] In the drawings, identical reference numbers indicate similar elements or acts. The sizes and relative positions of elements in the drawings are not necessarily drawn to scale. For example, the shapes and angles of various elements are not drawn to scale, and some of these elements have been appropriately enlarged and positioned at times to improve drawing readability. Furthermore, the particular shapes of the elements as drawn are not intended to convey any information regarding the actual shape of the particular elements, but have been selected solely for ease of recognition in the drawings.

[0017] [Figure 1]FIG. 1 is a schematic diagram of a robotic system according to one example implementation that includes multiple robots operating in a shared workspace to perform tasks, includes a motion planner that dynamically generates motion plans for the robots that take into account planned motions of other of the robots, and optionally includes a perception subsystem.

[0018] [Diagram 2] FIG. 2 is a functional block diagram of an environment in which a first robot is controlled via a robotic control system that includes a motion planner, optionally providing motion plans to other motion planners of other robots, and further includes a source of a plan graph that may be separate and distinct from the motion planner, according to one exemplary implementation.

[0019] [Figure 3A] FIG. 3A is an example motion planning graph in C-space for a robot operating in a shared workspace having a path between a current node representing a current pose and a goal node representing a goal pose, according to one example embodiment.

[0020] [Figure 3B] FIG. 3B is an example motion planning graph in C-space for a robot operating in a shared workspace having a path between a current node representing a current pose and a goal node representing a goal pose, with staging nodes representing staging poses, according to one example implementation.

[0021] [Figure 4] FIG. 4 is a flow diagram illustrating a method of operation in a processor-based system for generating motion planning graphs and swept volumes in accordance with at least one exemplary implementation.

[0022] [Diagram 5]FIG. 5 is a flow diagram illustrating a method of operation of a processor-based system for controlling one or more robots, for example during runtime of the robots, in accordance with at least one exemplary implementation.

[0023] [Figure 6] FIG. 6 is a flow diagram illustrating a method of operation of a processor-based system for executing motion plans for one or more robots, e.g., during runtime of the robots, according to at least one example implementation that can be executed as part of performing the method of FIG.

[0024] [Figure 7] FIG. 7 is a flow diagram illustrating a method of operation in a processor-based system for controlling the operation of one or more robots in a multi-robot environment, according to at least one example implementation, that can be executed as part of performing the method of FIG. 5 or FIG. 6 . DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0025] <Detailed Description> In the following description, certain specific details are described to provide a thorough understanding of various disclosed implementations. However, those skilled in the art will recognize that implementations may be practiced without one or more of these specific details, or with other methods, components, materials, etc. In other instances, well-known structures related to computer systems, robots, actuator systems, and / or communication networks have not been shown or described in detail to avoid unnecessarily obscuring the description of the implementations. In other instances, well-known computer vision methods and techniques for generating sensory data and volumetric representations of one or more objects, etc. have not been described in detail to avoid unnecessarily obscuring the description of the implementations.

[0026] In this specification and claims, unless reason requires, the term "comprise" and its derivatives (such as "comprises" and "comprising") are to be construed as having an open-ended inclusive meaning, i.e., "including," but not limited to.

[0027] Throughout this specification, a reference to "one implementation" or "implementation" or "one embodiment" or "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment is included in at least one implementation or at least one embodiment. Thus, the appearances of the phrases "one implementation" or "implementation" or "in one embodiment" or "in an embodiment" in various places throughout this specification do not necessarily all refer to the same implementation or embodiment. Furthermore, particular features, structures, or characteristics may be combined in any suitable manner in one or more implementations or embodiments.

[0028] As used in this specification and the appended claims, the singular includes the plural unless the content clearly dictates otherwise. Also, the term "or" is generally used in its sense to include "and / or" unless the content clearly dictates otherwise.

[0029] As used herein and in the appended claims, the terms "determine," "determining," and "determined" when used in the context of whether a collision will or will not occur means that an evaluation or prediction is made as to whether a given pose, or a movement between two poses through some intermediate poses, will or may result in a collision between a portion of the robot and some object (e.g., another portion of the robot, a portion of another robot, a permanent obstacle, a transient obstacle, such as a human).

[0030] As used herein and in the appended claims, the term robot refers to a machine that can perform a complex series of operations either autonomously or semi-autonomously. A robot may take the form of a robot having a base, a robotic appendage coupled to the base and movable, and an end effector or end-of-arm instrument carried by the robotic appendage, together with one or more actuators for driving its movements. Such a robot may or may not resemble a human. A robot may alternatively or additionally take the form of a vehicle (e.g., an autonomous or semi-autonomous vehicle).

[0031] The headings and disclosure abstracts provided herein are for convenience only and do not interpret the scope or meaning of any implementations or embodiments.

[0032] 1 illustrates a robotic system 100 including multiple robots 102a, 102b, 102c (collectively 102) operating in a shared workspace 104, employing one or more staging poses to better position the robot to perform a task when the robot's trajectory or path is, for example, blocked or about to be blocked by another robot, and / or when a suitable, unblocked trajectory or path cannot be found, according to one example implementation. Such methods can improve the operation of the robot in various ways, such as reducing the overall time to complete a task, enabling completion of a task that could not otherwise be completed, reducing overall energy usage, reducing the occurrence of collisions, and / or reducing consumption of computational resources.

[0033] The robot 102 may take any of a wide variety of forms. Typically, a robot takes the form of or has one or more robotic appendages. The robot 102 may include one or more linkages having one or more joints and an actuator (e.g., an electric motor, a stepper motor, a solenoid, a pneumatic actuator, or a hydraulic actuator) coupled and operable to move the linkage in response to a control or drive signal. A pneumatic actuator may include, for example, one or more pistons, cylinders, valves, a reservoir of gas, and / or a pressure source (e.g., a compressor, a blower). A hydraulic actuator may include, for example, one or more pistons, cylinders, valves, a reservoir of fluid (e.g., a low compressibility hydraulic fluid), and / or a pressure source (e.g., a compressor, a blower). The robotic system 100 may employ other forms of robots 102, for example, autonomous or semi-autonomous vehicles.

[0034] The shared workspace 104 typically represents a three-dimensional space in which the robots 102a-102c may operate and move, although in certain limited embodiments the shared workspace 104 may represent a two-dimensional space. The shared workspace 104 is a volume or area in which at least portions of the robots 102 may overlap in space and time or may collide if motion is not controlled to avoid collisions. Note that the workspace 104 is distinct from the "configuration space" or "C-space" of each of the robots 102a-102c, described below with reference to, for example, Figures 3A and 3B.

[0035] As described herein, the robot 102a or a portion thereof may constitute an obstacle when considered from the perspective of another robot 102b (i.e., when planning the motion of the other robot 102b). The shared workspace 104 may further include other obstacles, such as machinery (e.g., conveyors 106), posts, columns, walls, ceilings, floors, tables, humans, and / or animals. The shared workspace 104 may further include one or more work items or workpieces 108, such as one or more parcels, packaging, fasteners, tools, items, or other objects, that the robot 102 manipulates as part of performing a task.

[0036] The robotic system 100 can include one or more robot control systems 109a, 109b, 109c (showing three, collectively 109), which include one or more motion planners, e.g., respective motion planners 110a, 110b, 110c (showing three, collectively 110) for each of the respective robots 102a, 102b, 102c. In at least some implementations, a single motion planner 109 may be used to generate motion plans for two, more, or all of the robots 102. The motion planners 110 are communicatively coupled to control each one of the robots 102. The motion planners 110 are also communicatively coupled to receive various types of inputs, including, for example, robot kinematic models 112a, 112b, 112c (collectively 112) and / or task messages 114a, 114b, 114c (collectively 114). The motion planner 110 is optionally communicatively coupled (not shown in FIG. 1 ) to receive motion plans or other motion representations 116a, 116b, 116c (collectively 116) for other robots 102 operating within the shared workspace 104. The robot geometric model 112 defines the geometry of a given robot 102, for example, in terms of joints, degrees of freedom, dimensions (e.g., linkage lengths), and / or C-space of each of the robots 102. (As shown in FIG. 2 , the conversion of the robot geometric model 112 into a motion plan graph may be performed during configuration time that occurs before the robot's runtime or before the robot performs a task, and may be performed, for example, by a processor-based system that is separate and distinct from the robotic system 100 using any of a variety of techniques.) The task messages 114 specify the tasks to be performed, for example, in terms of goals (e.g., a target position, a target pose or end pose, a target configuration or state, and / or an end configuration or state). The execution of a task may also include transitions or movements of each robot 102 through several intermediate poses, configurations, or states.A pose, configuration or state may be defined in the configuration space of the robot (C-space), for example, in terms of joint positions and joint angles / rotations (e.g., joint pose, joint coordinates) of each robot 102. A target position may be specified in real-world coordinates of a workspace (e.g., 2-space, 2D space, 3-space, 3D space) or may be specified in C-space.

[0037] The motion planner 110 is optionally communicatively coupled to receive static object data 118a, 118b, 118c (collectively, static object data 118) as input. The static object data 118 is representative of static objects (e.g., size, shape, location, occupancy) in the workspace 104 and may be known, for example, a priori. The static objects may include, for example, one or more fixed structures in the workspace, such as columns, pillars, walls, ceilings, floors, conveyors 106. Because the robots 102 are operating in a shared workspace, the static objects are typically identical for each robot. Thus, in at least some implementations, the static object data 118a, 118b, 118c provided to the motion planner 110 is identical. In other implementations, the static object data 118a, 118b, 118c provided to the motion planner 110 may vary from robot to robot, for example, based on the position or orientation of the robot 102 in the environment or the robot's 102 perspective of the environment. Additionally, as discussed above, in some implementations, a single motion planner 110 may generate motion plans for two or more robots 102.

[0038] The motion planner 110 is optionally communicatively coupled to receive input sensory data 120, for example provided by a perception subsystem 124. The sensory data 120 represents static and / or dynamic objects within the workspace 104 that are not known a priori. The sensory data 120 may be raw data, as sensed via one or more sensors (e.g., cameras 122a, 122b), and / or raw data that is converted by the perception subsystem 124 into digital representations of obstacles.

[0039] The optional perception subsystem 124 may include one or more processors that may execute one or more machine-readable instructions that cause the perception subsystem 124 to generate respective discretizations of a representation of the environment in which the robot 102 operates to perform tasks for a variety of different scenarios.

[0040] Optional sensors (e.g., cameras 122a, 122b) provide raw sensory information (e.g., point clouds) to the perception subsystem 124. The optional perception subsystem 124 can process the raw sensory information, and the resulting sensory data can be provided as a point cloud, an occupancy grid, a box (e.g., bounding box) or other geometric object, or a stream of voxels (i.e., a "voxel" is equivalent to a 3D pixel or volumetric pixel) representing obstacles present in the environment. Representations of obstacles may optionally be stored in on-chip memory. The sensory data 120 may represent which voxels or sub-volumes (e.g., boxes) are occupied in the environment at the current time (e.g., during runtime). In some implementations, when representing either a robot or another obstacle in the environment, the respective surfaces of the robot or obstacle (e.g., including other robots) may be represented as either voxels or meshes of polygons (often triangles). In some cases, it is advantageous to instead represent the object as a box (rectangular prism, bounding box, hierarchical data structure such as an octree or sphere tree) or other geometric object. Due to the fact that the object is not randomly shaped, there can be a significant amount of structure to how the voxels are organized, with many voxels in the object being immediately next to each other in 3D space. Thus, representing the object as a box may require many fewer bits (i.e., only the x, y, z Cartesian coordinates for two opposing corners of the box may be required). Also, performing an intersection test for a box is comparable in complexity to performing an intersection test for voxels.

[0041] At least some implementations can combine the output of multiple sensors, and the sensors may provide very fine granularity voxelization. However, for the motion planner to efficiently execute motion planning, coarser voxels (i.e., "processor voxels") may be used to represent the environment and the volume in 3D space swept by the robot 102 or a portion thereof as it transitions between various states, configurations, or poses. Thus, the optional perception subsystem 124 may transform the output of the sensors (e.g., cameras 122a, 122b) accordingly. At runtime, if the robot control system 200 determines that any of the sensor voxels within the processor voxel are occupied, the robot control system 200 considers the processor voxel to be occupied and generates an occupancy grid accordingly.

[0042] Various communication paths are indicated by arrows in FIG. 1. The communication paths may take the form of, for example, one or more wired communication paths (e.g., electrical conductors, signal buses, or optical fibers) and / or one or more wireless communication paths (e.g., via RF or microwave radios and antennas, infrared transceivers). In particular, each of the motion planners 110a-110c may be communicatively coupled to one another, either directly or indirectly, to provide motion plans for respective ones of the robots 102a-102c to the others of the motion planners 110a-110c. For example, the motion planners 110a-110c may be communicatively coupled to one another via a network infrastructure, such as a non-proprietary network infrastructure (e.g., an Ethernet network infrastructure) 126. This may advantageously enable the operation of robots from different manufacturers in a shared workspace.

[0043] The term "environment" is used to refer to the robot's current workspace, a shared workspace where two or more robots operate in the same workspace. The environment may include obstacles and / or workpieces (i.e., items with which the robot interacts, acts, or is acted upon). The term "task" is used to refer to a robot task in which the robot transitions from a pose A to a target pose B, preferably without colliding with obstacles in its environment. The target pose is a pose for the robot to perform a specified task, typically on an object, at a location called a goal location or target. Tasks may possibly include grasping or un-grasping an item, moving or dropping an item, rotating an item, picking up or placing an item. The transition from pose A to target pose B may optionally include transitions between one or more intermediate poses. The term "scenario" is used to refer to a class of environment / task pairs. For example, a scenario might be "a pick-and-place task in an environment with a 3-foot table or conveyor, and between x- and y-obstacles with sizes and shapes within a given range," and there could be many different task / environment pairs that fit such criteria, depending on the location of the target and the size and shape of the obstacles.

[0044] The motion planner 110 is operable to dynamically generate motion plans 116 for the robots 102 to perform tasks within an environment while taking into account the planned motions of the other of the robots 102 (e.g., as represented by their respective motion plans 116 or resulting swept volumes). The motion planner 110 may optionally take into account representations of a priori static object data 118 and / or sensory data 120 when generating the motion plan 116. The motion planner 110 advantageously takes into account the known or predicted positions, poses, and / or motion states of the other robots 102 at a given time, e.g., whether another robot 102 has completed a given motion or task, and may enable recalculation of motion plans based on the completion of a motion or task of one of the other robots, e.g., allowing a path or trajectory that was previously ruled out to be selected.

[0045] As described herein, the motion planner 110 uses staging poses to facilitate efficient operation of two or more robots operating within a shared workspace or workcell, and to prevent or at least reduce the risk of the robots or robotic appendages of the robots colliding with each other while efficiently moving one or more of the robots to one or more targets, respectively, to perform their respective tasks within the shared workspace. The motion planner 110 may, for example, identify (e.g., select, generate) one or more staging poses for a robot to advantageously position or configure the robot whose trajectory or path is or will be blocked by one or more other robots, and / or when no suitable, unblocked trajectory or path can be found. Such may be performed, for example, during the runtime of the robot. The staging poses may be identified using various heuristics, and the robot may be efficiently positioned or configured to complete its task once its trajectory or path is unblocked or cleared, and / or not block the trajectory or path of another robot. The system may monitor other robots and move a given robot toward a goal in response to a suitable path becoming unblocked, cleared, or otherwise found, even before the robot has achieved a staging pose.

[0046] Optionally, the motion planner 110 may take into account the operational conditions of the robot 102, such as the occurrence or detection of a blocked trajectory or path, or a predicted blocked trajectory or path, and / or the occurrence or detection of a situation where a suitable, unblocked trajectory or path cannot be found.

[0047] FIG. 2 illustrates an environment according to one example implementation, including a first motion planner 204a in which a first robot control system 200 generates a first motion plan 206a for controlling the operation of a first robot 202 and, optionally, provides the first motion plan 206a and / or a representation of motion as an obstacle to another motion planner 204b in another robot control system 200b via at least one communication channel (represented by a direct arrow, e.g., transmitter, receiver, transceiver, radio, router, Ethernet) for controlling another robot (not shown in FIG. 2 ).

[0048] Similarly, the other motion planner 204b of the other robot control system 200b generates another motion plan 206b for controlling the operation of the other robot (not shown in FIG. 2 ) and optionally provides the other motion plan 206b to the first motion planner 204a and the other of the other motion planners 204b of the other robot control system 200b. The motion planners 204a, 204b can also optionally receive motion completion messages 209 indicating when the motions of the various robots 202 are completed. This may enable the motion planners 204a, 204b to generate new or updated motion plans based on a current or updated state of the environment. For example, after the first robot 202 completes an action that is part or all of a set of actions to be formed as part of the completion of a task by the first robot, a portion of the shared workspace may become unblocked or otherwise available for a second robot to perform the task. Additionally or alternatively, the motion planner 204a can receive information (e.g., images, occupancy grids, joint positions, and joint angles / rotations) collected by various sensors or generated by the other motion planner 204b that indicates when a portion of the shared workspace becomes unblocked or may otherwise become available for a second robot to perform a task after the first robot 202 completes an action that is part or all of a set of actions to be formed as part of the completion of a task by the first robot.

[0049] The robot control system 200 may, for example, be communicatively coupled to receive representations of the motion planning graph 208 and / or the swept volume 211 from one or more sources 212 of the motion planning graph 208 and / or the swept volume 211 via at least one communication channel (represented by direct arrows, e.g., transmitter, receiver, transceiver, radio, router, Ethernet). The sources 212 of the motion planning graph 208 and / or the swept volume 211 may be separate and distinct from the motion planners 204a, 204b according to one illustrated implementation. The sources 212 of the motion planning graph 208 and / or the swept volume 211 may, for example, be one or more processor-based computing systems (e.g., server computers), which may be operated or controlled by the respective manufacturer of the robot 202 or by some other entity. Each motion planning graph 208 may include a set of nodes 214 (only two are shown in FIG. 2 ) that represent a state, configuration, or pose of the respective robot, and a set of edges 216 (only two are shown in FIG. 2 ) that connect each pair of nodes 214 and represent legal or valid transitions between the states, configurations, or poses. The states, configurations, or poses may represent, for example, a set of joint positions, orientations, poses, or coordinates for each of the joints of the respective robot 202. Thus, each node 214 may represent a pose of the robot 202 or a portion thereof as fully defined by the poses of the joints that make up the robot 202. The motion planning graph 208 may be determined, set up, or defined prior to runtime, for example, prior to runtime or during configuration time (i.e., defined prior to execution of a task). The sweep volume 211 represents a respective volume that the robot 202 or a portion thereof will occupy when performing an action or transition corresponding to each edge 216 of the motion planning graph 208. The swept volume 211 may be represented in any of a variety of forms, for example, as voxels, a Euclidean distance field, or a hierarchy of geometric objects.This advantageously allows some of the most computationally intensive work to be performed before runtime, when responsiveness is not a particular concern.

[0050] Each robot 202 may optionally include a base (not shown in FIG. 2). The base may be fixed in the environment or may be movable therein (e.g., an autonomous or semi-autonomous vehicle). Each robot 202 may optionally include a set of links, joints, end-of-arm tools or end effectors, and / or actuators 218a, 218b, 218c (three shown, collectively 218) operable to move the links about the joints. The set of links, joints, end-of-arm tools, or end effectors typically comprise one or more appendages of the robot, which may be movably coupled to the base of the robot. Each robot 202 may optionally include one or more motion controllers (e.g., motor controllers) 220 (only one shown) that receive control signals, for example in the form of motion plans 206a, and provide drive signals to drive the actuators 218. Alternatively, the motion controllers 220 may be separate from and communicatively coupled to the robot 202.

[0051] There may be a respective robot control system 200 for each robot 202, or one robot control system 200 may execute the motion planning for more than one robot 202. For illustrative purposes, one robot control system 200 is described in detail. Those skilled in the art will recognize that this description may be applied to similar or even identical additional instances of other robot control systems 200.

[0052] The robotic control system 200 may include one or more processors 222 and one or more associated non-transitory computer-readable or processor-readable storage media, such as, for example, a system memory 224a, a drive 224b, and / or memory or registers (not shown) of the processor 222. The non-transitory computer-readable or processor-readable storage media (e.g., the system memory 224a, the drive 224b) are communicatively coupled to the processor 222a via one or more communication channels, such as a system bus 234. The system bus 234 may employ any known bus structure or architecture, including a memory bus with a memory controller, a peripheral bus, and / or a local bus. One or more of such components may also, or instead, communicate with each other via one or more other communication channels, such as, for example, one or more parallel cables, serial cables, or high-speed wireless network channels, such as, for example, Universal Serial Bus ("USB") 3.0, Peripheral Component Interconnect Express (PCIe), or via Thunderbolt®.

[0053] The robotic control system 200 may also be communicatively coupled to one or more remote computer systems, such as a server computer (e.g., source 212 of motion planning graph 208), desktop computer, laptop computer, ultra-portable computer, tablet computer, smartphone, wearable computer, and / or sensor (not shown in FIG. 2 ), that are directly or indirectly communicatively coupled to various components of the robotic control system 200, for example, via a network interface 227. The remote computing systems (e.g., server computer (e.g., source 212 of motion planning graph)) may be used to program, configure, control, or otherwise interface with, or input data (e.g., motion planning graph 208, swept volume 211, task specification 215) to the robotic control system 200 and various components therein. Such connections may be over one or more communication channels, such as one or more wide area networks (WANs), such as Ethernet, or the Internet, using Internet Protocol. As described above, the pre-runtime calculations may be performed by the robot control system 200 or a system separate from the robot 202, and the runtime calculations may be performed by the processor 222 of the robot control system 200, which in some implementations may be on-board the robot 202.

[0054] As previously mentioned, the robotic control system 200 may include one or more processors 222 (i.e., circuitry), non-transitory storage media (e.g., system memory 224a, drive 224b), and a system bus 234 coupling various system components. The processor 222 may be any logical processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphics processing units (GPUs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), programmable logic controllers (PLCs), and the like. The system memory 224a may include read only memory ("ROM") 226, random access memory ("RAM") 228, flash memory 230, and EEPROM (not shown). A basic input / output system ("BIOS") 232 may form part of the ROM 226 and contains the basic routines that help transfer information between elements within the robotic control system 200, such as during start-up.

[0055] The drives 224b may be, for example, hard disk drives for reading from and writing to magnetic disks, solid state (e.g., flash memory) drives for reading from and writing to solid state memory, and / or optical disk drives for reading from and writing to removable optical disks. The robotic control system 200 may also include any combination of such drives in various different implementations. The drives 224b may communicate with the processor 222 via a system bus 234. The drives 224b may include interfaces or controllers (not shown) coupled between such drives and the system bus 234 as known to those skilled in the art. The drives 224b and their associated computer-readable media provide non-volatile storage of computer-readable and / or processor-readable and / or executable instructions, data structures, program modules, and other data for the robotic control system 200. Those skilled in the art will appreciate that other types of computer-readable media capable of storing data accessible by a computer may be used, such as a WORM drive, a RAID drive, a magnetic cassette, a digital video disk ("DVD"), a Bernoulli cartridge, RAM, ROM, smart cards, etc.

[0056] Executable instructions and data may be stored in system memory 224a, for example, in operating system 236, one or more application programs 238, other programs or modules 240, and program data 242. Application programs 238 may include processor-executable instructions that cause processor 222 to perform one or more of the following: generate a discretized representation of the environment in which robot 202 operates, including obstacles and / or target objects or workpieces in the environment in which planned motions of other robots may be represented as obstacles; generate a motion plan or road map, including calling or otherwise obtaining results of collision assessments, setting cost values ​​for edges in the motion plan graph, and evaluating available paths in the motion plan graph; identify (e.g., select, generate) staging poses, optionally store the determined motion plans or road maps, and / or provide instructions for moving one or more robots according to the motion plan. Motion planning and motion plan construction (e.g., including collision detection or evaluation, updating costs of edges in a motion plan graph based on collision detection or evaluation, identifying staging poses (e.g., via one or more heuristics), and path searching or evaluation including evaluation of viable paths for suitable paths, and optionally selecting a selected path from the viable or suitable paths) may be performed as described herein (e.g., with reference to FIGS. 4, 5, 6, and 7) and in references incorporated herein by reference. Collision detection or evaluation may be performed using various structures and techniques described elsewhere herein. The application program 238 may also include one or more machine-readable and machine-executable instructions that cause the processor 222 to monitor the robot in an environment to determine when a trajectory or path (e.g., a viable path) becomes unblocked or cleared, and, in response to the trajectory or path becoming unblocked or cleared, move the robot toward a goal.The application programs 238 may further include one or more machine-readable and machine-executable instructions that cause the processor 222 to perform other operations, for example, optionally process sensory data (captured via the sensors). The application programs 238 may further include one or more machine-executable instructions that cause the processor 222 to perform various other methods described herein and in the references incorporated herein by reference.

[0057] In various implementations, one or more of the operations described above may be performed by one or more remote processing devices or computers linked through a communications network (e.g., network 210) via network interface 227.

[0058] Although shown in FIG. 2 as being stored in system memory 224a, operating system 236, application programs 238, other programs / modules 240, and program data 242 may be stored in other non-transitory computer-readable or processor-readable media, such as drive 224b.

[0059] The motion planner 204 of the robotic control system 200 may include dedicated motion planner hardware, or may be implemented in whole or in part via the processor 222 and processor-executable instructions stored in the system memory 224a and / or drive 224b.

[0060] The motion planner 204 may include or implement a motion converter 250, a collision detector 252, a staging pose identifier 253, a staging pose setter 255, a cost setter 254, and a path analyzer 256.

[0061] The motion converter 250 converts the motion of the other of the robot into a representation of an obstacle. The motion converter 250 receives a motion plan 204b from another motion planner 204b or other representation of a motion.

[0062] The motion converter 250 may include a trajectory predictor 251 for predicting the trajectory of a transient object (e.g., other robots, including other objects, e.g., humans), for example, when the trajectory of the transient object is unknown (e.g., the object is another robot, but the motion plan of the other robot has not been received, or the object is not another robot, e.g., a human). The trajectory predictor 251 may, for example, assume that the object will continue its existing motion without change, both in direction, speed, and acceleration. The trajectory predictor 251 may, for example, take into account expected changes in the motion or path of the object, for example, when the path of the object will result in a collision and therefore the object may be expected to stop or change direction, or if the object's goal is known, the object may be expected to stop upon reaching the goal. The trajectory predictor 251 may use a learned behavior model of the object to predict the trajectory of the object, at least in some examples.

[0063] The motion converter 250 then determines an area or volume that corresponds to the motion. For example, the motion converter may convert the motion, as represented by the motion plan, into a corresponding swept volume, i.e., a volume swept by the corresponding robot or portion thereof as it moves or transitions between poses. Advantageously, the motion converter 250 need only queue the obstacles (e.g., swept volumes) and need not determine, track, or indicate the time of the corresponding motion or swept volume. Although described as a motion converter 250 for a given robot 202 converting the motions of other robots into obstacles, in some implementations the other robots may provide obstacle representations (e.g., swept volumes) of specific motions to the given robot 202.

[0064] Collision detector 252 performs collision detection or analysis to determine whether a transition or movement of a given robot 202 or part thereof results in a collision with an obstacle. As described above, the movements of other robots may be advantageously represented as obstacles. Thus, collision detector 252 may determine whether movement of one robot results in a collision with another robot moving through a shared workspace.

[0065] In some implementations, the collision detector 252 implements software-based collision detection or evaluation, for example, performing bounding box-bounding box collision evaluation or evaluation based on a hierarchy of geometric (e.g., spherical) representations of the volumes swept by the robot 202, 202b or portions thereof during movement. In some implementations, the collision detector 252 implements hardware-based collision detection or evaluation, for example, employing a set of dedicated hardware logic circuits to represent obstacles and streaming a representation of the movement through the dedicated hardware logic circuits. In hardware-based collision detection or evaluation, the collision detector may use one or more configurable arrays of circuits, for example, one or more FPGAs 258, and may optionally generate a Boolean collision assessment.

[0066] In response to determining that another robot (e.g., a second robot) is blocking or will block a feasible trajectory or a feasible path toward a first goal for the robot (e.g., a first robot), a staging pose identifier 253 may identify a staging pose for the first robot. The staging pose identifier 253 may, for example, select a pose corresponding to a node in the motion plan graph for the staging pose. The staging pose may be a pose corresponding to a node already included in a feasible path between the current pose of the first robot and the first goal pose for the first robot. Alternatively, the staging pose may be a pose corresponding to a node not yet included in a feasible path between the current pose of the first robot and the first goal pose for the first robot. Thus, the staging pose may, for example, be a new node added to a feasible path to create a “corrected” feasible path between the current pose of the first robot and the first goal for the first robot. Note that the new node may have one or more intermediate nodes between itself and the closest (least number of edges) node in the existing feasible path. Thus, generating a modified feasible path may involve adding one, two, or even more nodes to an existing feasible path.

[0067] The staging pose identification device 253 may use one or more heuristics to identify the staging pose. The staging pose identification device 253 may, for example, determine a first staging pose based at least in part on the expected movement of at least the second robot. The staging pose identification device 253 may, for example, determine a first staging pose to place the first robot in a position that minimizes the probability of collision with the second robot when the second robot moves according to the expected movement. The staging pose identification device 253 may, for example, autonomously determine a first staging pose for the first robot, for example, by determining the first staging pose based at least in part on an evaluation of a respective distance from a first target for each of a plurality of poses.

[0068] In response to determining that another robot (e.g., a second robot) is blocking or will block a feasible trajectory or path toward a first goal for the robot (e.g., a first robot), the staging pose setter 255 may set a staging pose, e.g., using a staging pose identified by the staging pose identifier 253. For example, the staging pose setter 255 may modify the feasible path in the motion plan graph to include the staging pose, and any intermediate nodes, in the feasible path, e.g., resulting in a modified feasible path.

[0069] The cost setter 254 may set or adjust costs of edges in the motion plan graph based at least in part on the collision detection or evaluation. For example, the cost setter 254 may set relatively high cost values ​​for edges that represent transitions between states or movements between poses that result in or may result in collisions. Also, for example, the cost setter 254 may set relatively low cost values ​​for edges that represent transitions between states or movements between poses that do not result in or may not result in collisions. Setting the costs may include setting cost values ​​that are logically associated with the corresponding edges via some data structure (e.g., a field, a pointer, a table).

[0070] A path analyzer 256 may use the motion planning graph with cost values ​​to determine a (e.g., optimal or optimized) path. For example, the path analyzer 256 may configure a least cost path optimizer that determines a lowest or relatively low cost path between two states, configurations, or poses represented by respective nodes in the motion planning graph. The path analyzer 256 may use or execute any of a variety of path-finding algorithms, such as a least cost path-finding algorithm, taking into account a cost value associated with each edge that represents a collision probability and, optionally, one or more of a collision severity, energy consumption or consumption, and / or a time or latency to execute or complete.

[0071] Various algorithms and structures for determining a minimum cost path may be used, including those implementing the Bellman-Ford algorithm, although other algorithms and structures may be used, including but not limited to any such process in which a minimum cost path is determined as a path between two nodes in the motion planning graph 208 such that the sum of the costs or weights of its constituent edges is minimized. This process improves the technique of motion planning for the robot 102, 202 by using a motion planning graph that represents the motions of other robots as obstacles and collision detection to find the "best" path to perform a task without collisions, increasing efficiency and response time.

[0072] The motion planner 204a may optionally include a pruner 260. The pruner 260 may receive information representing the completion of a motion by another robot, referred to herein as a motion completion message 209. Alternatively, it may set a flag indicating completion. In response, the pruner 260 may remove the obstacle or portion of the obstacle representing the now completed motion. This may enable the generation of a new motion plan for the given robot, which may be more efficient, or may enable the given robot to participate in the execution of a task that was previously blocked by the motion of the other robot. This approach advantageously allows the motion converter 250 to ignore the timing of the motion when generating the obstacle representation of the motion, while still achieving better throughput than using other techniques. The motion planner 204a may additionally send signals, prompts, or triggers to cause the collision detector 252 to perform new collision detection or evaluation given the obstacle modifications and generate an updated motion plan graph with modified edge weights or costs associated with the edges, and to cause the cost setter 254 and path analyzer 256 to update the cost values ​​and determine a new or modified motion plan accordingly.

[0073] The motion planner 204a may optionally include an environment converter 263 that converts output (e.g., a digitized representation of the environment) from optional sensors 262 (e.g., a digital camera) into a representation of obstacles. Thus, the motion planner 204a may execute a motion plan that takes into account transient objects in the environment, such as people, animals, etc.

[0074] The processor 222 and / or the motion planner 204a may be or include any logical processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphics processing units (GPUs), application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), programmable logic controllers (PLCs), etc. Non-limiting examples of commercially available computer systems include, but are not limited to, the Celeron, Core, Core 2, Itanium, and Xeon families of microprocessors offered by Intel Corporation, the K8, K10, Bulldozer, and Bobcat series microprocessors offered by Advanced Micro Devices, Inc., the A5, A6, and A7 series microprocessors offered by Apple Computer, Inc., the Snapdragon series microprocessors offered by Qualcomm, Inc., and the SPARC series microprocessors offered by Oracle Corporation.The construction and operation of the various structures illustrated in FIG. 2 may be implemented using techniques similar to those described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," International Patent Application Publication No. WO2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," and / or U.S. Patent Application No. 62 / 616,783, filed January 12, 2018, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN ENVIRONMENTS WITH DYNAMIC OBJECTS," and / or U.S. Patent Application No. 62 / 616,783, filed January 12, 2018, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN ENVIRONMENTS WITH DYNAMIC OBJECTS." The present invention may implement or use the structures, techniques, and algorithms described in the "VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC OBJECTS" ECTS.

[0075] Although not required, many of the implementations are described in the general context of computer-executable instructions, such as program application modules, objects, or macros, stored on a computer- or processor-readable medium and executed by one or more computers or processors that may perform obstacle representation, collision assessment, and other motion planning operations.

[0076] Motion planning operations may include, but are not limited to, generating or converting one or more, or all, of a representation of the robot geometry based on the geometric model 112 (FIG. 1), the tasks 114 (FIG. 1), and a representation of volumes (e.g., swept volumes) occupied by the robot in various states or poses and / or during movement between states or poses into a digital format, such as a point cloud, a Euclidean distance field, a data structure format (e.g., hierarchical, non-hierarchical), and / or a curve (e.g., a polynomial or spline representation). Motion planning operations may optionally include, but are not limited to, generating or converting one or more, or all, of a representation of static or persistent obstacles (e.g., static object data 118, FIG. 1) and / or sensory data representing static or transient obstacles (e.g., sensory data 120, FIG. 1) into a digital format, such as a point cloud, a Euclidean distance field, a data structure format (e.g., hierarchical, non-hierarchical), and / or a curve (e.g., a polynomial or spline representation).

[0077] Motion planning operations may include, but are not limited to, using various collision assessment techniques or algorithms (e.g., software-based, hardware-based) to determine, detect or predict collisions of various states or poses of the robot, or movements of the robot between states or poses.

[0078] In some implementations, the motion planning operations may include, but are not limited to, determining one or more motion planning graphs, motion plans, or road maps, storing the determined planning graphs, motion plans, or road maps, and / or providing the planning graphs, motion plans, or road maps to control the motion of the robot.

[0079] In one implementation, the collision detection or evaluation is performed in response to a function call or similar process, which returns a Boolean value to the collision detector 252. The collision detector 252 may be implemented via one or more field programmable gate arrays (FPGAs) and / or one or more application specific integrated circuits (ASICs) to perform the collision detection while achieving low latency, relatively low power consumption, and an increased amount of information that can be processed.

[0080] In various implementations, such operations may be performed entirely in hardware circuitry or as software stored in memory storage, such as system memory 224a, and may be executed by one or more hardware processors 222a, such as one or more microprocessors, digital signal processors (DSPs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), graphics processing unit (GPU) processors, programmed logic controllers (PLCs), electrically programmable read only memories (EEPROMs), or may be executed as a combination of hardware circuitry and software stored in memory storage.

[0081] Various aspects of perception, planning graph construction, collision detection, and pathfinding that may be used in whole or in part are also described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," International Patent Application Publication No. WO 2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," and International Patent Application Publication No. WO 2016 / 122840, filed January 5, 2016, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT WITH DYNAMIC OBJECTS ...SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME." No. 62 / 616,783, filed January 12, 2018, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBJECTS," U.S. patent application Ser. No. 62 / 856,548, filed June 3, 2019, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTACLES," and International Patent Application No. PCT / US2020 / 039193, filed June 23, 2020, entitled "MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE," published as WO2020 / 263861.Those skilled in the art will appreciate that the illustrated implementation, as well as other implementations, may be implemented in other system architectures and arrangements, including those of robots, handheld devices, multiprocessor systems, microprocessor-based or programmable consumer electronics, personal computers ("PCs"), networked PCs, minicomputers, mainframe computers, and the like, and / or other computing system architectures and arrangements. An implementation or embodiment or portions thereof may be implemented in a distributed computing environment where tasks or modules are performed by remote processing devices linked through a communications network (e.g., at configuration time and run time). In a distributed computing environment, program modules may be located in both local and remote memory storage devices or media. However, where and how certain types of information are stored is important to help improve the operation plan.

[0082] For example, various motion planning solutions "bake in" a roadmap (i.e., motion planning graph) into a processor (e.g., an FPGA), where each edge in the roadmap corresponds to a non-reconfigurable Boolean circuit in the processor. Designs in which the planning graph is "baked" into the processor pose the problem of having limited processor circuitry to store multiple or large planning graphs, and are generally not reconfigurable for use with different robots.

[0083] One solution offers a reconfigurable design that puts the planning graph information into memory storage. This approach stores the information in memory instead of being burned into the circuit. Another approach uses templated reconfigurable circuits instead of memory.

[0084] As mentioned above, some of the information (e.g., the robot geometric model) may be captured, received, input, or provided during configuration time, prior to runtime. The received information may be processed during configuration time to generate processed information (e.g., a motion planning graph) to accelerate operation or reduce computational complexity during runtime.

[0085] During runtime, collision detection may be performed relative to the entire environment, including determining for any pose or movement between poses whether any part of the robot will collide or is predicted to collide with another part of the robot itself, other robots or parts thereof, permanent or static obstacles in the environment, or transient obstacles in the environment with unknown trajectories (e.g., people).

[0086] 3A and 3B show example motion plan graphs 300a, 300b for a robot 102 (FIG. 1), 202 (FIG. 2), whose goal is to perform a task while avoiding collisions with static and dynamic obstacles, which may include other robots operating in a shared workspace. In particular, the motion plan graph 300a shows a first feasible path 312a between a current node and a goal node. In this example, the first feasible path 312a includes edges between nodes 308a, 308b, 308c, 308d, 308e, 308f, 308g, 308h, and 308i, in sequence, and is shown in FIG. 3A using thicker lines than the other edges in the motion plan graph 300a. The motion plan graph 300b shows a "revised" feasible path 312b between the current node and the goal node, where a staging pose has been added to the first feasible path 312a to obtain the "revised" feasible path 312b. The "revised" feasible path 312b includes edges between nodes 308a, 308b, 308c, 308d, 308e, 308f, 308g, 308h, 308j, 308k, 308j, 308h, 308h, and 308i, in sequence, and is shown in FIG. 3B using thicker lines than the other edges in the motion plan graph 300b. As used herein, the term viable path refers to a path that is completed from a start or current node to a goal node. The feasible routes of the set of feasible routes may be evaluated to identify one or more suitable, or even optimal, feasible routes based at least in part on an associated cost or cost function of each feasible route, and optionally on a threshold or acceptable cost. As described herein, the cost or cost function may represent an assessment of the risk of collision, the severity of a collision, the consumption or amount of energy, and / or the time or latency associated with the feasible routes.

[0087] Each of the motion plan graphs 300a, 300b includes a number of nodes, represented as open circles, connected by edges, represented in the drawing as straight lines between pairs of nodes. A subset of the nodes is designated as nodes 308a-308k. 呼A node is called out (or displayed / called out) on the edge of the robot 102, 202, and a subset of edges are called as edges 310a-310i. Each node implicitly or explicitly represents a time and variables that characterize a state of the robot 102, 202 in the configuration space of the robot 102, 202. The configuration space, often referred to as C-space, is the space of states or configurations or poses of the robot 102, 202 represented in the motion planning graph 300a, 300b. For example, each node may represent a state, configuration, or pose of the robot 102, 202, which may include, but is not limited to, a position, orientation, or pose (i.e., position and orientation). The state, configuration, or pose may be represented, for example, by a set of joint positions and joint angles / rotations (e.g., joint poses, joint coordinates) for the joints of the robot 102, 202.

[0088] The edges in the motion planning graphs 300a, 300b represent valid or allowed transitions between these states, configurations, or poses of the robot 102, 202. The edges in the motion planning graphs 300a, 300b do not represent actual movements in Cartesian coordinates (2-space or 3-space), but rather represent transitions between states, configurations, or poses in the C-space of the robot. Each edge in the motion planning graphs 300a, 300b represents a transition of the robot 102, 202 between a respective pair of nodes. For example, edge 310a represents a transition of the robot 102, 202 between two nodes. In particular, edge 310a represents a transition between a state of the robot 102, 202 in a particular configuration associated with node 308b and a state of the robot 102, 202 in a particular configuration associated with node 308c. For example, the robot 102, 202 may currently be in a particular configuration associated with node 308a. Although the nodes are shown at various distances from each other, this is for illustrative purposes only and does not relate to any physical distance. Although there is no limit to the number of nodes or edges in the motion planning graph 300a, 300b, the more nodes and edges used in the motion planning graph 300a, 300b, the more accurately and precisely the motion planner can determine a suitable or even optimal path according to one or more states, configurations, or poses of the robot 102, 202 to perform a task, since there are more feasible paths from which to select the least cost path.

[0089] Each edge is assigned or associated with a cost value, which may represent a collision assessment for the movement represented by the corresponding edge, and may further represent other information, such as the energy consumption associated with the transition, or the duration to complete the associated movement or transition or task.

[0090] Typically, it is desirable for the robot 102, 202 to avoid certain obstacles, e.g., other robots in the shared workspace. In some situations, it may be desirable for the robot 102, 202 to contact or be in close proximity to certain objects in the shared workspace, e.g., to grasp or move the object or workpiece. Figures 3A and 3B show a motion plan graph 300a and a "modified" motion plan graph 300b, respectively, that are used by a motion planner to identify feasible paths (first feasible path 312a, modified feasible path 312b shown in bold) for the robot 102, 202. The first feasible path 312a and modified feasible path 312b are generated or selected to move the robot 102, 202 to a goal by moving through several "intermediate poses" while avoiding collisions with one or more obstacles. The goal is typically where the robot 102, 202 will perform a specified task (e.g., pick and place an object), and the goal pose is the pose of the robot to perform the specified task.

[0091] Obstacles may be digitally represented, for example, as bounding boxes, oriented bounding boxes, curves (e.g., splines), Euclidean distance fields, or hierarchies of geometric entities, whichever digital representation is most appropriate for the type of obstacle and the type of collision detection performed, which may itself depend on the particular hardware circuitry employed. In some implementations, a swept volume in a roadmap for the robot 102 is pre-computed. Examples of collision assessment are described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," U.S. Patent Application No. 62 / 722,067, filed August 23, 2018, entitled "COLLISION DETECTION USEFUL IN MOTION PLANNING FOR ROBOTICS," and International Patent Application Publication No. WO2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME."

[0092] The motion planner 110a, 110b, 110c (FIG. 1), 204 (FIG. 2) or a portion thereof (e.g., collision detector 252, FIG. 2) determines or evaluates the possibility or probability that a pose (represented by a node) and / or action or transition (represented by an edge) will result in a collision with an obstacle. In some cases, this determination results in a Boolean value, while in other cases, this determination may be expressed as a probability.

[0093] For nodes in the motion planning graph 300a, 300b where a direct transition between the nodes has a probability of causing a collision with an obstacle, the motion planner (e.g., cost setter 254, FIG. 2) assigns a cost value or weight to the edges of the planning graph 300 that transition between the nodes (e.g., edges 310a, 310b, 310c, 310d, 310e, 310f, 310g, 310h) that indicate the probability of collision with an obstacle.

[0094] For example, the motion planner may assign a cost value or weight having a value equal to or close to zero for each of the several edges of the motion plan graph 300a, 300b that have a respective probability of collision with an obstacle below a defined collision threshold probability. In this example, the motion planner assigns a cost value or weight of zero to edges in the motion plan graph 300a, 300b that represent transitions or actions of the robot 102, 202 that have no or little chance of collision with an obstacle. For each of the several edges of the motion plan graph 300a, 300b that have a respective probability of collision with an obstacle in the environment that exceeds a defined collision threshold probability, the motion planner assigns a cost value or weight having a value significantly greater than zero. In this example, the motion planner assigns a cost value or weight greater than zero to edges in the motion plan graph 300a, 300b that have a relatively high probability of collision with an obstacle. The particular threshold used for the collision probability may vary. For example, the threshold may be a probability of collision of 40%, 50%, 60%, or lower or higher. Also, assigning cost values ​​or weights having a value greater than zero may include assigning weights having a magnitude greater than zero that correspond to the respective probabilities of collision. For example, as shown in motion planning graphs 300a, 300b, the motion planner assigns relatively high cost values ​​or weights of 10 to some edges that have a higher probability of collision, while assigning relatively low magnitude cost values ​​or weights of 0 to other edges that the motion planner determines have a much lower probability of collision. In other implementations, the cost values ​​or weights may present a binary choice between collision and no collision, and only two cost values ​​or weights are selected in assigning cost values ​​or weights to edges.

[0095] The motion planner (e.g., cost setter 254, FIG. 2) may assign, set, or adjust a cost value or weight for each edge based on factors or parameters (e.g., energy consumption, associated movements, transitions, and / or overall time taken to complete a task) in addition to based on the associated collision probability.

[0096] After the motion planner sets cost values ​​or weights representing the probability of the robot 102, 202 colliding with an obstacle based at least in part on the collision assessment, the motion planner (e.g., the path analyzer 256 in FIG. 2) identifies or generates a feasible path (e.g., a feasible path having valid transitions between a start node and a goal node) and optionally performs optimization to identify a suitable path (e.g., a feasible path that satisfies one or more conditions or thresholds) or even select a selected feasible path (e.g., an optimal feasible path or a best feasible path or a best optimal feasible path) in the resulting motion plan graph 300a, 300b, which provides a motion plan for the robot 102, 202 as specified by, for example, the first feasible path 312a or the modified feasible path 312b.

[0097] In one implementation, once all edge costs of the motion planning graph 300a, 300b have been assigned or set, the motion planner (e.g., the path analyzer 256, FIG. 2) may perform calculations to determine a minimum cost path to or towards the goal represented by the goal node 308i. For example, the path analyzer 256 (FIG. 2) may perform a minimum cost path algorithm from the current state of the robot 102, 202 represented by the current node 308a in the motion planning graph 300a, 300b to a possible state, configuration, or pose. The minimum cost (closest to zero) path in the motion planning graph 300a, 300b is then selected by the motion planner. As explained above, the cost may reflect not only the probability of collision but also other factors or parameters (e.g., energy consumption, the overall time it takes to complete the associated moves, transitions, and / or tasks). 3A and 3B, the current state, configuration, or pose of the robot 102, 202 in the motion planning graph 300a, 300b is at node 308a, and the goal state, configuration, or pose of the robot 102, 202 that positions at least a portion of the given robot (e.g., end effector, end-of-arm tool) at a goal to perform a given task is at node 308i. In FIG. 3A, a feasible path between the current node 308a and the goal node 308i is shown as a first feasible path 312a (a bold path that includes a segment extending sequentially from node 308a through node 308i) in the motion planning graph 300a. 3B, a feasible path (e.g., modified) between current node 308a and goal node 308i is shown in motion plan graph 300b as modified feasible path 312b (a bold path including a segment extending from node 308a through node 308i, with intermediate nodes 308j and 308k). Note that in this example, to move to a staging pose represented by node 308k, a given robot first transitions to an intermediate pose represented by node 308j, and then transitions to a staging pose represented by node 308k (assuming the path does not become clear or unblocked during these transitions).It should also be noted that in this example, to move from a staging pose represented by node 308k to a goal pose represented by goal node 308i, a given robot first transitions from the staging pose represented by node 308k to an intermediate pose represented by node 308j, and then transitions to the goal pose represented by goal node 308i via a pose represented by node 308h. As described above, a staging pose (represented by node 308k) and an intermediate pose (represented by node 308j) are added to the first feasible path 312a to obtain a modified feasible path 312b. In other implementations or examples, the intermediate poses may be omitted, or two or more intermediate poses may be used. The staging poses may enable the robot 102, 202 to avoid collisions and improve operational efficiency, for example, staging the robot 102, 202 until a trajectory or feasible path is unobstructed or clear by other robots operating in the workspace.

[0098] Although the motion planning graphs 300a, 300b depict a first feasible path 312a and a modified feasible path 312b, respectively, with many sharp turns, such turns do not represent corresponding physical turns in the route, but rather represent logical transitions between states, configurations, or poses of the robot 102, 202. For example, each edge in the identified first feasible path 312a and / or modified feasible path 312b may represent a state change to the physical configuration of the robot 102, 202 in the environment, but may not necessarily represent a change in orientation of the robot 102, 202 corresponding to the angles of the first feasible path 312a and / or modified feasible path 312b depicted in FIGS.

[0099] 4 illustrates a method 400 of operation in a processor-based system for generating a motion planning graph and a swept volume according to at least one illustrated implementation. Method 400 may be performed prior to runtime, e.g., during configuration time. Method 400 may be performed by a processor-based system (e.g., a server computer) that is separate and distinct from, and possibly remote from, one or more robots and / or one or more robot control systems.

[0100] A motion planning graph takes significant time and resources to construct, but as described herein, it need only be done once, e.g., during configuration time that occurs before runtime. Once generated, the motion planning graph may be stored, e.g., in a plan graph edge information memory or other non-transitory computer or processor readable medium, and it is relatively quick and efficient for a processor to swap in and out motion planning graphs, or to select which motion planning graph to use, e.g., based on the current characteristics of the robot (e.g., when the robot is gripping an object of a particular size, etc.).

[0101] As mentioned above, some pre-processing activities may be performed before runtime, and thus in some implementations these operations may be performed by a remote processing device linked through a communication network to the robot control system via a network interface. For example, a pre-runtime programming or configuration stage allows the robot to be prepared for a problem of interest. In such implementations, extensive pre-processing is utilized to avoid runtime calculations. Pre-processing may include, for example, generating pre-computed data (i.e., calculated before the runtime of the robot) regarding volumes in the 3D space swept by the robot when making transitions in the motion planning graph from one state to another, the transitions being represented by edges of the motion planning graph. The pre-computed data may, for example, be stored in a memory (e.g., a planning graph edge information memory) and accessed by a suitable processor during runtime. The system may also build a family of motion planning graphs before runtime corresponding to different possible changing dimensional characteristics of the robot that may occur during runtime. The system then stores such planning graphs in a memory.

[0102] At 402, at least one component of the processor-based system receives a kinematic model representing a robot for which a motion plan is to be executed. The kinematic model, for example, represents a robot appendage having a number of links and a number of joints, a joint being between each pair of links. The kinematic model may be in any known format or may be in a format to be developed. Optionally, at least one component of the processor-based system generates a data structure representation of the robot based at least in part on the kinematic model of the robot. The data structure representation includes a representation of the number of links and joints. A variety of suitable data structures and techniques may be used, for example, a variety of hierarchical data structures (e.g., tree data structures) or non-hierarchical data structures (e.g., EDF data structures). Such may use, for example, any of the structures, methods, or techniques described in PCT / US2019 / 045270, published as WO 2020 / 040979.

[0103] At 404, the processor-based system generates a motion-planning graph for the robot based on the respective robot kinematic model. The motion-planning graph represents each state, configuration, or pose of the robot as a respective node, and represents valid transitions between pairs of states, configurations, or poses as edges connecting corresponding pairs of nodes. Although described in terms of graphs, the motion-planning graph need not necessarily be represented or stored as a traditional graph, but rather may be represented, for example, logically or within a memory circuit or computer processor using any of a variety of data structures (e.g., records and fields, tables, linked lists, pointers, trees).

[0104] At 406, the processor-based system optionally generates a swept volume for each edge of a motion plan graph of the robot for which the motion plan is being executed. The swept volume represents a volume swept by the robot or a portion thereof when executing a motion or transition corresponding to the respective edge. The swept volume may be represented in any of a wide variety of forms, for example, as voxels, a Euclidean distance field, a hierarchy of spheres, or other geometric object.

[0105] At 408, the processor-based system provides the motion planning graph and / or swept volume to the robot control system and / or motion planner. The processor-based system may provide the motion planning graph and / or swept volume over a non-proprietary communication channel (e.g., Ethernet). In some implementations, various robots from different robot manufacturers may operate in a shared workspace. In some implementations, various robot manufacturers may operate their own processor-based systems (e.g., server computers) that generate motion planning graphs and / or swept volumes for the various robots they manufacture. Each robot manufacturer may provide a respective motion planning graph and / or swept volume for use by the robot controller or motion planner.

[0106] The processor-based system may use, for example, any one or more of the structures and methods described in PCT / US2019 / 045270, published as WO 2020 / 040979, PCT / US2020 / 034551, published as WO 2020 / 247207, and / or PCT / US2019 / 016700, published as WO 2019 / 156984, to generate a motion planning graph for the robot on which the motion planning is to be performed and / or to generate a swept volume representative of the robot on which the motion planning is to be performed.

[0107] 5 illustrates a method 500 of operation of a processor-based system for controlling one or more robots according to at least one illustrated implementation. Method 500 may be performed during runtime of the robot. Alternatively, portions of method 500 may be performed during configuration time prior to runtime of the robot, and portions of method 500 may be performed during runtime of the robot.

[0108] Method 500 may be performed for one, two, or more robots operating in a multiple robot operating environment or workspace. For example, method 500 may be performed sequentially for each of two or more robots (e.g., a first robot R1, a second robot R2, a third robot R3). For ease of explanation, method 500 is described with respect to control of a first robot R1, where a second robot R2 in the operating environment potentially represents an obstacle to the movement of the first robot R1. Those skilled in the art will recognize that the method can be extrapolated to control one robot when two or more other robots are present in the operating environment, or to control two or more robots when two or more robots are present in the operating environment. Such may be performed, for example, by sequentially performing method 500 for each robot, and / or by sequentially and iteratively performing method 500 for a given robot with respect to each of the other robots that present potential obstacles to the movement of the given robot.

[0109] Method 500 may be performed during run-time, i.e., while at least one of the robots is operating (e.g., moving, performing a task), typically while more than one robot is operating, e.g., following a configuration time. Although method 500 is described with respect to one processor-based system, method 500 may be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems may, for example, be co-located or located "on-board" each of the robots.

[0110] The method 500 begins at 502, for example, in response to powering up the robot and / or robot control system, in response to a call or interrogation from a calling routine, or in response to receiving a task to be performed by the robot.

[0111] At 504, the processor-based system optionally receives a motion planning graph for one or more robots. For example, the processor-based system may receive the motion planning graph from another processor-based system that generated the motion planning graph during configuration time, e.g., as described and illustrated with respect to FIG. 4. The motion planning graph represents each state, configuration, or pose of the robot as a respective node, and represents valid transitions between pairs of states, configurations, or poses as edges connecting corresponding nodes. Although described with respect to graphs, each motion planning graph need not necessarily be represented or stored as a traditional graph, but rather may be represented, e.g., logically or within a memory circuit or computer processor, using any of a variety of data structures (e.g., records and fields, tables, linked lists, pointers, trees).

[0112] At 506, the processor-based system optionally receives a set of swept volumes for one or more robots, e.g., the robot R1 for which a motion is planned, and optionally other robots R2, R3 operating in the environment. For example, the processor-based system may receive the set of swept volumes from another processor-based system that generated a motion plan graph during configuration time, e.g., as described and illustrated with respect to FIG. 4. Alternatively, the processor-based system may generate the set of swept volumes itself, e.g., based on the motion plan graph. The swept volumes represent respective volumes that are swept by the robots or parts thereof when performing the motions or movements (or translations) corresponding to the respective edges. The swept volumes may be represented in any of a wide variety of forms, e.g., as voxels, Euclidean distance fields, hierarchies of spheres, or other geometric objects.

[0113] At 508, the processor-based system receives a number of tasks, e.g., in the form of task specifications. Task specifications specify a robot task to be performed or executed by the robot. A task specification may specify, for example, a first task in which the robot moves from a first position and a first pose to a second position (e.g., a first target) and a second pose (e.g., a first target pose) and grasps an object at the second position, and a second task in which the robot moves the object to a third position (e.g., a second target) and a third pose (e.g., a second target pose) and releases the object at the third position. Task specifications may take a variety of forms, e.g., a high-level specification (e.g., process and syntax) that requires parsing into lower level specifications. Task specifications may take the form of, for example, a low-level specification that specifies a set of joint positions and joint angles / rotations (e.g., joint poses, joint coordinates).

[0114] At 510, the processor-based system optionally receives or generates a representation of the environment, e.g., a representation of the environment as sensed by one or more sensors (e.g., camera, LIDAR). The representation can represent static or permanent objects in the environment, and / or can represent dynamic or temporary objects in the environment. In some implementations, one or more sensors capture and transmit sensory data to one or more processors. The sensory data can be, for example, occupancy information representing respective positions of obstacles in the environment. The occupancy information is typically generated by one or more sensors positioned and oriented to sense the environment in which the robot is to operate or will operate. The occupancy information may take the form of raw data or pre-processed data, and may be formatted in any existing or later created format or schema. The sensory data can be, for example, a stream indicating which voxels or boxes are occupied by objects in the current environment. The receipt or generation of the representation of the environment can occur for a period of time, e.g., for the entire runtime period, and can occur periodically, aperiodically, intermittently, and / or continuously.

[0115] For each of several iterations, the system receives or generates a respective discretization of a representation of the environment in which the robot 102 operates. Each discretization may, for example, include a respective set of voxels. The voxels of each discretization may, for example, be non-uniform in at least one of size and shape within the respective discretization. The respective distributions of non-uniformity of the voxels of each discretization may, for example, differ from each other. Obstacles may, for example, be digitally represented as bounding boxes, oriented bounding boxes (or bounding boxes), or curves (e.g., splines), with whichever digital representation is most appropriate for the type of obstacle and the type of collision detection being performed, which may itself depend on the particular hardware circuitry or software algorithms used.

[0116] The processor-based system may represent, for example, one or more static or persistent obstacles in the environment, including one or more other robots, and / or one or more dynamic or transient obstacles in the environment.

[0117] For example, at least one component of the processor-based system may receive occupancy information representative of some persistent obstacles in the environment and / or some transient obstacles in the environment. Persistent obstacles are obstacles that generally remain stationary or fixed and do not move during the runtime of the robot, at least during the execution of one or more tasks by the robot. Transient obstacles are obstacles that come in and out, disappear, or are dynamic or moving at least during the runtime of the robot. Thus, transient obstacles, if any, are obstacles that have a known position and orientation in the environment during at least a portion of the runtime, and that did not have a known fixed position or orientation in the environment at the time of configuration. The occupancy information may represent, for example, a respective volume occupied by each obstacle in the environment. The occupancy information may be in any known or developed format. The occupancy information may be generated by one or more sensors, or may be defined or specified via a computer, or even by a human. The occupancy information of the transient obstacles is typically received during the runtime of the robot. Persistent or static objects may be identified and planned during configuration time, while transient or dynamic objects are typically identified and planned during run-time.

[0118] At least one component of the processor-based system may generate one or more data structure representations of one or more objects or obstacles in the environment in which the robot operates. The data structure representation may include representations of some obstacles that, at configuration time, have a known fixed position and orientation in the environment and are therefore referred to as persistent in that those objects are assumed to persist in the environment in a known position and orientation from configuration time to runtime. The data structure representation may include representations of some obstacles that, at configuration time, do not have a known fixed position and orientation in the environment and / or are expected to move during runtime and are therefore referred to as transient phenomena in that those objects are assumed to appear, disappear, or move in the environment in unknown positions and orientations during runtime. A variety of suitable data structures may be used, for example, various hierarchical (e.g., tree data structures) or non-hierarchical (e.g., EDF data structures) and techniques, for example, as described in PCT / US2019 / 045270, published as WO 2020 / 040979. Thus, for example, at least one component of the processor-based system receives or generates a representation of obstacles in the environment as one or more of a Euclidean distance field, a hierarchy of bounding volumes, a tree of axis-aligned bounding boxes (AABBs), a tree of oriented (non-axis-aligned) bounding boxes, a tree of spheres, a hierarchy of bounding boxes with triangular meshes as leaf nodes, a hierarchy of spheres, a tree of k-ary spheres, a hierarchy of axis-aligned bounding boxes, a hierarchy of oriented bounding boxes, and / or an octree that stores voxel occupancy information. In particular, the leaves of any tree-type data structure may be of a different shape than other nodes of the data structure, e.g., all nodes are AABBs except for the root node or leaves, which may take the form of a triangular mesh. Using spheres as bounding volumes facilitates fast comparisons (i.e., it is computationally easy to determine whether spheres overlap one another).

[0119] The processor-based system may, for example, represent obstacles in the environment using any one or more of the structures and methods described in PCT / US2019 / 045270, published as WO 2020 / 040979.

[0120] Optionally, at 512, the processor-based system receives motion plans for one or more other robots, i.e., robots in the environment other than the given robot for which the particular instance of the motion plan is being executed. The processor-based system may advantageously use the motion plans for the one or more other robots to determine whether a trajectory of a given robot (e.g., a first robot R1) will result in a collision or has a non-zero probability of resulting in a collision with another robot (e.g., a second robot R2, a third robot R3), as described herein.

[0121] Optionally, at 514, the processor-based system predicts the movement of obstacles in the environment, e.g., the motion or trajectories of other robots in the environment (e.g., the second robot R2, the third robot R3). The processor-based system may, for example, extrapolate future trajectories from sampling of current trajectories and / or from knowledge or predictions of the other robots' targets, or from predicted or anticipated collisions of the other robots. This may be particularly beneficial when the motion plans of one or more other robots are not known by the processor-based system.

[0122] At 516, the processor-based system represents the other robots and / or the motions of the other robots as obstacles for the robot on which the particular instance of the motion plan is being executed. For example, the processor-based system may queue a swept volume corresponding to each motion as an obstacle in an obstacle queue. The swept volume may be predetermined or calculated for each edge and logically associated with each edge in memory via a data structure (e.g., a pointer). As previously mentioned, the swept volume may be represented in any of a variety of forms, for example, as voxels, a Euclidean distance field, a hierarchy of spheres, or other geometric object.

[0123] At 518, the processor-based system executes a motion plan for a given robot (e.g., the first robot R1) to perform a given task. In particular, the motion plan includes determining (e.g., selecting, generating) a staging pose for the given robot when the given robot or the trajectory of the given robot is blocked or has a defined, non-zero probability of being blocked, e.g., by being blocked or potentially blocked by the motion or trajectory of another robot (e.g., the second robot R2, the third robot R3) or other robots in the surroundings. In at least some implementations, the processor-based system evaluates whether the determined probability of being blocked exceeds a defined threshold probability. At least one implementation of the motion plan is illustrated and described with reference to FIG. 6 below, which includes collision checking and assigning costs to edges representing transitions in the motion plan graph. The example of FIG. 6 is not intended to be limiting, and other embodiments of the motion plan may be used.

[0124] The processor-based system may optionally determine and logically associate one or more trigger conditions with the determined staging pose. The trigger conditions may include, for example, 1) blocking of a given robot and / or blocking of a trajectory of a given robot, and / or 2) occurrence or detection of an error condition or anomaly in the operation of one or more robots. The error or anomaly condition may include a low power condition, a wear condition, or a scheduled service condition for the selected robot, all indicating that the selected robot may not be able to complete the task at a given time. Logically associating the one or more trigger conditions with the determined staging pose may include storing the logical association between the staging pose and the one or more trigger conditions in a memory or other non-transitory storage medium, such as a field or other element of a data structure.

[0125] The processor-based system can optionally determine and logically associate one or more release conditions with the determined staging pose. The release conditions may include, for example, 1) unblocking a given robot and / or unblocking a given robot's trajectory, 2) movement of another robot or completion of another robot's movement, and / or 3) completion of a task by another robot. Logically associating one or more release conditions with the determined staging pose may include storing the logical association between the staging pose and the one or more release conditions in a memory or other non-transitory storage medium, such as a field or other element of a data structure.

[0126] At 520, the processor-based system optionally controls the operation of a given robot (e.g., the first robot R1) for which a motion plan has been generated, causing the given robot to move according to the motion plan. For example, the processor-based system may send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages according to the motion plan. Note that although edges typically define valid transitions between a pair of nodes representing respective poses, any edge may correspond to one, two, or more movements of the robot or part thereof. Thus, an edge may correspond to a single movement of the robot from one pose to another in the pair of poses, or an edge may correspond to multiple movements of the robot from one pose to another in the pair of poses.

[0127] At 522, the processor-based system monitors the planned trajectory of a given robot (e.g., the first robot R1) for which a motion plan was generated. The monitoring may include monitoring to determine whether the robot or the trajectory of the robot is, for example, blocked by another robot. The monitoring may include monitoring to determine whether the given robot or the trajectory of the given robot may (e.g., potentially) be blocked by known or predicted movements of another robot, optionally including an assessment of the probability that such blocking will occur. The monitoring may include monitoring to determine whether the given robot or the trajectory of the given robot will become unblocked or potentially unblocked due to, for example, movements of another robot.

[0128] Monitoring may include monitoring to determine whether a given robot has reached a goal (e.g., a final goal) or achieved a goal pose (e.g., a final goal pose) at which a task will be completed. Monitoring may also include monitoring completion of a motion by the robot or other robots, which may advantageously enable the processor-based system to remove obstacles corresponding to the motion from consideration during subsequent collision detection or evaluation. In some implementations, such may result in the generation of a motion completion message that may be used, for example, to prune (or remove / pruning) the obstacles, as described below with reference to FIG. 7. The processor-based system may rely on the coordinates of the corresponding robot. The coordinates may be based on information from a motion controller, actuators, and / or sensors (e.g., cameras, including DOF cameras and LIDAR, with or without structured lighting, rotational encoders, reed switches). In at least some implementations, monitoring for blocking or unblocking may include performing collision detection using any of a variety of collision detection techniques, such as collision detection using swept volumes representing the volume swept by a given robot and / or the volumes swept by other robots or other obstacles in the environment, or collision detection employing an evaluation of whether two spheres intersect.

[0129] At 524, the processor-based system determines or evaluates whether a given robot (e.g., the first robot R1) or a trajectory of a given robot is blocked or potentially blocked, or optionally a trigger condition has occurred. For example, the processor-based system may perform a collision check on the path or trajectory of the given robot with respect to one or more objects or obstacles in the operating environment. Collision checking is typically performed during runtime of the robot and should be a relatively fast process, but a wide variety of forms of collision checking may be used. The collision check may, for example, use a swept volume that represents an area or volume swept by a given robot when transitioning from one pose to another. The collision check may, for example, use a swept volume that represents an area or volume swept by another robot when transitioning from one pose to another. The swept volume may be advantageously calculated during configuration time prior to runtime. The collision check may use a determination of intersection of spheres that represent the given robot and portions of the obstacle.

[0130] If a given robot (e.g., the first robot R1) or a given robot's trajectory becomes blocked or potentially blocked (e.g., the probability of being blocked or the probability of a collision is equal to or exceeds a predetermined threshold), or alternatively, if some other trigger condition occurs, control proceeds to 526. If the given robot or a given robot's trajectory is not blocked or potentially blocked (e.g., the probability of being blocked or the probability of a collision is less than a predetermined threshold), control proceeds to 532.

[0131] At 526, the processor-based system controls the operation of the given robot (e.g., the first robot R1) for which a motion plan has been generated, and moves the given robot toward a staging pose defined in the motion plan. For example, the processor-based system may send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages toward the staging pose according to the motion plan. It should be noted that in at least some particular implementations, the given robot may move through one or more intermediate poses as it moves toward the staging pose. Furthermore, it should be noted that while moving toward the staging pose, the given robot may not actually reach the staging pose, for example, if the given robot or the trajectory of the given robot becomes unblocked before reaching the staging pose. In at least some implementations, the staging pose may be determined to advantageously transition the given robot to make the trajectory of the given robot unblocked as quickly as possible, even before the given robot acquires the staging pose. In at least some implementations, a staging pose may be determined to favor a given robot's transition to eliminate or reduce (e.g., minimize) the probability that the given robot will block or potentially block other robots from moving along known or predicted trajectories of other robots. Additionally or alternatively, in at least some implementations, a staging pose may be determined to favor a given robot's transition to eliminate or reduce (e.g., minimize) the probability that the given robot's trajectory will block or potentially block other robots from moving along known or predicted trajectories of other robots.

[0132] At 528, the processor-based system determines whether the given robot (e.g., the first robot R1) or the trajectory of the given robot is no longer blocked or is no longer potentially blocked, or optionally another release condition has occurred. For example, the processor-based system may perform a collision check on the path or trajectory of the given robot with respect to one or more objects or obstacles in the operating environment. A wide variety of forms of collision checks may be used, such as collision checks using swept volumes or intersecting spheres, for example. If the given robot or the trajectory of the given robot is no longer blocked or is no longer potentially blocked, or optionally another release condition has occurred, control proceeds to 530. If the given robot or the trajectory of the given robot is no longer unblocked or is no longer potentially unblocked, or optionally another release condition has not occurred, control returns to 526, and the given robot is allowed to continue moving toward or remain in the staging pose. As mentioned above, in at least some particular implementations, a given robot may not actually achieve a staging pose.

[0133] At 530, the processor-based system causes the given robot to continue from the current pose toward a goal per the motion plan. Note that the current pose can be a staging pose, for example, if the staging pose was achieved before the trajectory was unblocked. In other cases, the current pose may not be a staging pose, for example, if the staging pose was not achieved before the trajectory was unblocked. The processor-based system can, for example, send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages according to the motion plan.

[0134] At 532, the processor-based system determines whether a given robot (e.g., the first robot R1) has reached a goal or achieved a goal pose. The processor-based system may rely on coordinates of the given robot. The coordinates may be based on information from motion controllers, actuators, and / or sensors (e.g., cameras, including DOF cameras and LIDAR, with or without structured lighting, rotational encoders, reed switches).

[0135] If the given robot has not reached the goal or achieved the target pose, control returns to 520 or optionally 518 to cause the given robot to continue moving according to the motion plan. If the given robot has reached the goal or achieved the target pose, method 500 ends at 534.

[0136] In particular, in many implementations, the method 500 is repeated for additional tasks and / or additional robots. For example, the processor-based system may determine whether the end of the task queue and / or the motion plan request queue has been reached. For example, the processor-based system may determine whether there are any motion plan requests remaining in the motion plan request queue and / or whether all tasks in the task queue have been completed. In at least some implementations, a set of tasks (e.g., a task queue) may be temporarily depleted, and new or additional tasks may arrive later. In such implementations, the processor-based system may execute a wait loop, checking for new or additional tasks every hour, or waiting for a signal indicating that new or additional tasks are available to be processed and executed.

[0137] In some implementations, multiple instances of method 500 may run on one or more cores of one or more processors, e.g., in parallel with one another, in a multithreaded process. Thus, method 500 may alternatively be repeated, e.g., until affirmatively stopped by a power down state or condition.

[0138] Although the method of operation 500 is described with respect to an ordered flow, various acts or operations are performed simultaneously or in parallel in many implementations. Often, a motion plan for one robot to perform a task may be performed while one or more robots perform the task. Thus, performance of a task by a robot may overlap, be simultaneous, or be parallel with performance of a motion plan by one or more motion planners. Performance of a task by a robot may overlap, be simultaneous, or be parallel with performance of a task by other robots. In some implementations, at least some portions of a motion plan for one robot may overlap, be simultaneous, or be parallel with at least some portions of a motion plan for one or more other robots.

[0139] 6 illustrates a method 600 of operation of a processor-based system for executing motion plans for one or more robots, according to at least one illustrated implementation. Method 600 may be performed during runtime of the robot. Alternatively, portions of method 600 may be performed during configuration time prior to runtime of the robot, and portions of method 600 may be performed during runtime of the robot. Method 600 may be performed, for example, in executing motion plans 518 of method 500 (FIG. 5), although motion plans 518 are not limited to those described and illustrated in method 600.

[0140] Method 600 may be performed for one, two, or more robots operating in a multiple robot operating environment. For example, method 600 may be performed sequentially for each robot (e.g., a first robot R1, a second robot R2, a third robot R3, or even more robots). For ease of explanation, method 600 is described with respect to control of a given robot (e.g., a first robot R1) when other robots (e.g., a second robot R2, a third robot R3) in the operating environment potentially represent obstacles to the movement of the first robot. Those skilled in the art will recognize from the discussion herein that method 600 may be extrapolated to the motion planning of one robot when there are two or more other robots in the operating environment, or to the motion planning of two or more robots when there are two or more other robots in the operating environment. Such may be performed, for example, by sequentially performing method 600 for each robot, and / or, for example, by sequentially and iteratively performing method 600 for a given robot with respect to each of the other robots that present potential obstacles to the movement of the given robot.

[0141] Method 600 can be performed at run-time, during which at least one of the robots is operating (e.g., moving, performing a task), e.g., following a configuration time. Method 600 can be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems can be, for example, co-located or located "on-board" a respective one of the robots.

[0142] Method 600 begins (602), for example, in response to powering up the robot and / or robot control system, in response to a call or interrogation from a calling routine such as a routine that performs method 500, in response to receiving a motion plan request or a motion plan request in a queue of motion plan requests to be performed for the robot, or in response to receiving a task to be performed by the robot.

[0143] At 604, the processor-based system performs collision detection or evaluation for a given robot (e.g., the first robot R1). The processor-based system may use any of a variety of structures and algorithms described herein or in the materials incorporated by reference herein or described elsewhere to perform the collision detection or evaluation. The collision detection or evaluation may include performing collision detection or evaluation for each motion against each obstacle in the obstacle queue. Examples of collision assessment are described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "Motion Planning for Autonomous Vehicles and Reconfigurable Motion Planning Processor"; U.S. Patent Application No. 62 / 722,067, filed August 23, 2018, entitled "Collision Detection Useful in Motion Planning for Robots"; International Patent Application No. WO2016 / 122840, filed January 5, 2016, entitled "Specialized Robot Motion Planning Hardware and Methods of Making and Using the Same"; and International Patent Application No. PCT / US2019 / 045270, filed January 20, 2019, entitled "Specialized Robot Motion Planning Hardware and Methods of Making and Using the Same." The collision evaluation may be used to create a motion plan graph, generate a swept volume, and / or perform collision checking (e.g., sphere intersection evaluation), as described herein, by a processor-based system that may use the collision evaluation to set cost values ​​or functions, or as part of setting cost values ​​or functions for various edges of the motion plan graph, which may be used in generating a motion plan that specifies an appropriate path (e.g., an ordered set of poses) for the robot to accomplish a task.

[0144] At 606, the processor-based system sets cost values ​​or cost functions for edges in the motion plan graph based at least in part on the collision detection or assessment of a given robot (e.g., the first robot R1). The cost values ​​or cost functions may represent a collision assessment or risk of collision (e.g., probability or occurrence). The cost values ​​may further represent the severity of the collision (e.g., amount of resulting damage) if a collision occurs. For example, a collision with a human or other animate object may be considered more serious and therefore more costly than a collision with a wall or table or other inanimate object. The cost values ​​or cost functions may further represent the resource usage or consumption associated with the transition corresponding to each edge, e.g., the amount of energy consumed or the amount of time taken in executing the associated transition. The processor-based system may employ any of a variety of structures and algorithms described herein or in the materials incorporated herein by reference to perform the setting of cost values ​​or cost functions, typically setting or adjusting the cost of edges that have no or low risk of collision to a relatively low value (e.g., zero) and setting or adjusting the cost of edges that result in collisions or have a relatively high risk of collision to a relatively high value (e.g., 100,000). The processor-based system may, for example, set the cost values ​​or cost functions of edges in the motion planning graph by logically associating each with an associated cost value or cost function via one or more data structures.

[0145] At 608, the processor-based system uses the motion planning graph to generate one or more feasible paths. A feasible path is a path that extends from either the start node or the current node through the goal node via valid transitions. A feasible path specifies possible trajectories for a given robot through various poses corresponding to the respective nodes. It should be noted that one or more of the generated feasible paths may not be suitable paths, for example because the cost or cost function at a point along the path, or the accumulated cost or accumulated cost function along the path is too high (e.g., above a threshold cost or value or magnitude), and the processor-based system may ultimately reject the feasible paths as not suitable for performing a particular task by the given robot, even if the performance of that task via the transitions defined by the path is feasible and therefore feasible. Thus, the processor-based system may optionally identify one or more suitable paths from a set of one or more feasible paths, for example based on a cost threshold. Further, as described below with reference to 616, one of the feasible paths, or even one of the suitable paths when such paths are identified, may be selected, for example, via a least-cost analysis.

[0146] At 610, for each feasible path, the processor-based system determines or evaluates whether another robot will block or is likely to block (i.e., relatively high probability) the given robot or a given robot from moving to the goal when the given robot moves along the trajectory specified by the feasible path. The processor-based system may, for example, use collision detection to determine or evaluate whether another robot will block or is likely to block the given robot or a given robot from moving to the goal when the given robot moves along the trajectory specified by the feasible path. For example, a probability of collision exceeding a threshold probability of collision, or a cost value or cost function exceeding a threshold cost or cost value, may indicate a blocking condition. For example, various collision detection approaches may be used, as described elsewhere herein. If another robot or multiple other robots will block or are likely to block the given robot or a given robot from moving to the goal, control proceeds to 612. If the other robot or robots do not block or are likely not to block (ie, relatively low probability of) the given robot or robots from moving to the goal, control proceeds to 616 .

[0147] At 612, for each feasible path that results in the given robot being blocked or likely to be blocked, the processor-based system determines, preferably autonomously, a staging pose for the given robot (R1). The staging pose corresponds to an existing node in the motion plan graph. In particular, there may be one or more intermediate or intervening nodes, and thus corresponding intermediate or intervening poses, between a node on a previously determined feasible path and a node corresponding to the staging pose. It is also noted that the staging pose may correspond to an existing node on a previously determined feasible path.

[0148] The processor-based system may, for example, determine the staging pose during configuration time prior to runtime of the robot, or the processor-based system may determine the staging pose during runtime of the robot. The processor-based system may dynamically determine the staging pose based, for example, on how a given robot is or will be blocked and / or how a given robot blocks or may block one or more other robots.

[0149] The processor-based system can, for example, use one or more heuristics to determine a staging pose, and its associated nodes, that efficiently positions or configures a given robot to complete its task once the robot's path-by-path trajectory is unblocked or cleared, and / or positions or configures a given robot to allow other robots to operate in performing their assigned tasks.

[0150] The processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes that increase the probability that a given robot's trajectory specified by a path will be unblocked or cleared. Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes that decrease the predicted time or duration that a given robot's trajectory specified by a given robot's path will be blocked or not cleared (or decrease the time or duration, i.e., decrease the predicted time or duration that a given robot's trajectory specified by a given robot's path will be blocked or not cleared).

[0151] Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes based at least in part on an evaluation of the distance of the staging pose to a target pose, e.g., to minimize distance, or an evaluation of the distance of a given robot end effector or end-of-arm tool from a target (e.g., a location in real space) when at the staging pose.

[0152] Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes based at least in part on an evaluation of energy consumption (e.g., minimizing energy consumption) to perform the tasks. The processor-based system may, for example, determine a staging pose to minimize energy consumption for a given robot to perform its assigned tasks, to minimize energy consumption for other robots to perform their assigned tasks, and / or to minimize energy consumption for a given robot in combination with other robots to perform their assigned tasks.

[0153] Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes based at least in part on an evaluation of the time consumption (e.g., minimizing time) to perform the tasks. The processor-based system may, for example, determine a staging pose to minimize the time consumption for a given robot to perform its assigned tasks, to minimize the time consumption for other robots to perform their assigned tasks, and / or to minimize the time consumption for a given robot in combination with other robots to perform their assigned tasks.

[0154] Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes that increases the likelihood (or probability) that a trajectory specified by a path of another robot (e.g., a second robot R2 moving toward a current goal, a second robot R2 retreating from a current position, or a second robot R2 retreating from a current goal) is unblocked or cleared. Additionally or alternatively, the processor-based system may, for example, use one or more heuristics to determine a staging pose and its associated nodes that reduces the predicted time or predicted duration, or reduces the time or duration, that a trajectory specified by a path of another robot (e.g., a second robot R2 moving toward a current goal, a second robot R2 retreating from a current position or a current goal) is blocked or not cleared.

[0155] The processor-based system can, for example, take into account the expected or predicted movement of the other robot (e.g., the second robot R2) so that the identified (e.g., selected, generated) staging pose is suitable (e.g., best suited) for the situation that will arise when the other robot moves. For example, consider a situation where the other robot (e.g., the second robot R2) is directly above a table, a given robot (e.g., the first robot R1) for which a motion plan is being executed is currently one meter above the second robot, and the current target of the given robot is on the table directly below the second robot. The shortest trajectory for the first robot R1 is to go straight down towards the table. However, if the expected or predicted trajectory of the second robot R2 is upwards, it is more efficient to have the first robot R1 move sideways first and then downwards.

[0156] At 614, for each feasible path that results in a given robot being blocked or likely to be blocked, the processor-based system adjusts the feasible path to include or add to the feasible path a node corresponding to the determined staging pose, along with any intermediate or intervening nodes, if any. In this regard, it is noted that there may be one or more intermediate or intervening poses between a pose corresponding to a node on a previously determined feasible path and the staging pose, and thus there may be one or more intermediate or intervening nodes corresponding to a respective one of the intermediate or intervening poses. It is also noted that a staging pose may correspond to an existing pose on the feasible path. As previously mentioned, there may be one or more trigger conditions and / or one or more release conditions logically associated with a staging pose.

[0157] Optionally, at 615, the processor-based system identifies one or more suitable paths (also interchangeably referred to as suitable feasible paths or identified suitable feasible paths) from the one or more feasible paths. The processor-based system may, for example, identify a set of feasible paths, each having an associated cumulative cost over the respective feasible path that is less than or equal to a threshold cost. Thus, the set of suitable feasible paths may include feasible paths that meet some defined cost condition. As noted elsewhere, the cost may reflect a variety of parameters, including, for example, risk of collision, severity of collision, energy consumption, duration or latency of execution or completion of a path or assigned task. The processor-based system may also impose other conditions in addition to or instead of the cost condition in identifying suitable feasible paths.

[0158] Optionally, at 616, the processor-based system selects a path, which is referred to as a selected path (or selected path). The processor-based system may, for example, select a path from one or more feasible paths (and thus interchangeably referred to as a selected feasible path). The processor-based system may, for example, optionally select a path from one or more identified suitable paths (and thus interchangeably referred to as a selected suitable path or selected suitable feasible path).

[0159] The processor-based system may, for example, perform a least-cost analysis on a set of feasible paths or a set of suitable paths to find a selected path that has either the smallest cost of all of the feasible paths or a relatively low cost of all of the feasible paths. The cost or cost function may be at least partially representative of collision checking, such that the selection of the path is based at least in part on collision detection or evaluation. The cost value or cost function may further represent other criteria and / or parameters, such as the severity of the collision if a collision occurs, the amount of energy consumed, and / or the amount of time consumed. The processor-based system may select a path (e.g., selected path, selected feasible path, selected suitable path, selected suitable feasible path) using any of the various structures and algorithms described herein or in the materials incorporated herein by reference, for example, via performing a least-cost analysis on feasible paths or suitable paths generated from a motion planning graph having associated cost values.

[0160] At 618, the processor-based system provides a motion plan that implements the selected path (e.g., selected path, selected feasible path, selected suitable path, selected suitable feasible path) to control operation of the given robot (e.g., first robot R1) for which the motion plan was generated. For example, the processor-based system may send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages to move the given robot or a portion thereof (e.g., appendage, end effector, end-of-arm tool) along a trajectory specified by the motion plan.

[0161] Method 600 may terminate at 620, for example, until called again. Alternatively, method 600 may repeat until affirmatively stopped, for example, by a power down state or condition. In some implementations, method 600 may execute as a multi-threaded process on one or more cores of one or more processors.

[0162] Although the method of operation 600 is described with respect to an ordered flow, various acts or operations are performed simultaneously or in parallel in many implementations. Often, a motion plan for one robot to perform a task may be performed while one or more robots perform the task. Thus, performance of a task by a robot may overlap, be simultaneous, or be parallel with performance of a motion plan by one or more motion planners. Performance of a task by a robot may overlap, be simultaneous, or be parallel with performance of a task by other robots. In some implementations, at least some portions of a motion plan for one robot may overlap, be simultaneous, or be parallel with at least some portions of a motion plan for one or more other robots.

[0163] 7 illustrates an optional method 700 of operation in a processor-based system for controlling operation of one or more robots in a multi-robot environment, according to at least one illustrated implementation. Method 700 may be performed, for example, during run-time of one or more robots, as part of the execution of method 500 (FIG. 5) and / or the execution of method 600 (FIG. 6). Method 700 may be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems may, for example, be co-located or located "on-board" each one of the robots.

[0164] Method 700 begins at 702, for example, in response to powering on the robot and / or robot control system, in response to a call or invocation from a calling routine (e.g., method 500), or in response to receiving a set or list or queue of tasks.

[0165] Optionally, at 704, the processor-based system optionally determines whether the corresponding motion is completed. Monitoring the completion of a motion may advantageously enable the processor-based system to remove obstacles corresponding to the motion from consideration during subsequent collision detection or evaluation. The processor-based system may rely on the coordinates of the corresponding robot. The coordinates may be based on information from motion controllers, actuators, and / or sensors (e.g., cameras, rotational encoders, reed switches) to determine whether a given motion is completed.

[0166] Optionally, the processor-based system generates or transmits an operation completion message in response to determining that a given operation is completed at 706. Alternatively, the processor-based system may set and reset a flag.

[0167] At 708, a processor-based system (e.g., obstacle pruner 260, FIG. 2) prune one or more obstacles corresponding to the given operation in response to determining that an operation complete message was generated or received or a flag was set. For example, the processor-based system may remove the corresponding obstacle from the obstacle queue. This advantageously allows collision detection or evaluation to proceed in an environment where swept volumes or other representations (e.g., spheres, bounding boxes) that no longer exist as obstacles have been removed. Also, for example, the processor-based system may remove swept volumes that correspond to edges that represent completed movements of a given robot. This also advantageously allows tracking of operations and corresponding swept volumes without having to track the timing of the operations or the corresponding swept volumes.

[0168] Method 700 may terminate at 710, for example, until called again. Alternatively, method 700 may repeat until affirmatively stopped, for example, by a power down state or condition. In some implementations, method 700 may execute as a multi-threaded process on one or more cores of one or more processors.

[0169] <Example> Example 1. A method of operation of a processor-based system for controlling one or more robots, comprising: For a first robot of the one or more robots, evaluating, by at least one processor, whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first objective for the first robot; in response to determining, by the at least one processor, that the second robot is blocking or will block a feasible path toward a first objective for the first robot, moving the first robot toward a first staging pose; monitoring, by at least one processor, a pose of at least the second robot while at least the first robot moves toward the first staging pose; and in response to determining, by at least one processor, from the monitored pose of at least the second robot that the second robot is not blocking or will not block a feasible path between the current pose of the first robot and the first target for the first robot, moving the first robot toward the first target via a feasible path between the current pose of the first robot and the first target for the first robot.

[0170] Example 2. The method of example 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check between the feasible path and a current pose of the second robot.

[0171] Example 3. The method of example 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check between the feasible path and a path of the second robot.

[0172] Example 4. The method of example 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check between at least one pose of the first robot at each defined time and at least one pose of the second robot at each defined time.

[0173] Example 5. A method as described in Example 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check of one or more swept volumes (or volumes) swept by the first robot or a portion thereof as it transitions (or moves / moves / transitions) along the feasible path.

[0174] Example 6. A method as described in any of Examples 1 to 5, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward the first target for the first robot includes evaluating whether at least the second robot is blocking or will block each of two or more feasible paths toward the first target for the first robot.

[0175] Example 7. The method of example 6, further comprising evaluating, by at least one processor, whether two or more paths toward the first objective for the first robot are feasible paths based at least in part on a respective cost of each of the two or more paths relative to a threshold cost.

[0176] Example 8. The method of any of Examples 1-5, further comprising autonomously determining, by at least one processor, a first staging pose for the first robot.

[0177] Example 9. The method of example 8, wherein the step of autonomously determining the first staging pose for the first robot includes determining a staging pose on the feasible path between a current pose of the first robot and the first goal for the first robot.

[0178] Example 10. The method of example 8, wherein the step of autonomously determining the first staging pose for the first robot includes determining a staging pose that is not on the feasible path between a current pose of the first robot and the first target for the first robot.

[0179] Example 11. The method described in Example 8, wherein the step of autonomously determining the first staging pose for the first robot includes evaluating each of a plurality of postures based at least in part on the probability over time that the first robot will not be blocked by the second robot if the respective pose is selected as the staging pose for the first robot.

[0180] Example 12. The method of example 8, wherein the step of autonomously determining the first staging pose for the first robot includes determining the first staging pose based at least in part on an evaluation of a respective distance of the first robot from the first target for each of a plurality of poses.

[0181] Example 13. The method of example 8, wherein the step of autonomously determining the first staging pose for the first robot includes determining the first staging pose based at least in part on an expected movement of at least the second robot.

[0182] Example 14. The method of example 13, wherein determining the first staging pose based at least in part on the expected movement of at least the second robot includes determining the first staging pose to place the first robot in a position that minimizes the probability of collision with the second robot when the second robot moves according to the expected movement.

[0183] Example 15. The method of example 13, wherein determining the first staging pose based at least in part on the expected movement of at least the second robot includes evaluating each of a plurality of poses based at least in part on a probability over time that the second robot will not be blocked by the first robot if the respective pose is selected as a staging pose for the first robot.

[0184] Example 16. The method of example 1, further comprising pausing the first robot at the first staging pose until the second robot is no longer blocking a feasible path between the first staging pose of the first robot and the first target for the first robot.

[0185] Example 17. The method of example 1, further comprising, in response to determining that the second robot is not or will not block the feasible path between the current pose of the first robot and the first target for the first robot before the first robot reaches the first staging pose, moving the first robot toward the first target via the feasible path between the current pose of the first robot and the first target for the first robot without reaching the first staging pose.

[0186] Example 18. generating a set of feasible paths through a motion planning graph for the first robot; identifying a set of suitable paths from the set of feasible paths based on at least one criterion; selecting a selected one of the suitable paths from the set of suitable paths to generate a motion plan; The method according to any one of Examples 1 to 17, further comprising: moving the first robot according to the motion plan.

[0187] Example 19. The method of example 18, wherein one, more, or all of the actions are performed during at least the runtime of the first robot.

[0188] Example 20. A method as described in any of Examples 1 to 19, further comprising, in response to determining, by at least one processor, from the monitored pose of at least the second robot that the second robot is not or will not block a feasible path between the current pose of the first robot and a first target for the first robot, executing a new iteration of a motion plan for the first robot before moving the first robot towards the first target via a feasible path between the current pose of the first robot and the first target for the first robot, wherein the first target pose positions at least a portion of the first robot at the first target.

[0189] Example 21. A processor-based system for controlling one or more robots, the system comprising: At least one processor; a program that is communicatively coupled to the at least one processor and that, when executed by the at least one processor, causes the at least one processor to: and at least one non-transitory processor-readable medium storing processor-executable instructions for performing a method according to any one of Examples 1 to 20.

[0190] Example 22. A processor-based system for controlling one or more robots, the system comprising: a first robot in a robot environment; at least a second robot in a robotic environment; At least one processor; a program communicatively coupled to the at least one processor and, when executed by the at least one processor, for a first robot of the one or more robots; evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot; in response to determining that the second robot is blocking or will block a feasible path toward a first target for the first robot, moving the first robot toward a first staging pose; monitoring a pose of at least the second robot while at least the first robot moves towards the first staging pose; and at least one non-transitory processor-readable medium storing processor-executable instructions that cause the processor to perform the steps of: in response to a determination that the second robot is not blocking or will not block a feasible path between a current pose of the first robot and a first target for the first robot, moving the first robot toward the first target via a feasible path between a current pose of the first robot and the first target for the first robot.

[0191] Example 23. The processor-based system of Example 22, wherein to evaluate whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first goal for the first robot, the at least one processor performs a collision check between the feasible path and a current pose of the second robot.

[0192] Example 24. The processor-based system of Example 22, wherein to evaluate whether at least a second robot of the one or more robots is blocking or blocks a feasible path toward a first goal for the first robot, the at least one processor performs a collision check between the feasible path and a path of the second robot.

[0193] Example 25. The processor-based system of Example 22, wherein the at least one processor performs a collision check between at least one pose of the first robot at each defined time and at least one pose of the second robot at each defined time to evaluate whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot.

[0194] Example 26. The processor-based system of Example 22, wherein to assess whether at least a second robot of the one or more robots is blocking or blocks a feasible path toward a first target for the first robot, the at least one processor performs a collision check of one or more swept volumes swept by the first robot or a portion thereof as it transitions along the feasible path.

[0195] Example 27. A processor-based system described in any of Examples 22 to 26, wherein to evaluate whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first goal for the first robot, the at least one processor evaluates whether the second robot is blocking or will block each of two or more feasible paths toward a first goal for the first robot.

[0196] Example 28. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: 27. The processor-based system of example 26, further comprising: executing a step of evaluating whether two or more paths toward the first target for the first robot are feasible paths based at least in part on a respective cost of each of the two or more paths relative to a threshold cost.

[0197] Example 29. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: A processor-based system according to any one of Examples 22 to 26, which executes the step of autonomously determining a first staging pose for the first robot.

[0198] Example 30. A processor-based system as described in Example 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines a staging pose on the feasible path between the current pose of the first robot and the first goal for the first robot.

[0199] Example 31. The processor-based system of Example 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines a staging pose that is not on the feasible path between the current pose of the first robot and the first target for the first robot.

[0200] Example 32. A processor-based system as described in Example 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor evaluates each of a plurality of poses based at least in part on the probability over time that the first robot will not be blocked by the second robot if the respective pose is selected as the staging pose for the first robot.

[0201] Example 33. The processor-based system of Example 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines the first staging pose based at least in part on an evaluation of a respective distance of the first robot from the first target for each of a plurality of poses.

[0202] Example 34. A processor-based system as described in Example 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines the first staging pose based at least in part on expected movement of at least the second robot.

[0203] Example 35. The processor-based system of Example 34, wherein to determine the first staging pose based at least in part on expected movement of at least the second robot, the at least one processor determines the first staging pose to place the first robot in a position that minimizes the probability of collision with the second robot when the second robot moves according to the expected movement.

[0204] Example 36. A processor-based system as described in Example 34, wherein to determine the first staging pose based at least in part on expected movement of at least the second robot, the at least one processor evaluates each of a plurality of poses based at least in part on a probability over time that the second robot will not be blocked by the first robot if the respective pose is selected as the staging pose for the first robot.

[0205] Example 37. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: The processor-based system of Example 22, further comprising: executing a step of pausing the first robot at a first staging pose until the second robot is no longer blocking a feasible path between the first staging pose of the first robot and the first target of the first robot.

[0206] Example 38. The processor-based system of Example 22, wherein in response to determining that the second robot is not or will not block the feasible path between the current pose of the first robot and a first target for the first robot before the first robot reaches the first staging pose, the at least one processor moves the first robot toward the first target via the feasible path between the current pose of the first robot and the first target for the first robot without reaching the first staging pose.

[0207] Example 39. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: generating a set of feasible paths through a motion planning graph for the first robot; identifying a set of suitable paths from the set of feasible paths based on at least one criterion; selecting a selected one of the suitable paths from the set of suitable paths to generate a motion plan; The processor-based system of any one of Examples 22 to 38, further comprising:

[0208] Example 40. A processor-based system as described in Example 39, wherein one, more, or all of the actions are performed during at least the runtime of the first robot.

[0209] Example 41. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: A processor-based system as described in any of Examples 22 to 40, comprising: in response to a determination from the monitored pose of at least the second robot that the second robot is not blocking or will not block a feasible path between the current pose of the first robot and a first target for the first robot, executing a new iteration of a motion plan for the first robot before moving the first robot toward the first target via a feasible path between the current pose of the first robot and the first target for the first robot, wherein the first target pose positions at least a portion of the first robot at the first target.

[0210] Example 42. A processor-based system according to any of Examples 22 to 41, further comprising at least one sensor positioned to sense the first robot and the second robot in the robot environment.

[0211] The foregoing detailed description illustrates various implementations of devices and / or processes through the use of block diagrams, schematic diagrams, and examples. To the extent that these block diagrams, schematic diagrams, and examples include one or more functions and / or operations, it will be apparent to one of ordinary skill in the art that each function and / or operation in these block diagrams, flow diagrams, or examples, individually and / or collectively, may be implemented by various hardware, software, firmware, or virtually any combination thereof. In one implementation, the subject matter may be implemented via Boolean circuits, application specific integrated circuits (ASICs), and / or FPGAs. However, it will be appreciated that the implementations disclosed herein may be implemented, in whole or in part, in a variety of different implementations in a standard integrated circuit, as one or more computer programs running on one or more computers (e.g., as one or more programs running on one or more computer systems), as one or more programs running on one or more processors (e.g., microprocessors), as firmware, or as virtually any combination thereof, and that designing circuitry and / or writing code for software and / or firmware is well within the skills of those of ordinary skill in the art in light of this disclosure.

[0212] Those skilled in the art will recognize that many of the methods or algorithms described herein may employ additional acts, omit some acts, and / or perform acts in an order other than as specified.

[0213] Furthermore, those skilled in the art will appreciate that the mechanisms taught herein can be implemented in hardware, for example, in one or more FPGAs or ASICs.

[0214] The various implementations described above can be combined to provide further implementations. No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS"; International Patent Application Publication No. WO2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME"; International Patent Application Publication No. WO2016 / 122840, filed January 12, 2018, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC OBJECTS"; No. 62 / 616,783, entitled "MOTION PLANNING OF A ROBOT STORING A DISCRETIZED ENVIRONMENT ON ONE OR MORE PROCESSORS AND IMPROVED OPERATION OF SAME," filed on February 6, 2018; U.S. patent application Ser. No. 62 / 626,939, entitled "MOTION PLANNING OF A ROBOT STORING A DISCRETIZED ENVIRONMENT ON ONE OR MORE PROCESSORS AND IMPROVED OPERATION OF SAME," filed on June 3, 2019; U.S. patent application Ser. No. 62 / 856, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTACLES," filed on June 3, 2019;No. 548, U.S. Patent Application No. 62 / 865,431, filed June 24, 2019, entitled “MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE,” International Patent Application No. PCT / US2019 / 016700, filed February 5, 2019, entitled “MOTION PLANNING OF A ROBOT STORING A DISCRETIZED ENVIRONMENT ONE OR MORE PROCESSERS AND IMPROVED OPERATION OF SAME,” published under WO2019 / 156984, and U.S. Patent Application No. 62 / 865,431, filed June 24, 2019, entitled “MOTION PLANNING OF A ROBOT STORING A DISCRETIZED ENVIRONMENT ONE OR MORE PROCESSERS AND IMPROVED OPERATION OF SAME,” published under WO2019 / 156984, International Patent Application No. PCT / US2019 / 016700, filed May 26, 2020, entitled “APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS WITH DYNAMIC OBSTACLES,” published under WO2019 / 156984, and U.S. Patent Application No. 62 / 865,431, filed June 24, 2019, entitled “MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE,” No. PCT / US2020 / 034551, published at WO2020 / 247207, entitled “MOTION PLANNING FOR ROBOTS TO OPTIMIZE VELOCITY WHILE MAINTAINING LIMITS ON ACCELERATION AND JERK,” filed on August 21, 2020, and published at WO2021041223; U.S. Patent Application No. 17 / 153, published at US2021 / 0220994, entitled “CONFIGURATION OF ROBOTS IN MULTI-ROBOT OPERATIONAL ENVIRONMENTS,” filed on January 20, 2021;All commonly assigned U.S. patent applications, U.S. patent applications, and foreign patent applications mentioned herein and / or listed in the Application Data Sheet, including but not limited to US Pat. No. 662, are hereby incorporated by reference in their entirety. These and other changes can be made to the implementations in light of the above detailed description. Generally, in the following claims, the terms used should not be construed to limit the claims to the specific implementations disclosed in the specification and claims, but rather to include all possible implementations along with the full scope of equivalents to which such claims are entitled. Accordingly, the claims are not limited by this disclosure.

Claims

1. 1. A method of operation of a processor-based system for controlling one or more robots, comprising: For a first robot of the one or more robots, evaluating, by at least one processor, whether at least a second robot of the one or more robots is blocking or will block a viable path toward a first objective for the first robot; in response to determining, by at least one processor, that the second robot is blocking or will block the feasible path toward the first target for the first robot, moving the first robot toward a first staging pose; monitoring, by at least one processor, a pose of at least the second robot while at least the first robot moves towards the first staging pose; and in response to determining, by at least one processor, from the monitored pose of at least the second robot that the second robot is not blocking or will not block a feasible path between the current pose of the first robot and the first goal for the first robot, moving the first robot toward the first goal via the feasible path between the current pose of the first robot and the first goal for the first robot.

2. 2. The method of claim 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first goal for the first robot includes performing a collision check between the feasible path and a current pose of the second robot.

3. 2. The method of claim 1, wherein evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check between the feasible path and a path of the second robot.

4. 2. The method of claim 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot comprises performing a collision check between at least one pose of the first robot at each defined time and at least one pose of the second robot at each defined time.

5. 2. The method of claim 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot includes performing a collision check of one or more swept volumes swept by the first robot or a portion thereof as it transitions along the feasible path.

6. 6. The method of claim 1, wherein the step of evaluating whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first goal for the first robot comprises evaluating whether at least the second robot is blocking or will block each of two or more feasible paths toward the first goal for the first robot.

7. 7. The method of claim 6, further comprising: evaluating, by the at least one processor, whether two or more paths toward the first goal for the first robot are feasible paths based at least in part on a respective cost of each of the two or more paths relative to a threshold cost.

8. The method of any one of claims 1 to 5, further comprising the step of autonomously determining, by the at least one processor, the first staging pose for the first robot.

9. 9. The method of claim 8, wherein autonomously determining the first staging pose for the first robot includes determining a staging pose on the feasible path between a current pose of the first robot and the first goal for the first robot.

10. 9. The method of claim 8, wherein autonomously determining the first staging pose for the first robot includes determining a staging pose that is not on the feasible path between a current pose of the first robot and the first goal for the first robot.

11. 9. The method of claim 8, wherein autonomously determining the first staging pose for the first robot includes evaluating each of a plurality of poses based at least in part on a probability over time that the first robot will not be blocked by the second robot if the respective pose is selected as the staging pose for the first robot.

12. 10. The method of claim 8, wherein autonomously determining the first staging pose for the first robot includes determining the first staging pose based at least in part on an evaluation of a respective distance of the first robot from the first target for each of a plurality of poses.

13. 9. The method of claim 8, wherein autonomously determining the first staging pose for the first robot includes determining the first staging pose based at least in part on expected movements of at least the second robot.

14. 14. The method of claim 13, wherein determining the first staging pose based at least in part on expected movements of at least the second robot includes determining the first staging pose to place the first robot in a position that minimizes a probability of collision with the second robot when the second robot moves according to the expected movements.

15. 14. The method of claim 13, wherein determining the first staging pose based at least in part on expected movements of at least the second robot includes evaluating each of a plurality of poses based at least in part on a probability over time that the second robot will not be blocked by the first robot if the respective pose is selected as the staging pose for the first robot.

16. The method of claim 1 , further comprising pausing the first robot at the first staging pose until the second robot is not blocking the feasible path between the first staging pose of the first robot and the first goal for the first robot.

17. 2. The method of claim 1, further comprising: in response to determining that the second robot is not or will not block the feasible path between the current pose of the first robot and the first goal for the first robot before the first robot reaches the first staging pose, moving the first robot toward the first goal via the feasible path between the current pose of the first robot and the first goal for the first robot without reaching the first staging pose.

18. generating a set of feasible paths through a motion planning graph for the first robot; identifying a set of suitable paths from said set of feasible paths based on at least one criterion; selecting a selected one of the suitable paths from the set of suitable paths to generate a motion plan; The method of any one of claims 1 to 17, further comprising the step of: moving the first robot according to the motion plan.

19. The method of claim 18 , wherein one, more, or all of the actions are performed during at least a runtime of the first robot.

20. 20. The method of any one of claims 1 to 19, further comprising: in response to determining, by at least one processor, from the monitored pose of at least the second robot that the second robot is not blocking or will not block a feasible path between a current pose of the first robot and the first target for the first robot, executing a new iteration of a motion plan for the first robot before moving the first robot towards the first target via the feasible path between the current pose of the first robot and the first target for the first robot, wherein a first target pose positions at least a portion of the first robot at the first target.

21. 1. A processor-based system for controlling one or more robots, the system comprising: At least one processor; a program code communicatively coupled to the at least one processor and, when executed by the at least one processor, causing the at least one processor to: At least one non-transitory processor-readable medium storing processor-executable instructions for performing a method according to any preceding claim.

22. 1. A processor-based system for controlling one or more robots, the system comprising: a first robot in a robot environment; at least a second robot in the robot environment; At least one processor; a program communicatively coupled to at least one processor and, when executed by the at least one processor, causing the at least one processor to: For a first robot of the one or more robots, evaluating whether at least a second robot of the one or more robots is blocking or will block a viable path toward a first target for the first robot; in response to determining that the second robot is blocking or will block the feasible path toward the first target for the first robot, moving the first robot toward a first staging pose; monitoring a pose of at least the second robot while at least the first robot moves towards the first staging pose; and in response to determining that the second robot is not blocking or will not block a feasible path between a current pose of the first robot and the first goal for the first robot, moving the first robot toward the first goal via the feasible path between the current pose of the first robot and the first goal for the first robot.

23. 23. The processor-based system of claim 22, wherein to evaluate whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first goal for the first robot, the at least one processor performs a collision check between the feasible path and a current pose of the second robot.

24. 23. The processor-based system of claim 22, wherein to evaluate whether at least a second robot of the one or more robots is blocking or will block a feasible path toward a first target for the first robot, the at least one processor performs a collision check between the feasible path and a path of the second robot.

25. 23. The processor-based system of claim 22, wherein the at least one processor performs a collision check between at least one pose of the first robot at each defined time and at least one pose of the second robot at each defined time to evaluate whether at least a second robot of the one or more robots is blocking or will block a viable path toward a first target for the first robot.

26. 23. The processor-based system of claim 22, wherein to assess whether at least a second of the one or more robots is blocking or will block a feasible path toward a first target for the first robot, the at least one processor performs a collision check of one or more swept volumes swept by the first robot or a portion thereof as it transitions along the feasible path.

27. 27. The processor-based system of any one of claims 22 to 26, wherein to evaluate whether at least a second robot of the one or more robots is blocking or will block a viable path toward a first goal for the first robot, the at least one processor evaluates whether at least the second robot is blocking or will block each of two or more viable paths toward the first goal for the first robot.

28. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to:

27. The processor-based system of claim 26, further comprising: evaluating whether two or more paths toward the first goal for the first robot are feasible paths based at least in part on a respective cost of each of the two or more paths relative to a threshold cost.

29. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: The processor-based system of any one of claims 22 to 26, further comprising: a processor configured to autonomously determine the first staging pose for the first robot.

30. 30. The processor-based system of claim 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines a staging pose on the feasible path between a current pose of the first robot and the first goal for the first robot.

31. 30. The processor-based system of claim 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines a staging pose that is not on the feasible path between a current pose of the first robot and the first goal for the first robot.

32. 30. The processor-based system of claim 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor evaluates each of a plurality of poses based at least in part on a probability over time that the first robot will not be blocked by the second robot if the respective pose is selected as the staging pose for the first robot.

33. 30. The processor-based system of claim 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines the first staging pose based at least in part on an evaluation of a respective distance of the first robot from the first target for each of a plurality of poses.

34. 30. The processor-based system of claim 29, wherein to autonomously determine the first staging pose for the first robot, the at least one processor determines the first staging pose based at least in part on an expected movement of at least the second robot.

35. 35. The processor-based system of claim 34, wherein to determine the first staging pose based at least in part on expected movement of the second robot, the at least one processor determines the first staging pose to place the first robot in a position that minimizes a probability of collision with the second robot when the second robot moves according to the expected movement.

36. 35. The processor-based system of claim 34, wherein to determine the first staging pose based at least in part on expected movement of at least the second robot, the at least one processor evaluates each of a plurality of poses based at least in part on a probability over time that the second robot will not be blocked by the first robot if the respective pose is selected as the staging pose for the first robot.

37. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to:

23. The processor-based system of claim 22, further comprising the step of pausing the first robot at the first staging pose until the second robot is not blocking the feasible path between the first staging of the first robot and the first goal for the first robot.

38. 23. The processor-based system of claim 22, wherein in response to determining that the second robot is not or will not block the feasible path between the current pose of the first robot and the first goal for the first robot before the first robot reaches the first staging pose, the at least one processor moves the first robot toward the first goal via the feasible path between the current pose of the first robot and the first goal for the first robot without reaching the first staging pose.

39. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to: generating a set of feasible paths through a motion planning graph for the first robot; identifying a set of suitable paths from said set of feasible paths based on at least one criterion; selecting a selected one of the suitable paths from the set of suitable paths to generate a motion plan; and moving the first robot according to the motion plan.

40. 40. The processor-based system of claim 39, wherein one, more, or all of the actions are performed during at least a runtime of the first robot.

41. The processor-executable instructions, when executed by the at least one processor, further cause the at least one processor to:

41. The processor-based system of any one of claims 22 to 40, further comprising: in response to determining, from the monitored pose of at least the second robot, that the second robot is not or will not block a feasible path between a current pose of the first robot and the first target for the first robot, executing a new iteration of a motion plan for the first robot before moving the first robot towards the first target via the feasible path between the current pose of the first robot and the first target for the first robot, wherein the first target pose positions at least a portion of the first robot at the first target.

42. The processor-based system of any one of claims 22 to 41, further comprising at least one sensor positioned to sense the first robot and the second robot in the robotic environment.

Images