Hugo Hadfield
Cambridge University PhD student, Signal Processing and Communications Laboratory
Kinematics of a Delta Robot in CGA
Hugo Hadfield
November 2019
Delta Robots
- Very fast moving
- Simple forward and inverse kinematics
- Cheap and easy to build
History of the Delta Robot
- Invented by Reymond Clavel at EPFL in 1985
- A type of parallel manipulator
Model of a Delta Robot
Forward Kinematics
\begin{aligned}
a_i &= (r_b - r_e + l\cos(\theta_i))s_i + l\sin(\theta_i)e_3\\
A_i &= \frac{1}{2}a_i^2n_\infty + a_i - n_0\\
\Sigma_i &= A_i - \frac{1}{2}\rho^2n_\infty\\
T &= I_5\bigwedge_{i=0}^{i=2}\Sigma_i\\
P &= 1 + \frac{T}{\sqrt{T^2}}\\
Y &= -\tilde{P}(T\cdot n_\infty)P\\
y &= \frac{-\sum_{j=1}^{j=3}(Y\cdot e_j)e_j}{Y\cdot n_\infty}
\end{aligned}
r_e
Y
A_i
r_b
l
\rho
\theta
Reachable Volume
Inverse Kinematics
\begin{aligned}
C_i &= \left( \text{up}(r_bs_i) - \frac{1}{2}l^2n_\infty \right)\wedge(I_3s_i\wedge e_3) \\
\Sigma_i &= \text{up}(y + r_es_i) - \frac{1}{2}\rho^2n_\infty \\
T_i &= (C_i\wedge\Sigma_i)I_5\\
P_i &= \frac{1}{2}(1 + \frac{T_i}{\sqrt{T_i^2}})\\
A_i &= -\tilde{P_i}(T_i\cdot n_\infty)P_i\\
a_i &= -\frac{\sum_{j=1}^{j=3}(A_i\cdot e_j )e_j}{A_i\cdot n_\infty} \\
z_i &= a_i - r_bs_i \\
\theta_i &= \arctan{\frac{z_i\cdot e_3}{z_i\cdot s_i}}
\end{aligned}
If \(T_i^2 < 0\) then the point is unreachable
Forward Jacobian
- Move only one motor
- This fixes two of the spheres, they intersect in a circle
- We are therefore interested in the differential of the end point of the point pair made from intersection of the circle and the moving sphere
- First we need to find how the center of the sphere changes as we move theta
\Sigma_i = A_i - \frac{1}{2}\rho^2n_\infty \\
- How does point pair move with the sphere?
= \frac{\partial \Sigma_i}{\partial \theta_i} = \frac{\partial A_i}{\partial \theta_i} = -(r_b - r_e)l\sin(\theta_i)n_\infty - l\sin(\theta_i)s_i + l\cos(\theta_i)e_3
= \left( \frac{\partial a_i}{ \partial \alpha}\cdot a_i \right)n_\infty + \frac{\partial a_i}{\partial \alpha}
\frac{\partial \Sigma_i}{\partial \alpha} = \frac{\partial A_i}{\partial \alpha} = \frac{1}{2}\frac{\partial a_i^2}{\partial \alpha}n_\infty + \frac{\partial a_i}{\partial \alpha} \\
Forward Jacobian (2)
T = \Sigma_i\wedge C\\
\frac{\partial T }{\partial \theta_i} = \frac{\partial \Sigma }{\partial \theta_i}\wedge C\\
- Now we need to know how the conformal endpoint of the point pair changes
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
P = \frac{1}{2}\left(1 + \frac{T}{\sqrt{T^2}}\right)\\
Y = -\tilde{P}(T\cdot n_\infty)P\\
\pdiff{P}{\theta_i} = \frac{1}{2T^2}\left(\sqrt{T^2}\pdiff{T}{\theta_i} - T\frac{\pdiff{T}{\theta_i}\cdot T}{\sqrt{T^2}}\right)\\
\pdiff{Y}{\theta_i} = -\tilde{\pdiff{P}{\theta_i}}(T\cdot n_\infty)P -\tilde{P}(\pdiff{T}{\theta_i}\cdot n_\infty)P -\tilde{P}(T\cdot n_\infty)\pdiff{P}{\theta_i}
- How does the 3D endpoint change with the conformal endpoint change? (derivative of 'down' function)
Forward Jacobian (3)
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\pdiff{y}{\theta_i} = \frac{ -\sum_{j=1}^{j=3}\left(\pdiff{Y}{\theta_i}\cdot e_{j}\right)e_j (Y\cdot\ninf) + \sum_{j=1}^{j=3}\left(Y\cdot e_{j}\right)e_j (\pdiff{Y}{\theta_i}\cdot\ninf) }{ (Y\cdot\ninf)^2 }
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
y = \frac{-\sum_{j=1}^{j=3}(Y\cdot e_j)e_j}{Y\cdot n_\infty}
- We now have a closed form expression for the derivative of the endpoint wrt. theta and we didn't have to use maple once!
- We can use this for control and optimisation. Consider a cost function:
Forward Jacobian In Use
g(y) = (y-t)^2 \\
\frac{\partial g}{\partial \theta_i} = 2\frac{\partial y}{\partial \theta_i}\cdot(y-t)
- We therefore have a closed form for
and so can do gradient descent
\nabla g
- Given the forward jacobian we can simply invert the 3x3 matrix at each point to get the inverse jacobian
OR
- We can differentiate our inverse kinematic equations..
Inverse Jacobian
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\begin{aligned}
\Sigma_i &= X_i - \frac{1}{2}\rho^2n_\infty \\
\frac{\partial \Sigma_i}{\partial \alpha} &= \frac{\partial X_i}{\partial \alpha} = \left(\pdiff{x_i}{\alpha}\cdot x_i\right) \ninf + \pdiff{x_i}{\alpha}\\
x_i &= y + r_es_i\\
\frac{\partial x_i}{\partial \alpha} &= \frac{\partial y}{\partial \alpha}
\end{aligned}
Starting with Sigma_i
Differentiate wrt. a scalar param \alpha
the sphere centred at the meeting of the 4 bar mechanism and the end platform
- Now we need to know how the elbow positions vary with \alpha
Inverse Jacobian (2)
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\begin{aligned}
C_i &= \left( B_i - \frac{1}{2}l^2n_\infty \right)\wedge(I_3s_i\wedge e_3)\\
T_i &= (\Sigma_i\wedge C_i)^*\\
\pdiff{T_i}{\alpha} &= (\pdiff{\Sigma_i}{\alpha}\wedge C_i)^*
\end{aligned}
- Here A_i is the conformal elbow position
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\begin{aligned}
\pdiff{A_i}{\alpha} &= -\tilde{\pdiff{P_i}{\alpha}}(T_i\cdot n_\infty)P_i -\tilde{P_i}(\pdiff{T_i}{\alpha}\cdot n_\infty)P_i -\tilde{P_i}(T_i\cdot n_\infty)\pdiff{P_i}{\alpha}\\
\pdiff{a_i}{\alpha} &= \frac{ -\sum_{j=1}^{j=3}\left(\pdiff{A_i}{\alpha}\cdot e_{j}\right)e_j (A_i\cdot\ninf) + \sum_{j=1}^{j=3}\left(A_i\cdot e_{j}\right)e_j (\pdiff{A_i}{\alpha}\cdot\ninf) }{ (A_i\cdot\ninf)^2 }
\end{aligned}
- Finally we need to know how the joints vary with elbow position
Inverse Jacobian (3)
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\begin{aligned}
z_i &= a_i - r_bs_i \\
\theta_i &= \arctan{\frac{z_i\cdot e_3}{z_i\cdot s_i}}\\
\frac{\partial \theta_i}{\partial \alpha} &= \frac{z_i\cdot s_i \frac{\partial z_i}{\partial \alpha}\cdot e_3 - z_i\cdot e_3 \frac{\partial z_i}{\partial \alpha}\cdot s_i}{z_i^2}
\end{aligned}
- We are done (Once again no Maple!)
- The inverse jacobian allows us to map velocity of the endpoint to velocity of joints
Inverse Jacobian in Use
g(y) = (y - t)^2 \\
\frac{\partial g}{\partial y} = 2(y-t)\\
- In other words, to minimise cost at maximum rate drive the end point linearly at our target point (obviously)
- With the inverse jacobian we can do exactly that
\newcommand{\la}{\langle}
\newcommand{\ra}{\rangle}
\newcommand{\nn}{\nonumber}
\newcommand{\ninf}{n_{\infty}}
\newcommand{\einf}{e_{\infty}}
\newcommand{\no}{n_{0}}
\newcommand{\eo}{e_{0}}
\newcommand{\wdg}{\wedge}
\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2} }
\begin{aligned}
\pdiff{y}{\alpha_j} \propto (2(t-y))\cdot e_j
\end{aligned}
- CGA provides a very neat framework for solving the forward and inverse kinematics of parallel robots
- We can easily simulate the dynamics of the robot with clifford and ganja.js
Summary
- Trivectors in CGA represent lines and circles
- The direct interpolation of lines and circles results in trivectors interpretable as general screws
- Screw theory is widely used in robotics for analysis of kinematics and dynamics
- Lets bring screw theory and CGA (also PGA) together properly and analyse a whole load of systems
Future Work
Kinematics of a Delta Robot in CGA
By Hugo Hadfield
