Roadmap-based Planning in Human-Robot Collaboration Environments

Zahid Iqbal, João Reis, Gil Gonçalves

SYSTEC, Research Center for Systems and Technologies

Faculty of Engineering, University of Porto
Rua Dr. Roberto Frias, 4200-465 Porto, Portugal
Email: {zahid,jpcreis,gil} @fe.up.pt

Abstract—Enabling humans and robots to work together allows potential of the robots by operating them in unstructured
for cost savings and workplace efficiency. The robot must be open environments that change dynamically. A robot would
equipped to perceive humans, and redirect its actions for co- only possess partial information of surroundings before it
operative tasks or under hazardous situations. Thus, dynamic commences operation; obstacles could appear or go away at
motion planning appears as an essential exercise. We use dynamic any time during its operation. This situation poses several
roadmaps for online motion planning in changing environments
combined with a voxel-based grid. The presented approach can
challenges for motion planning [5]. With efficient planning, we
answer path planning queries efficiently. Visualized simulation can leverage the benefits of cooperation by assigning specific
is an important technique for rapid verification of algorithms production tasks to robot and humans, as well as make the
or prototypes. We present an architecture that implements a robot more autonomous. Specifically, it requires the ability to
simulation of the robotic manipulator using the Robot Operating account for new obstacles or changes in the environment and
System (ROS) and MoveIt. continue to plan on the fly and quickly with some accuracy.
Keywords–robotics technology, intelligent robotics, sampling- This work is an incremental exercise concerning building
based planning, perception and sensing, dynamic environments,
voxel-based grid.
a collabarative planning solution. We define collaboration as
working as a team to reach a common goal. In the collaborative
environment, physical contact between human and robot may
I. I NTRODUCTION be inevitable and indeed desirable, enabling the robot to learn
Today, there are opportunities for automation in many in- from experience. To achieve accurate and efficient collabo-
dustries, such as automotive assembly lines, material transport, ration, both robot and human need to perceive each other’s
space exploration. Frequently, robots support this automation, intentions and must know their task set. With that knowledge,
improving thus the production volumes, bringing down costs, the robot can plan and adapt its actions accordingly, ultimately
and improving the precision and accuracy of the production leading to the achievement of the mutual objective, all the
process. Additionally, as robots capabilities increase, they while assuring safety. A typical scenario could be the robot
can take on jobs that might be impossible or dangerous for as a work assistant that can hand in the next tool piece to the
humans [1]. The ability to calculate and execute motion plans human. Robot predicts that this piece is the one needed for the
is central to the operation of these robots. Fundamental motion person to continue its job and is shelved and outside human’s
planning problem is to compute a set of inputs to the robot access. Collaboration scenario primarily comprises a dynamic
that drive it from a start to a goal position while avoiding environment, and any motion planning strategy that tackles the
collisions with the environment obstacles. Early works on dynamic obstacles must allow dealing with the collaboration
robot motion planning assumed a robot’s world to be known episodes accordingly. To this end, different approaches exist,
and fixed at the time of planning. During the execution such as Incremental Path Planning [6] and Experienced based
of the plan, however, the robot might discover a deviation Motion Planning (EBMP) [7]. The later employs a database of
from the assumed world state, such as a human entering its previous searches and recalls an experience from the database
operational area. Typical industrial assembly environments, when it encounters a similar planning exercise. This may
that confined robots to separate operation spaces isolated from involve repairing the previous plan and reduce the time of the
human workers, would bring robot operation to a halt under current query as opposed to planning from scratch. Likewise,
such scenario [2], safety being the primary concern. In other incremental search methods use previous search results to solve
cases, the robot must replan its trajectory from scratch, which similar path planning problems faster.
might be computationally intensive for real-time operation. Our work concerns path planning with obstacle avoidance
Random sampling is an important technique employed by in dynamic collaboration scenarios. For efficient collaboration,
popular planning algorithms such as Probabilistic Roadmaps an intelligent monitoring system is inevitable [8, 9]. The
(PRM) [3], Rapidly Exploring Random Trees (RRT) [4] to present work uses the monitoring solution in [8] and combines
build network of feasible paths. For static environments, these with a PRM based approach, dynamic roadmaps. We organize
methods can efficiently make and execute plans, even when the work presented here in two distinct parts. The first part
there are obstacles. With some of these algorithms, such as explains dynamic motion planning for robots and reviews some
PRM that are multi-query, we can delegate roadmap building strategies that address dynamic environments (Section II). In
as an offline exercise and run multiple queries online on the second part (Section IV), we present the tools that allow
the available roadmap. However, we can realize a better developing motion planning algorithms on a simulated model
of the robot. In Section III, we describe how we integrate x
the monitoring solution with planning algorithm for efficient
motion planning. In Section V, we present the preliminary path
planning architecture using the popular tool support and based (x + vp t, t + τ )
on a voxelized grid. Finally, in Section VI, we conclude the
vp τ (x, t + τ )
paper and present some future work directions. l
(x, t)
II. M OTION PLANNING IN DYNAMIC AND UNKNOWN (x − vp t, t + τ )
ENVIRONEMNTS
The work presented here concerns industrial robotic arms
or so-called manipulators. In particular, we do not consider t
mobile robots; proposed ideas will be applicable to trajectory τ
planning of a Universal Robot with a lift ability of 5 kg Figure 1. A state-time grid of a single roadmap arc [12].
(UR5) [10]. Mounted on a stationary platform, its links with
revolute joints and end-effector can move with a certain degree
of freedom.
preprocessing phase, i.e., each vertex in free interval graph
A. Definitions maps to one node in the roadmap. The approach builds a local
Fundamental to the path planning problem is the concept trajectory first, i.e. traversing a single arc between two roadmap
of configuration and configuration space. The configuration nodes, and then extends to the complete interval graph. Robot
of a robot is a set of independent parameters that char- movement velocity is given maximum vmax . The approach
acterize the position of every point in the object whereas interpolates motion along a single arc using a two-dimensional
configuration space (denoted C) is the space of all possible state-time grid where distance travelled along the arc and time
configurations [11]. The dimension of the configuration space are two dimensions. The state-time space is discretized by time
is determined by the degrees of freedom of the robot. A step τ and velocity value vp < vmax ; l is the length of the arc.
configuration q is a vector of robot joint values. For instance, a From a given state (x, t), three states are reachable (Figure 1).
robotic arm with six revolute joints has six degrees of freedom For an arc a with start state-time (xs , ts ) and destination
and its configuration is denoted q = {q1 , q2 , q3 , q4 , q5 , q6 } state xd , the approach maintains a stack where initially (xs , ts )
where qi denotes ith joint angle. The configuration space C is present. In a loop, it pops a stack element. If it is collision-
is a space of these configurations, i.e., for 6 dimensions, we free and not visited before, it pushes the reachable grid points
have C = R6 . Any path finding strategy places configurations in an order that favours movement towards the goal. The
in C into two categories, those that are free Cf ree , and others algorithm runs until the stack is empty or goal state xd has
that are in collision Cf orb , i.e., occupied by obstacles. A been reached. Backpointers and the visit status of each state are
configuration q ∈ Cf ree if the robot placed at q does not maintained. So if a certain movement encounters an obstacle,
intersect with workspace obstacles. A path is a continuous we move back to the next unexplored state on the stack. The
sequence of configurations in Cf ree connecting initial and goal algorithm chooses a short time step τ for better accuracy of the
configurations. For dynamic scenarios, a notion of time is algorithm. For finding a global trajectory, the algorithm sends
incorporated in C, and resulting space may be termed as state- probes corresponding to single arcs and evaluates a function,
time space [12]. It consists of pair (x, t) where x ∈ C and t f (p) = g(p)+h(p), to determine how promising a given probe
denotes the time. A path obeying dynamic constraints is termed p is. g(p) is the cost so far and h(p) is the estimate until the
as trajectory. goal state. Assuming (xu , tu ) to be the top state on the stack
for probe p, the trivial estimate for g(p) is tu . To estimate h(p),
B. PRM based approaches for dynamic environments it uses a roadmap distance function D(xu .g) from the current
PRM approach, in the planning phase, randomly samples state xu until the goal state g. This estimate could come from
configurations in the free space and builds a roadmap, i.e., running a simple Dijkstra’s algorithm on the roadmap before
Cf ree . In the query phase, it finds paths in the roadmap to the query phase, giving h(p) = D(xu , g)/vmax . The probe
guide robot from start to the goal. Dynamic obstacles in the with minimum f (p) value has the highest priority. For a start
environment can invalidate parts of the previously constructed state s, the algorithm initializes probes on all arcs starting
roadmap in what concerns free space. Therefore, we need from s and puts them on the priority queue, from which it
methods to update the roadmap taking into consideration the examines probes one by one. If the local trajectory found by
current state. a prob reaches the goal, we have a solution. Otherwise, it
The works in [13] and [12] use PRM-based approach to starts probes on outgoing edges and continues. The algorithm
handle dynamic scenarios. The work in [12] extends PRM terminates either when it finds the goal node or if no probes
by augmenting configuration space with a dimension of time. are remaining.
The preprocessing step is like regular PRM. Online, checking The work in [13] start their approach alike the [12], i.e.,
for static obstacles is not required for collision testing. It building the roadmap in the preprocessing phase, accounting
incorporates the notion of free interval, the maximum contin- for static obstacles. In the query phase, at first, the approach
uous segment of time that a configuration (a roadmap node) connects query nodes to the roadmap. Next, it divides roadmap
is collision-free. Arriving earliest in a free interval allows nodes into three categories, those that are reachable from the
reaching the goal faster. Implicitly, this approach builds a free start node, those that are reachable from goal node, and those
interval graph that corresponds to the roadmap built in the that are not reachable from query nodes. A first attempt to
find path labels some edges as blocked; edges that are not
graph search
collision-free due to dynamic obstacles. An essential element
of this approach is the connectivity extension in two ways,
from a query node to a node reachable from the other query
node, and from query nodes to the roadmap portion which is
not yet connected to query nodes. The approach searches for plan
a path with enhanced connectivity. When the approach cannot
retrieve path from the roadmap, it uses RRT-connect [14] update previous
which incrementally builds two random trees one rooted at
search
the start node and the other rooted at the goal node, and execute
tries to find a path. Finally, it can create new nodes if the
existing roadmap does not allow any path. This method can
store the key positions of the moving obstacle in a database.
For subsequent edge labelling, it compares the current position
of an obstacle to the stored value. If it’s the same, the previous no yes
graph changes
exercise (collision testing, and pathfinding) is still valid, thus,
avoiding new effort. Since dynamic obstacles can move at any
time different positions, so, practically, there can be infinite Figure 2. General methodology of incremental path planning.
positions that must be stored. For efficient memory usage,
the approach stores only the last checked positions. However,
this approach is beneficial in certain situations; for example,
when moving obstacles are doors that could be closed or plan, we receive changes, if any, in the graph. This information
open, then storing a few positions might save significant future could come from different sensors outside of the robot, such
recalculations. as laser scanners or RGB cameras. Using this information, we
update the previous map instead of replanning from scratch.
C. Experienced based motion planning We continue to perform this loop until we reach the goal
Experience-based approach to motion planning builds on (Figure 2).
knowledge from past planning exercises, i.e., [7]. It maintains In the following, we briefly discuss an incremental planning
a database of past experiences from which it can retrieve a algorithm D* [15].
partial or complete solution to the current problem. Such an 1) D* algorithm: D* is an incremental search algorithm,
approach may be particularly beneficial in situations where a considered to be the dynamic version of A* algorithm [16]
robot is supposed to be operational in a fixed environment for since it permits edge costs to change as the algorithm runs.
long periods. In such a situation, the robot would frequently The algorithm begins at the goal state donated G and works
encounter similar obstacles and environments. Thus, planning its way back to the start state; on its way, it keeps on reiterating
from scratch is not desirable because of redundant computation and updating costs to reflect optimal paths. Each graph node
effort. Repairing an available solution, instead, is faster. The can be in one of the following states NEW, CLOSED, OPEN,
approach maintains a sparse roadmap for efficient memory RAISE or LOWER. The OPEN list maintains nodes needing
usage. Further, for speed up, it delays the collision checking expansion. The algorithm runs until the current robot state is
until after it has found a candidate path. Later, it tests the CLOSED. The algorithm maintains two cost estimates for each
path for collisions and removes any invalid segments. When node, namely path cost function h(G, X) and key function
the path gets broken, the search is commenced. In a post- k(G, X). Path cost function gives the current estimate of the
processing step, it smoothes repaired paths before sending sum of arc costs from state X to goal state G, and key function
these to the robot. The approach discretizes the solution path denotes the minimum of h(G, X) which we can regard as the
into an ordered set of vertices and incorporates it into the previous estimate of h(G, X). The algorithm sorts the OPEN
experience roadmap. In the query phase, it follows the general list according to nodes key values. When expanding a node on
approach, connecting query nodes to the graph, if this is the OPEN list, the key function allows changing the current
successful, then A* algorithm can return the optimal path node state as RAISE or LOWER. When k(G, X) < h(G, X),
between start and goal nodes. As stated, it checks for possible current estimate indicates an increased cost, so X state is
collisions with environmental obstacles after it has found the changed as RAISE, otherwise as LOWER. Next, it propagates
candidate path. For this case, invariant constraints do not lend changes from expansion to the neighbours that are now placed
any recomputation. Moving obstacles, however, can appear and on the OPEN list and expanded in turn. Each node has a back
invalidate path segments. For this case, a new A* search is pointer which leads to the target. Algorithm reports a solution
run to find the path. The approach to finding a path through path when the next node for expansion is the start node. We
disconnected components is similar to that of [13]. follow back pointers from the start until the goal.
There have been different improvements to the original D*
D. Incremental path planning such as D* Lite [17], and Focussed D* [18]. Focussed D* is an
Incremental search methods reuse recent solutions to speed informed incremental heuristic search algorithm that combines
up searching for similar problems. Assuming that we have ideas from A* and D*. D* Lite, though not directly based on
incomplete information about the world, replanning occurs D*, implements similar ideas to path planning in unknown
from the current state until the goal whenever an obstacle environments as D*. With lower implementation overhead, it
encounters. When we have to execute the next action in the yields better performance.
 III. P ROPOSED APPROACH
In this section, we describe the approach that makes use
of the voxel-based grid for motion planning. We envisage
using a roadmap based approach for its simplicity and ease
of development. This is an extension of previous work [19],
now addressing the dynamic environment.
(500, 100, 200)
A. Dynamic roadmap with voxel-based occupancy grid
(400 cm, 100 voxels)
A voxel represents a value on a regular grid in three- (100, -300, -200)

dimensional space, useful for many applications such as Dy-

namic Roadmaps [20] to create a mapping from an area in
the workspace to states in C. The work in [8] presents a (400 cm, 100 voxels)

monitoring approach that outputs a voxel grid. The voxel-based (400 cm, 100 voxels)

grid renders the entire scene of the collaborative environment

Figure 3. An example voxel grid.
as a grid composed of cubes with a given dimension, known
as voxels. We can describe the granularity of the grid with the
size of one side of the cube. The resolution can be set higher or
lower by choosing the dimension of the unit cube in the grid,
i.e., for a higher resolution, we choose a smaller size of the
voxel, thus corresponding to more voxels in the grid, and vice grid G. The nodes in R are configurations qi in Cf ree , and
versa. The resolution of the grid presents a tradeoff between cells of grid G are denoted c. For each c in G, we check
the accuracy of the planning queries and computation effort. configurations qi representing a position of the end-effector
The monitoring solution can also label the workspace objects. and that fall in c. With a large number of cells in G having
When voxels in the grid are labelled, we can identify, at first, if many such configurations, we can stop computing the roadmap
an object in the collaborative environment occupies the voxel, R, lending a uniform distribution of end-effector positions
and secondly, which type of object, i.e., either a robotic part over the workspace. An occupied c invalidates the respective
itself, human, or an obstacle object. A labelled voxel-based q in R. Obstacles appearing, disappearing, or changing their
grid can efficiently solve planning queries, in particular, in the positions represent dynamic scenarios. The grid G can inform
presence of static obstacles. In the query phase, before the us when a cell gets occupied or becomes free. The scene can
search commences, occupied voxels would indicate Cf orb and update based on actions of an external agent, e.g., a human that
the remaining nodes would make the Cf ree . shares the workspace with the robot. Whenever one or more
objects occupy a cell, a reference counter keeps increasing
In our system, we need an accurate mapping of workspace in respective node or edge in R, denoting invalid nodes. The
objects from the voxel grid to the coordinates that planning approach keeps renewing the state of some nodes or edges
algorithms can work with. Monitoring solution considers the as these become occupied or free. An RRT algorithm is used
origin (0, 0, 0) to be at the base-center of the robotic arm. when the path gets broken, and the algorithm cannot compute
For the tools used in this work (section IV), every point on the query.
the robotic arm, and in the workspace can be calculated in
reference to the planning-frame, which is the frame of the
”base-link” of the arm, and its origin is at position (0, 0, 0). IV. T OOL S UPPORT FOR DEVELOPING MOTION PLANNING
ALGORITHMS
When we have a common reference point, and with some
additional information from the voxel grid such as dimensions In this section, we briefly describe the tools that we have
of individual voxels as well as of the hyper-cube that represents used to develop motion-planning solution.
the complete voxel grid, we can map the goal positions and
obstacle positions from the voxel grid into the planning so- A. Robot Operating System (ROS)
lution, which allows solving subsequent path-planning queries
ROS [21] provides a framework to develop and test robot
efficiently. Figure 3 shows an example voxel grid with extreme-
software as well as to deploy such software to the real robots.
points and dimensions identified.
As the scope of the robots has significantly grown, a fully
However, for dynamic obstacles, we incorporate further functional code must contain software for hardware drivers,
information, such as input from prediction algorithms dic- perception algorithms, abstract reasoning, trajectory planning,
tating the temporal occupancy status of voxels. Accounting control and more. For any single user, it might not be possible
for dynamic scenarios, here we stipulate two possible ways to cover all aspects of software development for a robot. For
to handle it. We can have an additional dimension of time the same reason, the final software architecture would incur
to the voxel-based grid. And, we can associate a probability a significant integration effort. The main objective of ROS
of occupancy procc for any voxel c at any given time t, i.e., is to manage the complexity of software development for
procc (c, t) ∈ [0, 1]. This input, might come from the perception robots. It contains helpful tools and open software packages
sensor, and aid in redirecting paths of the manipulator in the for perception, navigation, transforms and simulations, allow-
presence of dynamic obstacles. ing rapid prototyping of software for experiments. Emulating
Secondly, we employ ideas from the reviewed approaches, the motion of robots and the world in a virtual workspace
in particular, those based on random roadmaps in combination without physically depending on an actual robot saves cost
with voxel-based grid for dynamic path planning of robotic and time. ROS supports development in different programming
manipulator. Consider the roadmap R and the voxel-based languages, mainly C++ and Python.
 roscore respective forward and inverse kinematics analysis to produce
the relative transforms. The model also allows specifying
visuals for robot parts using notations as a cylinder, sphere
or box, and colour values with material tags. More complex
ROS master Parameter server rosout information, such as collision information, inertia, joint and
velocity limits, can also be given here. Specific utilities, e.g.,
urdf parser can check for the validity of the URDF file.
ROS master
C. MoveIt
registration registration MoveIt [22, 23] is a set of software packages with spe-
cific capabilities for mobile manipulation, such as kinematics,
messages motion planning and control, 3D perception and navigation.
node 1 node 2 MoveIt is integrated with standard ROS. It uses plugins for
publisher subscriber most of its functionality; motion planning (Open Motion
Planning Library (OMPL) [24] ), collision detection (default:
publish subscribe Fast Collision Library (FCL) [25], and kinematics (default:
OROCOS Kinematics and Dynamics Library (KDL) for for-
Topic ward and inverse kinematics for generic arms.
1) MoveIt setup assistant (SA): has the objective to re-
Figure 4. ROS core communication system and an example of nodes publish duce the entry barrier for developing robot software. It can
& subscribe to topics. automatically set up the task pipeline for producing an initial
configuration quickly. Robot model URDF (section IV-B) is a
prerequisite for MoveIt setup assistant. The configurations set
by the assistant include self-collision matrix, planning group
Conceptually, the computation graph within ROS is the definitions, robot poses, end effector semantics, virtual joints
peer-to-peer network of ROS processes that are processing list, and passive joints list. The first step of the SA is the
data together. roscore is the core of the system providing a generation of a self-collision matrix for the robot that can
collection of nodes and programs. roscore contains three main be used in future planning to speed up collision checking.
functional module suits, which are ROS Master, Parameter This collision matrix encodes pairs of links on a robot that
Server and rosout logging node. ROS Master provides naming can safely be discarded from collision checking due to the
and registration to nodes in the ROS system; it allows nodes kinematic infeasibility of there actually being a collision.
to find each other. A node is an executable unit of code During the step-by-step process user can provide information
that implements a specific functionality, similar to a software on different motion planning aspects. Virtual joints attach the
module in traditional usage. A typical system within ROS robot to the world. Planning groups semantically describe
comprises many nodes and is a graph where links represent parts of the manipulator, i.e., what constitutes a gripper or
nodes communication. ROS has a publish-subscribe model of which joints and links comprise an arm. SA allows adding
communications. Nodes communicate with each other using certain fixed poses of the manipulator. We can define query
topics as communication channels. Nodes publish to a topic positions such as the initial and the goal configurations of the
to send messages and subscribe to it for receiving messages. manipulator, and end-effectors can be labelled. In the last step,
There can be multiple publishers and subscribers of a given SA generates several configurations and launch files that can
topic (Figure 4). The basic message types include boolean, be used inside a ROS package.
float, integer or string; users may use custom types as well.
V. P ROPOSED SOLUTION ARCHITECTURE
B. URDF In this section, we present the general architecture for de-
veloping a dynamic motion planning approach using the tools
Unified Robot Description Format (URDF) is an XML
described in the last section. For a simulation of the robotic
format for describing a robot model in ROS. Different ROS
manipulator, we identify three key functional modules in the
programs can interpret the URDF model of physical robot,
architecture, perception, modelling and planning (Figure 5).
e.g., tools such as rviz that allows to visualize it. In the most
basic form, URDF describes the topology of the robot, listing The central component Motion planning framework is
its joints and links, where a joint connects two links. Topology developed as a ROS node and integrates different modules
makes a tree structure, where each link has precisely one into the final motion-planning solution. Perception of the
parent, but it can have multiple child nodes. The geometry world is available through the voxel-based grid (discussed in
of the robot is represented through coordinate frames and section III-A). This grid is formed using data from vision
transforms between these frames. Transforms have a transla- sensor ZED 2K stereo vision camera [8].
tion and rotation components; translation measured in meters The model of the manipulator, a prerquisite for the MoveIt
is specified in coordinate axes x, y and z whereas rotations setup assistant is given as a URDF file; central node looks
in radians along x, y and z-axis are known as roll, pitch for the robot description parameter on the ROS param server
and yaw respectively. For ROS system to know about robot to get the URDF. We used the model available in the UR5
model, a launch script loads URDF file to the parameter server. repositories [26] that we have installed and built. An initial set
The same script publishes joint state values. The kinematics of configurations generated by MoveIt setup assistant defining
libraries within ROS can read joint states, and carry out query poses, self-collision matrix, planning groups, is also
 Planning Perception
Robot description

Planning algorithms
URDF model
OMPL Sensor data

Planning scene
collision detection

Forward & inverse

Motion planning
kinematics

framework
Voxel-based grid

Trajectory execution

legend

ROS package
external modules
(a)
moveit packages

Figure 5. Robot manipulator motion planning - simulation architecture and

module relationship.

available on the param server, that Motion planning framework

can look.
Concerning motion planning, MoveIt has a plugin based
on the OMPL library that primarily implements randomized
motion planners, such as PRM (Probabilistic RoadMaps), RRT
(Rapidly Exploring Random Trees), RRTConnect etc. The
central node offers an interface to the motion planner through
ROS actions or services (a communication paradigm in ROS). (b)
Collision checking in MoveIt happens inside the planning Figure 6. Motion planning of UR5 manipulator.
scene. In particular, we can specify either pose goals or joint-
space goals. Pose goals define a position of the end-effector in
3-d cartesian coordinates, whereas a joint-space goal identifies
a distinct final configuration for the joints (given by individual the voxel-grid is a complementary component of intelligent
joint angles). For both cases, we can plan the movement of motion planning. Its main objective is to provide online
the robotic arm to the desired goal. These tests have been information on the workspace occupancy by different objects
done within graphical simulator Rviz as well as on the UR5 in the scene. Path planning component availabe via MoveIt is
robotic arm. The framework allows us to add collision objects a standalone component. The reported experiment, validates
(obstacles) to the workspace. Collision objects are geometric the later.
primitives such as a box or a cylinder, and can be easily
specified through their key dimensions and 3d position. In VI. C ONCLUSION
this case, the planned trajectory will avoid the obstacle or Robot path planning is a classic problem and is compli-
fail to find a solution when the target configuration cannot cated in human-robot collaborative environments that present
be achieved in the presence of the obstacle. dynamic scenarios for motion planning. In this paper, we
have reviewed some motion planning approaches that address
A. Preliminary test such environments, i.e., where complete information is not
We perform a simple test with regards to chosen tools. In available at planning time. We combine a voxel-based grid
this experiment, we use PRMstar [27] algorithm. We consider with a roadmap-based approach, for its simplicity and ease
a joint-space goal given by the following configuration vector of development. Random sampling might result in roadmaps
{−1.83, −1.732, 1.8, −1.634, −1.57, 2.88} where joint angles with disconnected components, and thus, it fails to find a
are listed in radians. We test the motion of UR5 arm to path when start and goal configurations lie in disconnected
this goal configuration in the absence of a collision object. components. For such cases, an online planner such as RRT
Figure 6 shows the results for this test. For reference, see would be helpful as it incrementally builds the complete path
Figure 6 (a) that considers the case with no obstacle object to the target. The software development for robots requires
and start and goal positions of the robotic arm. Start position a breadth of knowledge and steep learning curve. For this
is the manipulator lying in the x-y plane. In Figure 6 (b) we reason, tools such as ROS and MoveIt with available functional
see the path trail that UR5 follows to reach the goal position. packages can improve development time. We briefly described
For the cases (a) and (b), the algorithm creates 785 roadmap some essential concepts from these tools. And, presented a
states and solution is found in 5.009850s. simulation architecture based on these tools.
It is important to note that monitoring solution that outputs The temporal validity of free spaces is not apparent in the
reviewed approaches. The cell marked as occupied may get [11] T. Lozano-Pérez, “Spatial Planning: A Configuration
free before the execution, but the algorithm has rendered it as Space Approach,” in Autonomous Robot Vehicles.
occupied. It can be marked free only in the following planning Springer-Verlag, 1990, pp. 259–271.
episode. In this case, the planner might find an alternative path [12] J. P. Van Den Berg and M. H. Overmars, “Roadmap-
without the high cost. The contrary is more costly when space based Motion Planning in Dynamic Environments,” IEEE
gets occupied between planning episodes, and the algorithm Transactions on Robotics, vol. 21, no. 5, 2005, pp. 885–
has not yet marked it occupied; leading to a failed query. 897.
Our future work will investigate this scenario. To efficiently [13] L. Jaillet and T. Siméon, “A PRM-based Motion Plan-
handle dynamic scenarios, we can input fresh voxel grids to the ner for Dynamically Changing Environments,” in 2004
planning program, with a certain frequency. The refresh rate IEEE/RSJ International Conference on Intelligent Robots
of the voxel grid depends on the response time of previous and Systems (IROS)(IEEE Cat. No. 04CH37566), vol. 2.
planning query. Currently, we are looking into solutions to IEEE, 2004, pp. 1606–1611.
address these issues. [14] J. J. Kuffner Jr and S. M. LaValle, “RRT-Connect: An
Efficient Approach to Single-Query Path Planning,” in
ACKNOWLEDGMENTS Proceedings of the 17th IEEE International Conference on
INDTECH 4.0 – New technologies for intelligent manu- Robotics and Automation (ICRA 2000), vol. 2. IEEE,
facturing. Support on behalf of IS for Technological Research April 2000, pp. 995–1001.
and Development (SI à Investigação e Desenvolvimento Tec- [15] A. Stentz, “Optimal and Efficient Path Planning for
nológico). POCI-01-0247-FEDER-026653 Partially-Known Environments,” in Intelligent Unmanned
Ground Vehicles. Springer, 1997, pp. 203–220.
R EFERENCES [16] P. E. Hart, N. J. Nilsson, and B. Raphael, “A Formal Basis
[1] R. I. Association, “How Robots Are Taking for the Heuristic Determination of Minimum Cost Paths,”
on the Dirty, Dangerous, and Dull Job,” IEEE Transactions on Systems Science and Cybernetics,
https://www.robotics.org/blog-article.cfm/How-Robots- vol. 4, no. 2, July 1968, pp. 100–107.
Are-Taking-on-the-Dirty-Dangerous-and-Dull-Jobs/209, [17] S. Koenig and M. Likhachev, “Fast Replanning for
accessed: 2020-05-19. Navigation in Unknown Terrain,” IEEE Transactions on
[2] P. Anderson-Sprecher, “Intelligent Monitoring of As- Robotics, vol. 21, no. 3, 2005, pp. 354–363.
sembly Operations,” Robotics Institute, Carnegie Mellon [18] A. Stentz et al., “The Focussed Dˆ* Algorithm for Real-
University, Tech. Rep. CMU-RI-TR-12-03, June 2011. Time Replanning,” in IJCAI, vol. 95, 1995, pp. 1652–
[3] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. 1659.
Overmars, “Probabilistic Roadmaps for Path Planning in [19] Z. Iqbal, J. Reis, and G. Gonçalves, “Path planning for
High-Dimensional Configuration Spaces,” IEEE Transac- an industrial robotic arm,” in The Eighth International
tions on Robotics and Automation, vol. 12, no. 4, August Conference on Intelligent Systems and Applications (IN-
1996, pp. 566–580. TELLI 2019). IARIA, 2019, pp. 30–36.
[4] S. M. LaValle, “Rapidly-Exploring Random Trees: A [20] M. Kallman and M. Mataric, “Motion planning using
New Tool for Path Planning,” Department of Computer dynamic roadmaps,” in IEEE International Conference on
Science, Iowa State University, Tech. Rep. TR 98-11, Robotics and Automation, 2004. Proceedings. ICRA’04.
October 1998. 2004, vol. 5. IEEE, 2004, pp. 4399–4404.
[5] D. Katz, J. Kenney, and O. Brock, “How Can Robots [21] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote,
Succeed in Unstructured Environments,” in Workshop J. Leibs, R. Wheeler, and A. Y. Ng, “Ros: an open-
on Robot Manipulation: Intelligence in Human Environ- source robot operating system,” in ICRA workshop on
ments at 4th Robotics: Science and Systems Conference open source software, vol. 3, no. 3.2. Kobe, Japan,
(RSS 2008). Citeseer, June 2008. 2009, p. 5.
[6] S. Koenig, M. Likhachev, and D. Furcy, “Lifelong Plan- [22] S. Chitta, I. Sucan, and S. Cousins, “Moveit![ros topics],”
ning A*,” Artificial Intelligence, vol. 155, no. 1-2, 2004, IEEE Robotics & Automation Magazine, vol. 19, no. 1,
pp. 93–146. 2012, pp. 18–19.
[7] D. Thornton Coleman IV, “Methods for Improving Mo- [23] S. Chitta and I. Sucan, “Moveit,” https://moveit.ros.org/,
tion Planning Using Experience,” Ph.D. dissertation, Uni- 2012, accessed: 2019-10-31.
versity of Colorado, 2017. [24] I. A. Sucan, M. Moll, and L. E. Kavraki, “The open
[8] L. Antão, J. Reis, and G. Gonçalves, “Voxel-based motion planning library,” IEEE Robotics & Automation
Space Monitoring in Human-Robot Collaboration En- Magazine, vol. 19, no. 4, 2012, pp. 72–82.
vironments,” in 2019 24th IEEE International Confer- [25] J. Pan, S. Chitta, and D. Manocha, “Fcl: A general
ence on Emerging Technologies and Factory Automation purpose library for collision and proximity queries,” in
(ETFA). IEEE, 2019, pp. 552–559. 2012 IEEE International Conference on Robotics and
[9] R. Nogueira, J. Reis, R. Pinto, and G. Gonçalves, “Self- Automation. IEEE, 2012, pp. 3859–3866.
adaptive cobots in cyber-physical production systems,” in [26] I. GitHub, “Universal Robot,” https://github.com/ros-
2019 24th IEEE International Conference on Emerging industrial/universal robot, 2019.
Technologies and Factory Automation (ETFA). IEEE, [27] S. Karaman and E. Frazzoli, “Sampling-based algorithms
2019, pp. 521–528. for optimal motion planning,” The international journal
[10] U. Robots, “UR5 collaborative robotic arm,” https:// of robotics research, vol. 30, no. 7, 2011, pp. 846–894.
www.universal-robots.com/products/ur5-robot/, 2015, ac-
cessed: 2019-12-20.