![](https://private-user-images.githubusercontent.com/69509720/336621250-5d0b34e0-8cbd-4d3d-9884-382a565008ef.jpg?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjQ3Mjg4NDQsIm5iZiI6MTcyNDcyODU0NCwicGF0aCI6Ii82OTUwOTcyMC8zMzY2MjEyNTAtNWQwYjM0ZTAtOGNiZC00ZDNkLTk4ODQtMzgyYTU2NTAwOGVmLmpwZz9YLUFtei1BbGdvcml0aG09QVdTNC1ITUFDLVNIQTI1NiZYLUFtei1DcmVkZW50aWFsPUFLSUFWQ09EWUxTQTUzUFFLNFpBJTJGMjAyNDA4MjclMkZ1cy1lYXN0LTElMkZzMyUyRmF3czRfcmVxdWVzdCZYLUFtei1EYXRlPTIwMjQwODI3VDAzMTU0NFomWC1BbXotRXhwaXJlcz0zMDAmWC1BbXotU2lnbmF0dXJlPWVjODMxNjQ1MjlkNjhkMDc2NDRhZGViOGIzYjgwNGM1YTFiODhlNGEwNzI0NTJjOGZjMTJlM2U1Nzc3MDg3ODImWC1BbXotU2lnbmVkSGVhZGVycz1ob3N0JmFjdG9yX2lkPTAma2V5X2lkPTAmcmVwb19pZD0wIn0.EkpFflAXaRUoc4ZZ10xjZqM4p4zeRBicilyvTvEPKiQ)
Overview:
- Introduction
- Delta Robot Kinematics
- Theoretical Study - Point-to-Point Trajectory Generation
- Theoretical Study - Multi-Point Trajectory Generation
- Experimental Study - Adept Cycle Trajectory Generation
- References
- Ending Note
What is a Delta Robot and Why is it Important
People who search for this repository already know what the delta robot is and know why it's important. Delta robot parallel kinematic structure and high-speed capabilities make them ideal for precise and speedy tasks, particularly in pick-and-place operations. This repository studies trajectory planning methods for Delta robots, focusing on smooth motion for the end-effector while minimizing deviations.
The applications of Delta robots are virtually endless but mainly they come down to a certain pattern of pick-and-place or 3D printing or tasks that require somewhat similar movements to these two. Here's three applications that I worked on (PLEASE CITE MY 4 PAPERS):
Pros and Cons of Delta Robot
Delta robot has basically two advantages at the cost of two things (I'm talking about the important ones).
Advantages:
- High Speed
- High Precision
Disadvantages:
- Small Workspace
- Small Workload
Trajectory Planning
Take a look at the figure below. There are four main stages to any sort of robotic operation:
- Task Planning: Figuring out what the robot is going to be doing [in order]
- Path Planning: What points in 3D space the robot is going through [in order]
- Trajectory Planning: Position of the robot as a function of time
- Control: Giving the trajectory as a reference to the robot's controller
What are Forward and Inverse Kinematics
Look at the figure below. Let's say our robot has actuated joints of
- Forward Kinematics: Given the actuated joint parameters to calculate the position of end-effector
- Inverse Kinematics: Given the position of the end-effector to calculate the actuated joint parameters
What is the Jacobian of a Robot
This basically has the same logic as the FK and IK but this time instead of converting between positions and angles, the conversion occurs between velocity of the end-effector and the velocity of the joint parameters.Given Data & Assumptions
The Delta robot consists of three main chains. Each chain starts from the base platform(
The angles between
We have the following assumptions:
and also:
Calculating Relative Positions
- The position of
$A_i$ in relation to the${O_0}$ -frame:
- The position of
$B_i$ in relation to the${O_0}$ -frame:
- The position of
$C_i$ in relation to the${O_0}$ -frame:
- The position of
$C_i$ in relation to the${O_0}$ -frame, given that the position of end-effector center is equal to$[X_P \quad Y_P \quad Z_P]^T$ :
- Given the mentioned equations we can say that we have the following constraint:
- Re-writing the previous equation we have:
Solution of Foward and Inverse Kinematics
-
For FK we numerically solve the above constraint equation for
$(X_P, Y_P, Z_P)$ given$\theta_i$ -
For IK we assume a variable change of
$t_i = \tan(\theta_i/2)$ , which gives us$\sin(\theta_i) = \frac{2t}{t^2+1}$ and$\cos(\theta_i) = \frac{1 - t^2}{t^2+1}$ . Applying this, we solve the constraint equation for$t_i$ , which in turn, gives us$\theta_i$ .
If you need a plug and place code that JUST WORKS i suggest the following code: LINK - Reference #2
The same code as in the reference is implemented in the file DeltaKinematics.py
(LINK)
Basically point-to-point trajectory planning is like interpolation between two values (let's call them
Note
The output array does not include any time information, the time information comes into play when we want to give the PID controller the next target point from the array. So basically when the robot wants to move from Point A to B in either 2 seconds or 3 seconds, the interpolation array for both of these actions are the same.
Note
Since the array does not include time information, the duration for the whole process is considered to be 1, hence it simplifies a lot of the calculations. We call this "normalized time". If you need the time information included you should refer to the main references.
We need to define the main phases of movement at the start of each method, so for this the important time instances are:
So the trajectory is also called the parabolic trajectory and is actually made of two 2nd order polynomials glued together, in mathmetical form that will look like:
What we can do is to set
Then we write down the conditions for solving
- At the start of movement in this phase,
$\theta_a(t)$ is equal to$\theta^I$ - At the end of the movement in this phase,
$\theta_a$ is exactly half-way between$\theta^I$ and$\theta^F$ - At the start of movement in this phase, velocity (1st differential) of
$\theta_a(t)$ is equal to zero which in mathematical form is written as:
Solving the system of linear equations we get:
Then we write down the conditions for solving
- At the start of movement in this phase
$\theta_b(t)$ is exactly half-way between$\theta^I$ and$\theta^F$ - At the end of the movement in this phase,
$\theta_b(t)$ is equal to$\theta^F$ - At the end of movement velocity (1st differential) of
$\theta_b(t)$ is equal to zero which in mathematical form is written as:
Solving the system of linear equations we get:
Finally the overall mathematicall function can be described as:
As explained, the goal here is to basically use a trapezoidal diagram as a way to interpolate between two given motor rotations. The trapezoidal diagram is defined as the following:
For the sake of simplicity we say that
So we can calculate all of the
Next up is
And the next is
where
$T = \frac{1}{3}$ $v_{\text{max}} = \frac{\theta_f - \theta_i}{1 - T}$ $a = 3 v_{\text{max}}$
The S-curve method is somewhat similar to the trapezoidal method with the difference that it is smoother. So in mathemtical terms that would be:
We'll do the same thing as the trapezoidal method and calculate the entire movement based on
Next is
Next is
Finally it's
Where:
$T = \frac{1}{7}$ $v_{\text{max}} = \frac{\theta_f - \theta_i}{4T}$ $a_{\text{max}} = \frac{v_{\text{max}}}{2T}$ $j_{\text{max}} = \frac{a_{\text{max}}}{T}$
When interpolating between
Now knowing that, we can talk about
The
we can write the conditions to solve for
- starting and finishing position of
$s$ which are 0 and 1 - starting and finishing velocity of
$s$ which are both 0 - starting and finishing acceleration of
$s$ which are both 0 These conditions can be written in mathmetical format:
Solving for the resulting system of linear equations we get:
and in the end we have:
which means:
We go through the same process of as before in the 5th order polynomial. First it is to write down
then writing down the conditions:
Then solving the linear system of equations based on the conditions:
and at the end we can write:
which means for
Again the same shit is both 5th and 7th order polynomial. First
then writing down the conditions:
Then solving the system of linear equations:
So we have for
and we have for
POINT TO POINT TRAJECTORY GENERATION METHODS
Parabolic Method | Trapezoidal Method | S-Curve Method |
5th-order polynomial | 7th-order polynomial | 9th-order polynomial |
Multi-point trajectory generation generates a trajectory between multiple target points given as a path, along with its time information. For example if the robot needs to be moved from point A, passes point B and then stop at point C (same logic with more points, basically we have a path which contains more than the starting and final points).
So like the point-to-point methods the inputs are the path points which want to hit, and then the output is the array of values (sampled from the position as a function of time) that we'll give to the PID Controller as a reference.
How can we use a point-to-point method for multiple points? It’s straightforward. Let's revisit the previous example, where we want to create a trajectory for the values [0, 0.2, 1]. First, we can generate a trajectory between [0, 0.2], and then create another trajectory between [0.2, 1]. This approach works for any number of points and can be applied using any of the six point-to-point methods mentioned. However, this method might not be suitable for certain tasks. For instance, in applications where smooth, continuous motion is required, we may not want to stop at each path point. In such cases, using these point-to-point methods may not be the best idea. Here's an example of where this method might be applied:
![](https://private-user-images.githubusercontent.com/69509720/356884681-02cdf85e-d61f-4f3b-afe9-3974e3a63551.png?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjQ3Mjg4NDQsIm5iZiI6MTcyNDcyODU0NCwicGF0aCI6Ii82OTUwOTcyMC8zNTY4ODQ2ODEtMDJjZGY4NWUtZDYxZi00ZjNiLWFmZTktMzk3NGUzYTYzNTUxLnBuZz9YLUFtei1BbGdvcml0aG09QVdTNC1ITUFDLVNIQTI1NiZYLUFtei1DcmVkZW50aWFsPUFLSUFWQ09EWUxTQTUzUFFLNFpBJTJGMjAyNDA4MjclMkZ1cy1lYXN0LTElMkZzMyUyRmF3czRfcmVxdWVzdCZYLUFtei1EYXRlPTIwMjQwODI3VDAzMTU0NFomWC1BbXotRXhwaXJlcz0zMDAmWC1BbXotU2lnbmF0dXJlPWJkMGNhZGZmNzlkNjc0NTkzYmFkNDk5YmU4Zjg0ODFjMTJhNWFlMDBhN2Q2MzcwODM1Y2ZiNTgzM2NkNGU0ZTgmWC1BbXotU2lnbmVkSGVhZGVycz1ob3N0JmFjdG9yX2lkPTAma2V5X2lkPTAmcmVwb19pZD0wIn0.7OK-wgKSEQBW6NcQiuTOrKkOi4_kfcciiA7Sp8dge90)
In this application of petri dish sampling in microorganism culturing (which my team implemented), the robot is required to stop at each path point because a liquid drop needs to be placed at each location.
And the overall idea is to interpolate between each two points in the path, with a point-to-point method. If we interpolate between four values of [0, 1, -1, 0], the following diagrams are what we're getting:
POINT TO POINT TRAJECTORY GENERATION METHODS USED FOR MULTIPLE POINTS
Parabolic Method | Trapezoidal Method | S-curve Method |
5th order polynomial | 7th order polynomial | 9th order polynomial |
Say we are interpolating a set of values such as [0, 1, -1, 0] and we wanted to create a trajectory for these values with a single polynomial:
So given this information we list the conditions for creating the polynomial function:
- The three positions of [0, 1, -1, 0]: Conditions x4
- Initial and final velocity values [0, 0]: Conditions x2
- Initial and final acceleration values [0, 0]: Conditions x2
- Initial and final jerk values [0, 0]: Conditions x2
- Initial and final snap values [0, 0]: Conditions x2
Based on if we want to control the value of jerk and snap, this'll give us three choices:
- 7th order polynomial (doesn't control jerk or snap)
- 9th order polynomial (controls jerk but doesn't control snap)
- 11th order polynomial (controls both jerk and snap)
Defining the polynomial:
The polynomial solution for
Defining the polynomial:
The polynomial solution for
Defining the polynomial:
The polynomial solution for
HIGHER ORDER POLYNOMIAL METHODS USED FOR MULTIPLE POINTS INTERPOLATION
7th-order polynomial | 9th-order polynomial | 11th-order polynomial |
This method basically uses multiple polynomials on end for generating a trajectory between multiple path points. So if we are given
Each polynomial will have a degree of
So the input is:
The overall trajectory function can be described as:
So as I said there are
- Adhering to the path values (at the starting and finishing point of each polynomial) x(2n) Conditions
- The velocity should be continuous at the transition points between each two successive polynomials (such as polynomial number
$k$ and$k+1$ ) x(n-1) Conditions - The acceleration should be continuous at the transition points between each two successive polynomials (such as polynomial number
$k$ and$k+1$ ) x(n-1) Conditions - Initial and final velocity of the movement x2 Conditions
Adding these conditions up will result in 4n conditions, hence 4n equations. Solving this system of linear equations will give us the result. Let's write the conditions one more time:
The coefficient
Where
But this is for when the velocities of the points are known, which they are not (except the initial and final points). So the velocities have to be calculated, in this instance we use the continuity conditions of acceleration. Velocities can be found with a matrix of
For better understanding please refer to [3] in the reference section.
This one is a bit tough to explain so bear with me for a second. When you have a certain pattern the previous trajectory planning methods won't work. Why is that? Because in the previous methods we were given a few number of points and we wanted to generate a trajectory in-between those points, but with a 'pattern' we already have the trajectory that the robot needs to go.
So in this instance what we should do is to sample that pattern which results in an array of values much like before, which we will give to the PID controller of the robot as a reference to follow. How do we sample the pattern? well what I do is this: I first sample at a constant rate (based on distance) then I'll repeat the values which correspond to the sharp edges of the pattern. This way I can emphasis on the sharp ends for the PID controller so it comes out sharper (because the PID controller will have more time to reach it's destination with more precision). That's it.
Well what is adept cycle? it's basically just four points in 3D space that the robot must hit in order to perform a pick-and-place operation. it looks something like this:
ADEPT CYCLE VIA THE POINT-TO-POINT METHODS
Point-to-Point Parabolic | Point-to-Point Trapezoidal | Point-to-Point S-Curve |
Point-to-Point 5th-Order Polynomial | Point-to-Point 7th-Order Polynomial | Point-to-Point 9th-Order Polynomial |
ADEPT CYCLE VIA THE HIGHER-ORDER POLYNOMIAL METHODS
Multi-Point 7th-Order Polynomial | Multi-Point 9th-Order Polynomial | Multi-Point 11th-Order Polynomial |
ADEPT CYCLE VIA THE CUBIC-SPLINE METHODS
Adept Cycle Via Cubic Spline |
In this section I'll explain each of the code files and how to use them. Firstly there is a SimpleMath.py
file which you don't have to worry about too much since it only introduces functions of sind
, cosd
, and tand
which are based on their corresponding numpy functions with the difference of getting inputs in degrees rather than radians.
This file calculates the delta robot inverse and forward kinematics
UPPER_LINK = 0.2
LOWER_LINK = 0.46
BASE_RADIUS = 0.1
EE_RADIUS = 0.074
P1 = [0, -0.15, -0.42]
delta_robot = DeltaKinematics(UPPER_LINK, LOWER_LINK, BASE_RADIUS, EE_RADIUS)
inverse_kinematics = delta_robot.ik(P1)
forward_kinematics = delta_robot.fk(inverse_kinematics)
print(ik)
print(fk)
This file introduces a point-to-point path planner class and for initiating the class you can write:
THETA_I = 0 # intial value
THETA_F = 1 # final value
path_planner = PathPlannerPTP(THETA_I, THETA_F)
As discussed in the previous sections there are 6 point to point methods implemented:
- Parabolic
- Trapezoidal Velocity Method
- S-Curve Velocity Method
- 5th-Order Interpolating Polynomial
- 7th-Order Interpolating Polynomial
- 9th-Order Interpolating Polynomial
You can call each of those methods and plot the results like this:
# results for the parabolic method
results = path_planner.ptp_bangbang()
path_planner.plot(results, "Parabolic Method", num_differentials=2)
# results for the trapezoidal velocity profile
results = path_planner.ptp_trapezoidal()
path_planner.plot(results, "Trapezoidal Velocity Profile", num_differentials=2)
# results for the S-curve velocity profile
results = path_planner.ptp_scurve()
path_planner.plot(results, "S-curve Profile")
# results for the 5th order interpolating polynomial
results = path_planner.ptp_polynomial5th()
path_planner.plot(results, "5th order polynomial")
# results for the 7th order interpolating polynomial
results = path_planner.ptp_polynomial7th()
path_planner.plot(results, "7th order polynomial")
# results for the 9th order interpolating polynomial
results = path_planner.ptp_polynomial9th()
path_planner.plot(results, "9th order polynomial")
in each of the different methods used, the results
is always an array of values that interpolates the initial and final values
This file introduces a multi-point path planner class and for initiating the class you can write:
# Set the path
PATH = [0, -0.2, 0.3, 0.8, -0.1, 1]
# initialize the path planner class
path_planner = PathPlannerMLTP(PATH)
Here are the multi-point trajectory planning methods implemented:
- Point to Point Methods - Used for multiple points
- Higher-order Polynomial Multi-Point Interpolation
- Cubic Spline
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_polynomial5th")
path_planner.plot(results, "mltp - ptp polynomial5th")
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_polynomial7th")
path_planner.plot(results, "mltp - ptp polynomial7th")
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_polynomial9th")
path_planner.plot(results, "mltp - ptp polynomial9th")
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_bangbang")
path_planner.plot(results, "mltp - ptp bangbang")
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_trapezoidal")
path_planner.plot(results, "mltp - ptp trapezoidal")
# point to point methods for multiple points
results = path_planner.mltp_ptpmethods("ptp_scurve")
path_planner.plot(results, "mltp - ptp scurve")
# calculate the trajectory based on cubic spline
results = path_planner.mltp_polynomial7th_4point()
path_planner.plot(results, "7th order polynomial")
# calculate the trajectory based on cubic spline
results = path_planner.mltp_polynomial9th_4point()
path_planner.plot(results, "9th order polynomial")
# calculate the trajectory based on cubic spline
results = path_planner.mltp_polynomial11th_4point()
path_planner.plot(results, "11th order polynomial")
# calculate the trajectory based on cubic spline
results = path_planner.mltp_cubicspline()
path_planner.plot(results, "cubic spline")
First we use define the kinematics of the delta robot, then define the four-point-path and finally ask for the results from the code:
# Set the path
PATH = [[0, 0, 0], [0, 0, 0.5], [1, 1, 0.5], [1, 1, 0]]
# init the robot
robot = DeltaKinematics(0.2, 0.46, 0.1, 0.074)
# initialize the path planner class
path_planner = DeltaPathPlanner(robot,PATH)
Here is how to output the results and plot them:
# calculate the trajectory based on cubic spline
results = path_planner.trajectory_gen("mltp_cubicspline")
path_planner.plot(results, "adept mltp_cubicspline")
path_planner.plot3d(results[1], "adept cycle cubic spline")
# multi-point high order polynomials
results = path_planner.trajectory_gen('mltp_polynomial7th_4point')
path_planner.plot(results, "mltp_polynomial7th_4point")
path_planner.plot3d(results[1], "adept cycle 4 point polynoimal 7th order")
results = path_planner.trajectory_gen('mltp_polynomial9th_4point')
path_planner.plot(results, "mltp_polynomial9th_4point")
path_planner.plot3d(results[1], "adept cycle 4 point polynoimal 9th order")
results = path_planner.trajectory_gen('mltp_polynomial11th_4point')
path_planner.plot(results, "mltp_polynomial11th_4point")
path_planner.plot3d(results[1], "adept cycle 4 point polynoimal 11th order")
# point to point methods
results = path_planner.trajectory_gen('ptp_polynomial5th')
path_planner.plot(results, 'ptp_polynomial5th')
path_planner.plot3d(results[1], 'point to point 5th order polynomial')
results = path_planner.trajectory_gen('ptp_polynomial7th')
path_planner.plot(results, 'ptp_polynomial7th')
path_planner.plot3d(results[1], 'point to point 7th order polynomial')
results = path_planner.trajectory_gen('ptp_polynomial9th')
path_planner.plot(results, 'ptp_polynomial9th')
path_planner.plot3d(results[1], 'point to point 9th order polynomial')
results = path_planner.trajectory_gen('ptp_bangbang')
path_planner.plot(results, 'ptp_bangbang')
path_planner.plot3d(results[1], 'point to point parabolic method')
results = path_planner.trajectory_gen('ptp_trapezoidal')
path_planner.plot(results, 'ptp_trapezoidal')
path_planner.plot3d(results[1], 'point to point trapezoidal')
results = path_planner.trajectory_gen('ptp_scurve')
path_planner.plot(results, 'ptp_scurve')
path_planner.plot3d(results[1], 'point to point S-curve')
- Kinematic Analysis of Delta Parallel Robot: Simulation Study - A. Eltayeb
- ?
- Trajectory Planning for Automatic Machines and Robots - Luigi Biagiotti
- Fundamentals of Robotic Mechanical Systems: Theory, Methods, and Algorithms - Jorge Angeles
Ok real talk here. I'm absolutely sick of working on papers and things like that. I think they are pretentious and faily useless for computer and robotics projects such as this one. I think github projects are millions of times more valuable because they provide real insight and tend to lean on explaining how things work instead of trying to convince you why the work is really important. But I need to work on publishing papers and also finishing my bachelor thesis ... at least for now. But just know that sharing this information in the raw format and purely practical way like this with the people around the world who might need it, is what makes me even slightly excited about doing this. that's what keeps me going. so thanks for taking interest in the previous projects I did on Delta robot trajectory planning. this will be the final version and I will hopefully never work or even see another delta robot in my entire life also follow me on instagram if you're into gaming 🖥️🎮🤖