Project topics and materials
Get Complete Project Material File(s) Now! »
Background
This chapter introduces the robot model and the calculation of DOF of the system and a short introduction to unit quaternions. Section 2.4 introduces the underlying concept of motion planning, kinematic configuration and sampling theory. Furthermore, the concepts of Robot Operating System (ROS), MoveIt and The Open Motion Planning Library (OMPL) are explained in respective subsections 2.5.1, 2.5.2 and 2.5.3. Finally, section 2.6 presents the underlying theory of all the motion planners investigated in this project.
Related work
Motion planning is one one the most discussed topic in robotics. There are various methods for motion planning. One recent example of novel motion planners is the neural network-based planning algorithm. This network is known as MPNet [1]. MPNet comprises two neural networks, an encoder network (Enet) and a planning network (Pnet). Enet takes the environment information such as a point cloud converted to a voxel. Pnet takes the encoding of the environment, the robot’s current state, and goal state to output samples for a path. This method utilizes a two algorithm known as Gradient episodic memory for continuum learning to solve the problem of catastrophic forgetting and solve various problems without forgetting previous tasks [2].
Another method for planning the motion for complex systems is sampling-based motion planning which has become more widespread recently. All sample-based motion planning, sample the environment either by point checking or generating segments and finds a path between the start and goal state irrespective of obstacles geometry if a feasible path exists. However, these methods have their weaknesses. They fail to find the absolute shortest path solution in a finite time, and they become computationally inefficient as the dimensionality of the planning problem increases. There are a few exceptions that can find the optimal path, such as RRT* [3] and PRM* [4]. These methods are optimizing planners and provide optimality guarantees with the cost of having a slower plan time.
Another family of motion planning algorithms are multi-query planners. Probabilistic roadmap is the most utilized multi-query planner, presented in [5]. This method takes a new approach to explore the environment. This algorithm builds and retains a roadmap in the workspace that can be reused for later planning attempts. The main advantage of using PRM is that it keeps a structure of the generated graph to reuses it for every query, and the motion planner does not require to create a graph in each run.
On the other hand, single query motion planners typically build one or two trees of connected nodes by exploring the environment. In [6], a single-query planner is presented. This method is known as Kinematic Planning by Interior-Exterior Cell Exploration and suitable for operations with complex dynamics. In this method, the workspace discretizes into a grid used to estimate the state space coverage. The exploration strategy is biased towards the unexplored areas of the state space. So, this method keeps track of the explored region with a specific boundary such that the majority of the exploration takes place outside of this boundary using state space coverage. KPIECE does not require any state sampler and is a desirable motion planner for problems characterized by its physical models.
Finally, control-based planning methods are a suitable option if system is subject to differential constraints. Control-based motion algorithms derive differential equations from the system’s motion using various methods and imply control theory concepts such as feedback and feed-forward to follow the trajectory. In [7], A control-based method called Expansive Space Trees is presented. This method is a tree-structured motion planner that explores the state space biased towards the less explored areas. EST is a bidirectional motion planner similar to RRT-Connect. Hence, it grows two trees, rooted in a start and a goal state. The EST’s main advantage is the ability to handle difficulties caused by the presence of narrow passages that prevent many planners from reaching to the goal state, especially if the start and goal states can only be connected to each other via a narrow passage.
Robot model
UR5 is a relatively adaptable, lightweight, and collaborative robot produced by Universal Robots and is a member of the CB3 family. UR5 is a medium-sized industrial robot ideal for automating different duties with low-weight and medium-duty applications with high accuracy and flexibility. This robot is easy to set up and grants a perfect ratio between the generated power and its weight. UR5 has six rotational joints with the specification presented in Table 2.1.
Degree of freedom of the UR5
The DOF for a robot arm is the dimension of the configuration space (c-space), which in turn is the minimum real number needed to represent the c-space of that robot. According to Grubler’s formula, degrees of freedom for a mechanism are derived by subtracting a joint’s freedom from the number of independent constraints. [8].
DOF =( Freedoms of bodies ) – Number of independent constraints (2.1)
For a robot arm with J number of joints this formula become :
DOF = m(N − 1) − ci (2.2) i=1
Where m is the number of freedoms for a single rigid body, for spatial bodies, m equals six with three rotational and three transitional in a 3D space. For planer bodies, m equals three with one rotational and two transitional in 2D space. In our case, the planning takes place in a 3D space that sets m equal to six. N is the number of bodies inclusive of the ground. UR5 has six revolute joints, and adding ground to this number sets N equals seven. Finally, c is the constraint between two rigid bodies. For a revolute joint, c is equal to 5. Inserting these values in Equation 2.3 gives the DOF of the UR5 robot arm.
DOF = 6(7 − 1) − 6(5) = 6 (2.3)
Orientation
There are several options to describe spatial rotations in 3D space. The two most common ways of representing rotation are unit quaternions and Euler angles. This section will describe both representations briefly and how to convert Euler angles to unit quaternions [9].
Unit quaternions
There are several ways of describing spatial rotations in a three-dimensional space. One of the most common ways of representing rotation is the unit quaternions, also known as versors. The main idea of quaternions is to add one extra dimension to multiply triples. A quaternion q is a summation of a scalar and a three-dimensional vector. For a scalar q0 and a vector q = [q1, q2, q3], the quaternion q is defined as :
q = q0 + q = q0 + q1i + q2j + q3k. (2.4)
q consist of a scalar part q0 and a imaginary vector q = q0 + q1i + q2j + q3k.
The imaginary unit vectors i, j and k have the following relationship [10]: i2 + j2 + k2 = ijk = −1 (2.5)
Unit quaternion is a quaternion with a norm equal to 1. The norm of a quaternion is a scalar property and derives from |q| = √qq∗. In this equation, q∗ star is the complex conjugate of q. The conjugate of q derives by reversing the sign of the imaginary part of vector q . Finally, the inverse of a quaternion q is defined as : q − 1 = q∗ (2.6) ∥q∥2
With the inverse quaternion defined, it is possible to verify if q is a unit quaternion or not. By definition, q is a unit quaternion if q−1q = qq−1 = 1 which is equivalent with, the inverse of q be same as its complex conjugate. As mentioned earlier, a unit quaternion q can be written as the sum of an imaginary part and a real part, namely, q = Img(q) + Real(q) = q0 + q while ∥q∥ = 1 [10]. Absolute value equals to one, implies that q02 + |q|2 = 1. Comparing this equation with the « Pythagorean » identities, we can conclude that sin(ϕ)2 = |q|2 and cos(ϕ)2 = q02. In fact, cos(ϕ) = q0 and sin(ϕ) = ∥q∥ for a specific angle ϕ ∈ [0, 2π ] and the unit quaternion q, can be formulates in terms of this angle and a unit vector u as following: q = cos(ϕ) + u sin(ϕ), u = q (2.7) ∥q∥
Representation of rotations and massage transformation
ROS uses quaternions to represent rotations in R3. In ROS a quaternion q is usually described as (x, y, z, w) while (x, y, z) is the real part of q and w is the scalar part donates by q0.
It is challenging to imagine rotations in terms of quaternions as it is difficult to describe elements of quaternion with physical phenomena. For this reason, rotations have been implemented in terms of Eulers angels and the result converted to a quaternion afterward. A tf2::quaternion transformation object is created to perform this transformation which receives the rotations in terms of roll, pitch and yaw and returns the rotation in terms of unit quaternions. (See Listing G.9 for representation of thef2::Quaternion object).
The alternative way of injecting rotations is to pass the Euler angles directly to the kinematic solver. Kinematics and Dynamics Library (KDL) (see Appendix C) kinematic solver can receive and return Euler angles directly. By invoking the constructor of the KDL::Rotation class and calling the Rotation::RPY(double roll, double pitch, double yaw) function, angles in Eulers format can be injected into the kinematic solver.
Motion planning
The objective of a motion planning process is to find a solution to the problem of moving from an initial state to a goal state in the state space while satisfying the constraints forced by either the robot limitations or the obstacles in the environment. A famous example of motion planning is the classical piano mover’s problem. This problem is described as finding a path for a rigid body, piano, which starts from one initial state and ends at a goal state. Figure 2.1 [11] presents a scenario of moving a piano in the environment. In this problem, all the obstacle’s locations are assumed to be known, and the main challenge is to find a collision-free path connecting the start and end states. A rigid body such as a piano in 3D space has six degrees of freedom, including 3 DOF for movement in the x, y and z planes, and three more named rolls, pitch, and yaw to represent rotation along the axes of these planes. In this setup with a high degree of freedom, finding an exact solution is challenging. To find a collision-free path, the motion planner needs to compute continuous changes for the set of (x, y, z, roll, pitch, yaw). From a mathematical point of view, motion planning problem is defined as finding a set of control inputs u : [0, T ] → u for a given initial state Xstart and a desired final state Xfinal in a specific amount of time T such that the motion of the robot along the trajectory satisfies x(T ) = xgoal and q(t) in Cfree for all t ∈ [0, T ]. For example, motion of the robot x˙ with configuration q ∈ C ⊆ Rn and C = Cfree ∪ Cobs and acceleration as the a control inputs where (q, v) ∈ X, can be written as Equation 2.8.
x˙ = f(x, u), u ∈ U (2.8)
Where u is an element of the feasible control set U.
x(T ) = x(0) + f(x(t), u(t)) dt
Single-query and multi-query motion planner
A Motion planner can be either a multi-query or single-query. A multi-query planner invests time in generating a good representation of the environment so that future motion planning problems in the same space can be solved quickly. On the other hand, a single query planner attempts to solve a single motion planning problem as quickly as possible by finding a connected path between the states in each run separately from the other planning attempts.
Completeness
A motion planner is complete if it always finds a valid solution for a motion planning problem if one exists. On the other hand, a motion planner has resolution completeness if the planner finds a solution whenever a solution exists. Finally, a motion planner is probabilistically complete if the probability of finding a solution, if one exists, is 100 percent if the planning time tends to infinity [12].
Sampling Theory
The state space for motion planning in real-world scenarios is either a two-dimensional area or three-dimensional space med infinite amounts of states, which significantly limits the exploring and planning processes. Hence the main idea of a sampling-based algorithm is to address this limitation. A sampling-based planning algorithm can consider a countable number of samples. However, if the algorithm runs forever, the number of states becomes countably infinite. In practice, a motion planning process utilized with sample-based algorithms is expected to terminate after a finite number of iterations (samples).
Kinematic configuration
Inverse Kinematics (IK) is defined as finding a solution to the problem of determining a value set for the joints(orientation and position) as the end effector is in specific coordination in the cartesian space. In common motion planning problems, trajectories are defined in cartesian space and not in configuration space. Hence, the IK methods can be utilized to perform this transformation and provide a set of joint configurations given the pose of the end effector.
The kinematic equations are used to set up closed-loop equations with non-linear constraints on the configuration variables of the robot’s joints. The number of constraints on the configuration variables of a closed-loop system equation is also known as the DOF of that system. For many kinematic equations, there is no guarantee for an IK problem to have an analytical solution. For instance, if the DOF of a robot exceeds the DOF of the end-effector, infinite amount of solutions for the IK problem exists. Hence this problem lacks an analytical closed-loop solution.
The closed-loop solution has a significantly high computational complexity. Therefore the computation times would be substantially higher than the numerical techniques. The numerical solutions are not as accurate as the analytical method. However, the accuracy of numerical techniques is often sufficient for most motion planning problems.
Derive IK equation
Consider a kinematic system with n number of joints where each cartesian coordinate is a function of the joint angles. Let X = [x, y, z, ϕ1, ϕ2, ϕ3] be the cartesian coordinates (pose) and configuration Θ = [θ1, θ2, θ3, θ4, θ5, θ6] be the joint angles. Then the forward kinematic Kf is defined as: X = Kf (θ) (2.10)
The differential kinematics are obtained by taking the partial derivatives of each Cartesian coordinate with respect to each joint angle which is shown in Equation 2.11. X = J(θ)θ (2.11)
The Jacobian J(θ) is the Jacobian of the Kf describing the transformation between velocities in configuration or cartesian spaces where each element jmn is derived from ∂K(θm) . Thus, each column in J can be seen as the motion in X (∆Xi) caused by motion in the joint θi . The number of rows of the Jacobian matrix equals the DOF of the robot, and the number of columns is equal to the number of joints of the manipulator.
Configuration θ is the solution of Equation 2.11. One method to solve this equation system is to define a linear least squares optimization problem that minimizes ∥X˙ − J(θ)θ˙∥. The optimal fit can be determined through an n-dimensional hyperspace and solved by a gradient descent method. A famous example of a gradient descent method is the Newton-Raphson (NR) method [13] which offers one such form of minimization. Multiplying of both side of Equation 2.11 to the inverse of Jacobian, namely J(θ)−1 returns the representation of joint velocities in terms of cartesian velocities and inverse Jacobian which is shown in Equation 2.12. θ˙ = J(θ)−1X˙ (2.12)
The inverse of the Jacobian matrix is trivial to calculate if this matrix is invertible. Inverse Jacobian, J(θ) can be derived only for a square matrix. The Jacobian of a robotic system is a square matrix if and only if the number of joints is equal to the DOF of the robot. If J is not invertible, pseudo-inverse can be used instead. Different pseudo-inverse methods can be utilized to calculate the inverse of the Jacobian for redundant arms approximately. Another option is to use Singular Value Decomposition (SVD) method to derive the pseudo-inverse of the Jacobian[14].
Motion planning tools
The Robot Operating System
ROS 1is a flexible and open-ended collaboration framework to develop robotic software. ROS contains a set of tools, libraries and infrastructure that aims to ease the task of developing complex and robust automated practices across a broad spectrum of robotic applications. The ROS community consists of many worldwide users who work on projects of varying scales and complexity for research and industrial purposes (see Appendix A for more information about the ROS and the communication tools ROS provides).
MoveIt!
MoveIt! 2 is one the most comprehensive open-source software for robotic manipulation. MoveIt is compatible with a large variant of robots and has been considered the state-of-the-art tool to solve various robotic problems such as manipulation, navigation, control, etc. Moreover, MoveIt provides a user-friendly ecosystem for a developer to perform:
1. Implementation of the complex and advanced robotics applications.
2. Simulation and visualization of different environment instances such as obstacle, point of interest and the robot.
3. Evaluation of motion planning results.
MoveIt provides all the necessary functionality needed for robotic manipulation. The property that makes MoveIt a valuable and unique tool is a vast library of robotic capabilities for manipulation, motion planning and control available for MoveIt users.
MoveIt! Structure
MoveIt has been designed as a plugin-based structure. Different packages and services are connected to a central node called move group. Move group is a collection of classes, structs, unions and interfaces that provide essential functionality for motion planning, moving the robot, adding objects and obstacles into the environment. MoveIt is specially adapted for the ROS environment to enable users and developers to improve and extend MoveIt capabilities [15].
MoveIt! APIs
MoveIt supports various Interfaces, enabling users to access the functionalities and services provided by the Move group. The three primary Interfaces are Move_group_interface, MoveIt_commander and Rviz GUI. Move_group_ interface (see Appendix D) and Rviz GUI (see Appendix E) are the two interfaces used in this project.
The Open Motion Planning Library
OMPL is a collection of state-of-the-art sampling-based motion planning algorithms, including implementations of a large variety of sample-based planning algorithms. OMPL represent all the essential concept of motion planning such as state space, workspace, goal and start state with an abstract representation. Hence it is a suitable library to solve and evaluate both simulation and physical robotic problems.
Note that OMPL does not include the geometry representation of the workspace or the robot. Consequently, the user must define the kinematic configuration of the robot and pick a suitable collision detection method. Different tools and formats exist to represent the robot kinematics and collision boundaries, such as Unified Robot Description Format (URDF) files. The user must select the collision checking module since OMPL does not have any default collision checker. However, it is compatible with several collision checking methods such as Flexible Collision Library (FCL) (see Appendix B) [16].
OMPL consists of several modules to execute the motion plannings process. These modules have specific tasks such as the sampler module which is responsible for computing valid configurations, the state validity checker module evaluates the robot configuration’s validity, and the local planner module finds a local collision-free path between adjacent samples (See Appendix F for more information about the modular structure of OMPL).
Table of contents :
Introduction
1.1 Background
1.2 Problem
1.3 Research Question
1.4 Goals
1.5 Limitation
1.6 Ethic and sustainability
2 Background
2.1 Related work
2.2 Robot model
2.2.1 Degree of freedom of the UR5
2.3 Orientation
2.3.1 Unit quaternions
2.3.2 Representation of rotations and massage transformation
2.4 Motion planning
2.4.1 Sampling Theory
2.4.2 Kinematic configuration
2.5 Motion planning tools
2.5.1 The Robot Operating System
2.5.2 MoveIt!
2.5.3 The Open Motion Planning Library
2.6 Motion planners
2.6.1 Rapidly-exploring Random Trees
2.6.2 Expansive Space Trees
2.6.3 The Kinodynamic Motion Planning by Interior-Exterior Cell Exploration
2.6.4 Path-Directed Subdivision Trees
3 Methods
3.1 Motion planning and visualization
3.1.1 Description of ROS packages and their design purposes
3.1.2 Description of nodes and their underlying structure
3.2 Defining kinematic, visual and collision boundary of the UR5
3.2.1 Kinematics chain generation
3.2.2 Define Object’s geometry, visual and collision boundary
3.3 Algorithm benchmarking
3.3.1 Primary benchmarking
3.3.2 Second experiment
3.4 Data Collection
4 Results
4.1 Result of primary experiment
4.2 Result of second experiment
4.2.1 Planning time
4.2.2 Path length
4.2.3 Solution smoothness
4.2.4 Memory usage
4.2.5 Number of solved cases
4.2.6 Summary of the motion planners performance
5 Discussion
5.1 Implications of the results
5.2 Another conceivable method
5.3 Assessing reliability of the data
6 Conclusions and Future work
6.1 Summary
6.2 Conclusions
6.3 Future work
6.4 Reflections
References
A Essential concepts of ROS
B Flexible Collision Library
C KDL Inverse Kinematic solver
