Trajectory planning - HackMD

<style> .reveal { font-size: 24px; } </style> # Trajectory planning Summary from the book of [Professor Peter Corke](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) and [John J. Craig](http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040) For better look check this note in [HackMD.io](https://hackmd.io/@libernormous/trajectory-planning) by: [Liber Normous](https://hackmd.io/@libernormous) --- # Intro + it is explained on Professor Peter Corke [[1]](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) book page 44 and John J. Craig [[2]](http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040) page 207 + Path is a way from one point to another point + Trajectory is a path with a specific timing --- # Smooth One-Dimensional Trajectories Smoothness in this context means that its first few temporal derivatives are **continuous**. Typically velocity and acceleration are required to be continuous and sometimes also the derivative of acceleration or jerk ---- ## Ultimate matrix $$ \left(\begin{matrix} s_0\\ s_T\\ \dot{s_0}\\ \dot{s_T}\\ \ddot{s_0}\\ \ddot{s_T}\\ \end{matrix}\right)= \left(\begin{matrix} 0 & 0 & 0 & 0 & 0 & 1\\ T^5 & T^4 & T^3 & T^2 & T & 1\\ 0 & 0 & 0 & 0 & 1 & 0\\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0\\ 0 & 0 & 0 & 2 & 0 & 0\\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0\\ \end{matrix}\right) \left(\begin{matrix} A\\ B\\ C\\ D\\ E\\ F\\ \end{matrix}\right) $$ ---- ## Design steps 1. Decide your initial position $s_0$ and final position $s_T$ 2. Decide your initial velocity $\dot{s_0}$ and final velocity $\dot{s_T}$ 3. Decide your initial acceleration $\ddot{s_0}$ and final acceleration $\ddot{s_T}$ 4. Find coefficient $A\ B\ C\ D\ E\ F$ 5. How? inverse the matrix of course! 6. Note: this step is only for 1 axis, repeat the process for another axis 7. Note: $s_0$ is only for when $t=0$, and $s_t$ is only for when $t=T$ ---- ## Actuation step $$ \left(\begin{matrix} s(t)\\ \dot{s(t)}\\ \ddot{s(t)}\\ \end{matrix}\right)= \left(\begin{matrix} t^5 & t^4 & t^3 & t^2 & t & 1\\ 5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0\\ 20t^3 & 12t^2 & 6t & 2 & 0 & 0\\ \end{matrix}\right) \left(\begin{matrix} A\\ B\\ C\\ D\\ E\\ F\\ \end{matrix}\right) $$ Depends on what you want. Do you want to actuate using 1. position $s(t)$ (for direct animation) 2. speed $\dot{s(t)}$ (for motor) 3. acceleration $\ddot{s(t)}$ (IDK) --- # example $$ T=10 $$ $$ \left(\begin{matrix} s(0)\\ s(10)\\ \dot{s}(0)\\ \dot{s}(10)\\ \ddot{s}(0)\\ \ddot{s}(10)\\ \end{matrix}\right)=\left(\begin{matrix} 0\\ 1\\ 0\\ 0\\ 0\\ 0\\ \end{matrix}\right) $$ $$ \left(\begin{matrix} 0 & 0 & 0 & 0 & 0 & 1\\ T^5 & T^4 & T^3 & T^2 & T & 1\\ 0 & 0 & 0 & 0 & 1 & 0\\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0\\ 0 & 0 & 0 & 2 & 0 & 0\\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0\\ \end{matrix}\right)^{-1} \left(\begin{matrix} 0\\ 1\\ 0\\ 0\\ 0\\ 0\\ \end{matrix}\right)= \left(\begin{matrix} A\\ B\\ C\\ D\\ E\\ F\\ \end{matrix}\right) $$ --- ## CODE ```python=0 import numpy as np import matplotlib.pyplot as plt from math import sin,acos,sqrt # %% def traj_poly(s0,stf,sd0,sdtf,sdd0,sddtf,t): t0=t[0] # NOTe! t0 must always 0 tf=t[-1] if t0 != 0: return 0 #solving for equation coef = np.zeros((6,1)) #we are looking for this param = np.asarray([[s0],[stf],[sd0],[sdtf],[sdd0],[sddtf]]) mat = np.asarray([[0,0,0,0,0,1], [tf**5,tf**4,tf**3,tf**2,tf,1], [0,0,0,0,1,0], [5*tf**4,4*tf**3,3*tf**2,2*tf,1,0], [0,0,0,2,0,0], [20*tf**3,12*tf**2,6*tf,2,0,0]]) mat_i = np.linalg.inv(mat) #inverse coef = np.matmul(mat_i,param) #acquiring A B C D E F #using equation zeros = np.zeros(t.shape) ones = np.ones(t.shape) twos = ones*2 mat = np.asarray([ #the original equation [t**5,t**4,t**3,t**2,t,ones], [5*t**4,4*t**3,3*t**2,2*t,ones,zeros], [20*t**3,12*t**2,6*t,twos,zeros,zeros] ]) coef_tensor=(np.repeat(coef,t.size,axis=1)) coef_tensor=np.reshape(coef_tensor,(coef_tensor.shape[0],1,coef_tensor.shape[1])) # d = np.tensordot(mat,coef_tensor,axes=[1, 0]).diagonal(axis1=1, axis2=3) #alternative way res = np.einsum('mnr,ndr->mdr', mat, coef_tensor) return res ``` ---- ## Example CODE: ```python=0 # %% t = np.arange(1,11) y = traj_poly(0,1,0,0,0,0,t) plt.plot(y[0,0,:],'r',y[1,0,:],'g',y[2,0,:],'b') ``` ![](https://i.imgur.com/2opNhWJ.png) ---- ## Shift the time To shift a signal $f(x)=a+bx$ to the right (delay), we can do $f(x-\tau)=a+b(x-\tau)$ with $\tau$ as the amount of delay. Our matrix became more general like this: $$ \left(\begin{matrix} s_0\\ s_T\\ \dot{s_0}\\ \dot{s_T}\\ \ddot{s_0}\\ \ddot{s_T}\\ \end{matrix}\right)= \left(\begin{matrix} T_0^5 & T_0^4 & T_0^3 & T_0^2 & T_0 & 1\\ T_f^5 & T_f^4 & T_f^3 & T_f^2 & T_f & 1\\ 5T_0^4 & 4T_0^3 & 3T_0^2 & 2T_0 & 1 & 0\\ 5T_f^4 & 4T_f^3 & 3T_f^2 & 2T_f & 1 & 0\\ 20T_0^3 & 12T_0^2 & 6T_0 & 2 & 0 & 0\\ 20T_f^3 & 12T_f^2 & 6T_f & 2 & 0 & 0\\ \end{matrix}\right) \left(\begin{matrix} A\\ B\\ C\\ D\\ E\\ F\\ \end{matrix}\right) $$ ---- Then you can actuate with $T_0<t<T_f$ $$ \left(\begin{matrix} s(t)\\ \dot{s(t)}\\ \ddot{s(t)}\\ \end{matrix}\right)= \left(\begin{matrix} t^5 & t^4 & t^3 & t^2 & t & 1\\ 5t^4 & 4t^3 & 3t^2 & 2t & 1 & 0\\ 20t^3 & 12t^2 & 6t & 2 & 0 & 0\\ \end{matrix}\right) \left(\begin{matrix} A\\ B\\ C\\ D\\ E\\ F\\ \end{matrix}\right) $$ ---- CODE: ```python=0 def traj_poly2(s0,stf,sd0,sdtf,sdd0,sddtf,t0,tf,step=1): #arranging time t = np.arange(t0,tf+step,step) #solving for equation coef = np.zeros((6,1)) #we are looking for this param = np.asarray([[s0],[stf],[sd0],[sdtf],[sdd0],[sddtf]]) mat = np.asarray([ [t0**5,t0**4,t0**3,t0**2,t0,1], [tf**5,tf**4,tf**3,tf**2,tf,1], [5*t0**4,4*t0**3,3*t0**2,2*t0,1,0], [5*tf**4,4*tf**3,3*tf**2,2*tf,1,0], [20*t0**3,12*t0**2,6*t0,2,0,0], [20*tf**3,12*tf**2,6*tf,2,0,0] ]) mat_i = np.linalg.inv(mat) #inverse coef = np.matmul(mat_i,param) #acquiring A B C D E F #using equation zeros = np.zeros(t.shape) ones = np.ones(t.shape) twos = ones*2 mat = np.asarray([ #the original equation [(t)**5,(t)**4,(t)**3,(t)**2,(t),ones], [5*(t)**4,4*(t)**3,3*(t)**2,2*(t),ones,zeros], [20*(t)**3,12*(t)**2,6*(t),twos,zeros,zeros] ]) coef_tensor=(np.repeat(coef,t.size,axis=1)) coef_tensor=np.reshape(coef_tensor,(coef_tensor.shape[0],1,coef_tensor.shape[1])) # d = np.tensordot(mat,coef_tensor,axes=[1, 0]).diagonal(axis1=1, axis2=3) #alternative way res = np.einsum('mnr,ndr->mdr', mat, coef_tensor) time = t possi = res[0,0,:] speed = res[1,0,:] accel = res[2,0,:] return (time,possi,speed,accel) ``` ---- ## Example CODE: ```python=0 #Call function y = traj_poly2(40,0,0,0,0,0,0,10,0.1) plt.subplot(3,2,1) plt.plot(y[0],y[1],'r') plt.title('Position') plt.subplot(3,2,3) plt.plot(y[0],y[2],'g') plt.title('Speed') plt.subplot(3,2,5) plt.plot(y[0],y[3],'b') plt.title('Acceleration') y = traj_poly2(40,0,0,0,0,0,10,20,0.1) plt.subplot(3,2,2) plt.plot(y[0],y[1],'r') plt.title('Position delayed') plt.subplot(3,2,4) plt.plot(y[0],y[2],'g') plt.title('Speed delayed') plt.subplot(3,2,6) plt.plot(y[0],y[3],'b') plt.title('Acceleration delayed') ``` ---- RESULT: ![](https://i.imgur.com/tgQfCLT.png) --- # Linear segment with parabolic blends ![](https://i.imgur.com/Tmrg0No.png) ---- 1. Because the previous movement method actuates our motor to full speed only at 50% of the total time 2. That case, we need more linear area to drive our motor full speed 3. Check Ref [[3]](https://smartech.gatech.edu/bitstream/handle/1853/41948/ParabolicBlends.pdf). This guy has a very intuitive explanation that helps me write the code 4. We use via a transition between linear movement 5. Around this via we introduce blend time to smooth the speed transition 6. On linear area speed is constant 7. On parabolic area acceleration is constant but speed is changing, for example from $v_0$ to $0$ then to $v_f=-v_0$. ---- ## Linear phase At the linear phase we use this equation $$ q(t) = q_i + v_i(t-T_i)\\ \dot{q}(t) = v_i\\ \ddot{q}(t) = 0 $$ Where $T_i$ is the time delay. $v_i$ can be calculated or specified I have 2 different codes for different needs. If you only use linear you might want `line2()`. `line()` will be used in our next function ---- How to use it? 1. Specify a. Initial position b. Speed c. Time initial d. Time final 2. Or specify a. Position initial b. Position final c. Time initial d. Time final ---- CODE: ```python=0 # function for linear interpolation def line(point0, v0, t0, t1, step=1): # Generate a series of timestep t = np.arange(t0, t1+step,step)#makit one column # Calculate velocity v = v0 #time shift Ti = t0 #equation s = point0 + v*(t-Ti) v = np.ones(t.size)*v a = np.zeros(t.size) return (t,s,v,a) # function for linear interpolation def line2(point0, point1, t0, t1, step=1): # Generate a series of timestep t = np.arange(t0, t1+step,step)#makit one column # Calculate velocity v = (point1-point0)/(t1-t0) #time shift Ti = t0 #equation s = point0 + v*(t-Ti) v = np.ones(t.size)*v a = np.zeros(t.size) return (t,s,v,a) ``` ---- ### Example 1 CODE: ```python=0 # %% # Call function y = line(0,1,-10,10,1) plt.title('LINE') plt.plot(y[0],y[1]) ``` ![](https://i.imgur.com/UMryohx.png) ---- ### Example 2 CODE: ```python=0 #%% # Call function y = line2(10,20,-10,10,1) plt.title('LINE2') plt.plot(y[0],y[1]) ``` ![](https://i.imgur.com/cBfzRTX.png) ---- ## Parabolic phase This phase we use equation $$ q(t) = q_i + v_{i-1}(t-T_i) + \frac{1}{2}a(t-T_i+\frac{t_i^b}{2})^2\\ \dot{q}(t) = v_{i-1}(t-T_i) + a(t-T_i+\frac{t_i^b}{2})\\ \ddot{q}(t) = a $$ $q_i$ is the starting value. $i$ is the index of the via. $t$ is between $T_i-\frac{t_i^b}{2}<t<T_i+\frac{t_i^b}{2}$. $T_i$ is the time via happens and $t_i^b$ is the amount of blend *around* that via. So the parabolic phase start half $t_i^b$ before $T_i$ and half after that. Because the signal start at $T_i-\frac{t_i^b}{2}$ so we shift the equation to $T_i-\frac{t_i^b}{2}$. ---- CODE: ```python=0 def parab(p0, v0, v1, t0, t1, step=1): # Generate a series of timestep t = np.arange(t0, t1+step,step) #calculate acceleration a = (v1-v0)/(t1-t0) #time shift Ti=t0 # equation s = p0 +v0*(t-Ti) +0.5*a*(t-Ti)**2 v = v0 + a*(t-Ti) a = np.ones(t.size)*a return (t,s,v,a) ``` How to use it? Specify: 1. Initial pose 2. Speed before via 3. Speed after via 4. Start of period which is $T_i-\frac{t_i^b}{2}$ 5. Final of period which is $T_i+\frac{t_i^b}{2}$ ---- CODE: ```python=0 #%% y = parab(0, 5, 0, 0, 100, step=1) plt.plot(y[0],y[1]) y = parab(y[1][-1], 0, -5, 100, 200, step=1) plt.plot(y[0],y[1]) y = parab(50, 5, -5, 50, 250, step=1) plt.plot(y[0],y[1]) ``` RESULT: ![](https://i.imgur.com/mPVhTIp.png) ---- ## Linear and parabolic phase combined CODE: ```python=0 # %% def lspb(via,dur,tb): #1. It must start and end at the first and last waypoint respectively with zero velocity #2. Note that during the linear phase acceleration is zero, velocity is constant and position is linear in time # if acc.min < 0 : # print('acc must bigger than 0') # return 0 if ((via.size-1) != dur.size): print('duration must equal to number of segment which is via-1') return 0 if (via.size <2): print('minimum of via is 2') return 0 if (via.size != (tb.size)): print('acc must equal to number of via') return 0 #=====CALCULATE-VELOCITY-EACH-SEGMENT===== v_seg=np.zeros(dur.size) for i in range(0,len(via)-1): v_seg[i]=(via[i+1]-via[i])/dur[i] #=====CALCULATE-ACCELERATION-EACH-VIA===== a_via=np.zeros(via.size) a_via[0]=(v_seg[0]-0)/tb[0] for i in range(1,len(via)-1): a_via[i]=(v_seg[i]-v_seg[i-1])/tb[i] a_via[-1]=(0-v_seg[-1])/tb[-1] #=====CALCULATE-TIMING-EACH-VIA===== T_via=np.zeros(via.size) T_via[0]=0.5*tb[0] for i in range(1,len(via)-1): T_via[i]=T_via[i-1]+dur[i-1] T_via[-1]=T_via[-2]+dur[-1] #=====GENERATING-CHART/GRAPH/FIGURE===== # q(t) = q_i + v_{i-1}(t-T_i) + \frac{1}{2}a(t-T_i+\frac{t_i^b}{2})^2 #parabolic phase # q(t) = q_i + v_i*(t-T_i) #linear phase #parabolic t,s,v,a = parab(via[0], 0, v_seg[0], T_via[0]-0.5*tb[0], T_via[0]+0.5*tb[0], step=1) time = t pos = s speed = v accel = a for i in range(1,len(via)-1): # linear t,s,v,a = lerp(pos[-1],v_seg[i-1],T_via[i-1]+0.5*tb[i],T_via[i]-0.5*tb[i+1],0.01) time = np.concatenate((time,t)) pos = np.concatenate((pos,s)) speed = np.concatenate((speed,v)) accel = np.concatenate((accel,a)) #parabolic t,s,v,a = parab(pos[-1], v_seg[i-1], v_seg[i], T_via[i]-0.5*tb[i+1], T_via[i]+0.5*tb[i+1], 0.01) time = np.concatenate((time,t)) pos = np.concatenate((pos,s)) speed = np.concatenate((speed,v)) accel = np.concatenate((accel,a)) # linear t,s,v,a = lerp(pos[-1],v_seg[-1],T_via[-2]+0.5*tb[-2],T_via[-1]-0.5*tb[-1],0.01) time = np.concatenate((time,t)) pos = np.concatenate((pos,s)) speed = np.concatenate((speed,v)) accel = np.concatenate((accel,a)) #parabolic t,s,v,a = parab(pos[-1], v_seg[-1], 0, T_via[-1]-0.5*tb[-1], T_via[-1]+0.5*tb[-1], 0.01) time = np.concatenate((time,t)) pos = np.concatenate((pos,s)) speed = np.concatenate((speed,v)) accel = np.concatenate((accel,a)) print('v seg = ',v_seg, '\na via = ',a_via, '\nT via = ',T_via, '\ntime = ',time, '\npos = ',pos) return(v_seg,a_via,T_via,time,pos,speed,accel) ``` ---- How to use it? Specify: 1. All the via 2. All the duration between via 3. Blend time In Ref [[1]](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) [[2]](http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040) they prefer acceleration on each via as input. But I follow [[3]](https://smartech.gatech.edu/bitstream/handle/1853/41948/ParabolicBlends.pdf) to use blend time as an input because it is more intuitive, easy to imagine and understand. Instead of duration between via, we could use specific time each via, it will be easier to code. But don't input $t_b=0$ that means infinite acceleration in that blend $a=\frac{(v_1-v_0)}{t_b}$ ---- CODE: ```python=0 # %% via = np.asarray([0,40,0,40,0]) dur = np.asarray([20,20,20,20]) tb = np.asarray([1,1,1,1,1])*5 res=lspb(via,dur,tb) plt.plot(res[2],via,'*',res[3],res[4]) ``` RESULT: ![](https://i.imgur.com/LVZffTD.png) ---- We can plot acceleration, speed, together. As you can see that the speed is zero at the via with constant accelereration ```python=0 # %% plt.plot(res[2],np.zeros(via.size),'*') plt.plot(res[3],res[5]) plt.plot(res[3],res[6]) ``` ![](https://i.imgur.com/GLwGNHx.png) ---- Let's try anoter CODE: ```python=0 # %% via = np.asarray([0,30,40,10,0])-20 dur = np.asarray([20,20,20,20]) tb = np.asarray([1,1,1,1,1])*5 res=lspb(via,dur,tb) plt.plot(res[2],via,'*',res[3],res[4]) ``` ![](https://i.imgur.com/4a59pkN.png) ---- This thime the transition speed is not zero ```python=0 # %% plt.plot(res[2],np.zeros(via.size),'*') plt.plot(res[3],res[5]) plt.plot(res[3],res[6]) ``` ![](https://i.imgur.com/CU2yTOo.png) --- # Interpolation If you have 2 points, and you want to know a point in between. Interpolation works by specifying a scale $s$ between $0$ and $1$. $s=0$ means first point. $s=1$ means last point. $s=0.5$ is middle value between first and last point. ---- ## Linear interpolation (LERP) $$ \textrm{Lerp}(p_0, p_1, s)=(1-s)p_0+(s){p_1} $$ CODE: ```python=0 # linear interopolation def lerp(p0, p1, s): # p0 : point 1 # p1 : point 2 # s : scale, 0 to 1, 0 is p0, 1 is p1, and 0.5 is value between p0 and p1 return (1 - s) * p0 + s * p1 def lerp2(p0, p1, t0, t1, step=1): # map t0 to 0 and t1 to 1 t = np.arange(t0,t1+step,step) s = (t-t0)/(t1-t0) #map function pos = (1 - s) * p0 + s * p1 return (t,pos) ``` ---- Example 1: First point = 10, last point = 50. what is value when $s=\{0,0.5,0.7,1\}$ ```python=0 s = np.asarray([0,0.5,0.7,1]) y = lerp(10, 50, s) plt.plot(s,y,'o') ``` Result: ``` [10. 30. 38. 50.] ``` ![](https://i.imgur.com/gRidxjL.png) ---- By using the same idea we can build a function but not scale based but time based. So we can input first point with its time step, and last point with its time step. Basically we create a time step array and map each value in the time step array to be between $0$ and $1$. Example 2: $p=40$ when $t=10$ and $p=10$ when $t=40$. Plot the graph ```python=0 y = lerp2(40, 10, 10, 40, step=1) plt.plot(y[0],y[1]) ``` Result: ![](https://i.imgur.com/J0H1w35.png) ---- ## Spherical interpolation (SLERP) This kind of interpolation are linear based on the relation between points and scale but the point path it creates is arch like path. The center of the arch is always on the origin of coordinate frame. This function is really useful if we have 2 quaternion vectors. By this function we can interpolate these 2 quaternion vectors to make a direct rotation path. $$ \cos\Omega = q_1q_2\\ q_1q_2 = q_{1x}q_{2x}+q_{1y}q_{2y}+q_{1z}q_{2z}\\ \textrm{Slerp}(q_1,q_2,s) = \frac{\sin((1-s)\Omega)}{\sin\Omega}q_1+\frac{\sin(s\Omega)}{\sin\Omega}q_2 $$ ---- ![](https://upload.wikimedia.org/wikipedia/commons/thumb/b/b3/Slerp_factor_explanation.png/800px-Slerp_factor_explanation.png) ---- CODE: ```python=0 # %% # Spherical linear interopolation def slerp(q1,q2,s): #Normalize q1_norm = q1/np.sum(np.square(q1)) q2_norm = q2/np.sum(np.square(q2)) #dor product q1\{w1,x1,y1,z1} q2\{w2,x2,y2,z2} q1q2=w1w2+x1x2+y1y2+z1z2 q1q2=q1_norm*q2_norm q1q2=np.sum(q1q2) #omega omega = acos(q1q2) #main eq res = np.sin((1-s)*omega)/np.sin(omega)*q1 + np.sin(s*omega)/np.sin(omega)*q2 return res #%% def slerp2(q1,q2,t0,t1,step=0.1): #arangig scale t = np.arange(t0,t1+step,step) s = (t-t0)/(t1-t0) #map t to scale #Normalize q1_norm = q1/np.sum(np.square(q1)) q2_norm = q2/np.sum(np.square(q2)) #dor product q1\{w1,x1,y1,z1} q2\{w2,x2,y2,z2} q1q2=w1w2+x1x2+y1y2+z1z2 q1q2=q1_norm*q2_norm q1q2=np.sum(q1q2) #omega omega = acos(q1q2) #main eq res = np.sin((1-s)*omega)/np.sin(omega)*q1 + np.sin(s*omega)/np.sin(omega)*q2 return (t,res) ``` ---- Example 1 : If we have $v0=(5,0,0)^T$ and $v1=(0,0,5)^T$. What's value in between the scale of $s={0,0.5,0.8,1}$ ```python=0 s = np.arange(0,1,0.01) s = np.asarray([0,0.5,0.8,1]) p0 = np.asarray([ [5], [0], [0] ]) p1 = np.asarray([ [0], [0], [5] ]) pos = slerp(p0, p1, s) print(pos) ``` ---- Result: ``` [[5. 3.53553391 1.54508497 0. ] [0. 0. 0. 0. ] [0. 3.53553391 4.75528258 5. ]] ``` ![](https://i.imgur.com/4mcXCC1.png) ---- Same as LERP, we can construct a version of SLERP that uses time step as trajectory. Example 1 : We have $v0=(0,-5,-5)^T$ at $t=0$ and $v1=(0,-5,5)^T$ at $t=10$. Plot the graph. ```python=0 # %% %matplotlib inline from mpl_toolkits.mplot3d import axes3d p0 = np.asarray([ [0], [-5], [-5] ]) p1 = np.asarray([ [0], [-5], [5] ]) t, pos = slerp2(p0, p1, 0, 10, 0.1) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(pos[0],pos[1],pos[2]) ``` ---- Result: ![](https://i.imgur.com/AiB24ne.png) ---- If you are curious about time plot of example 2, here I give you an answer. This is the plot of Y-axes across time. ![](https://i.imgur.com/1v6RNwt.png) --- # MATLAB class README **How to use** 1. $s \in [0,1]$ so first parameter always starts with $0$ and ends with $1$. Define in rows vector. For example `p_param = [0; 0.5; 1];` 3. Second parameter is velocity. Define in rows vector. For example `v_param = [0; 0.5; 0];` 4. Thirs parameter is acceleration, It is not mandatory, you can put `[]`. Define in rows vector. For example `a_param = [0; 0.5; 0];` <details> <summary> CODE (not using acceleration) </summary> ```c= %% clc;clear; gen_traj = gen_traj(); T = 60; t_series = 0:0.1:T; p_param = [0; 1]; v_param = [0; 1]; gen_traj.pos_vel_T(p_param, v_param, t_series); gen_traj.plot_s_sdot(); ``` </details> <details> <summary> CODE (using acceleration) </summary> ```c= %% clear; clc; gen_traj = gen_traj(); T = 60; t_series = 0:0.1:T; p_param = [0; 1]; v_param = [0; 0]; a_param = [0; 0]; gen_traj.pos_vel_acc_T(p_param, v_param, a_param, t_series); s_t = gen_traj.s_t; s_t_dot = gen_traj.s_t_dot; s_t_ddot = gen_traj.s_t_ddot; t = t_series; gen_traj.plot_s_sdot_sddot(); ``` </details> <details> <summary> CLASS </summary> ```c= classdef gen_traj < handle %GEN_TRAJ Summary of this class goes here % Detailed explanation goes here properties s_t; s_t_dot; s_t_ddot; coef_ABCD; mat_design; mat_utilize; end properties (Access = private) n1 n2 n3 deg t_series end methods function obj = gen_traj() end function obj = pos_vel_T(obj, p_param, v_param, t_series) obj.t_series = t_series; %design design_pos_vel(obj, p_param, v_param, t_series); % utilizing utilize_pos_vel(obj, t_series); end function obj = pos_vel_acc_T(obj, p_param, v_param, a_param, t_series) obj.t_series = t_series; T = t_series(end); obj.n1 = length(p_param); % obj.n2 = length(v_param); obj.n3 = length(a_param); obj.deg = obj.n1+obj.n2+obj.n3-1; %design mat_p = zeros(obj.n1, obj.deg+1); for i=0:obj.n1-1 for j=0:obj.deg t = (i)/(obj.n1-1)*T; % i is from 1 to n1+1; t is from 0/(n1-1) ~ (n1-1)/(n1-1) pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mat_p(i+1,j+1)=t^j; end end mat_v = zeros(obj.n2, obj.deg+1); for i=0:obj.n2-1 for j=1:obj.deg t = (i)/(obj.n2-1)*T; % i is from 1 to n2+1; t is from 0/(n2-1) ~ (n2-1)/(n2-1) pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mul = j; mat_v(i+1,j+1)=mul*t^pow; end end mat_a = zeros(obj.n3, obj.deg+1); for i=0:obj.n3-1 for j=2:obj.deg t = (i)/(obj.n3-1)*T; % i is from 1 to n3+1; t is from 0/(n3-1) ~ (n3-1)/(n3-1) pow = j-2; % j is from 1 to deg+1; pow is from 0 to deg mul1 = j; mul2 = j-1; mat_a(i+1,j+1)=mul1 * mul2 * t^pow; end end obj.mat_design = [mat_p; mat_v; mat_a]; obj.coef_ABCD = obj.mat_design\[p_param;v_param;a_param]; % Utilize t = t_series; mat_p_u = zeros(1, obj.deg+1, length(t)); mat_v_u = zeros(1, obj.deg+1, length(t)); mat_a_u = zeros(1, obj.deg+1, length(t)); for i=1:length(t) for j=0:obj.deg pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mat_p_u(1,j+1, i)=t(i)^j; end for j=1:obj.deg pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mul = j; mat_v_u(1,j+1,i)=mul*t(i)^pow; end for j=2:obj.deg pow = j-2; % j is from 1 to deg+1; pow is from 0 to deg mul1 = j; mul2 = j-1; mat_a_u(1,j+1,i)=mul1 * mul2 * t(i)^pow; end end obj.mat_utilize = [mat_p_u;mat_v_u;mat_a_u]; obj.s_t = zeros(length(t), 1); obj.s_t_dot = zeros(length(t), 1); obj.s_t_ddot = zeros(length(t), 1); for i=1:length(t) obj.s_t(i) =obj.mat_utilize(1,:, i)*obj.coef_ABCD; obj.s_t_dot(i) =obj.mat_utilize(2,:, i)*obj.coef_ABCD; obj.s_t_ddot(i) =obj.mat_utilize(3,:, i)*obj.coef_ABCD; end end function obj = design_pos_vel(obj, p_param, v_param, t_series) T = t_series(end); obj.n1 = length(p_param); % obj.n2 = length(v_param); obj.deg = obj.n1+obj.n2-1; mat_p = zeros(obj.n1, obj.n1+obj.n2); for i=0:obj.n1-1 for j=0:obj.deg t = (i)/(obj.n1-1)*T; % i is from 1 to n1+1; t is from 0/(n1-1) ~ (n1-1)/(n1-1) pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mat_p(i+1,j+1)=t^j; end end mat_v = zeros(obj.n2, obj.n1+obj.n2); for i=0:obj.n2-1 for j=1:obj.deg t = (i)/(obj.n2-1)*T; % i is from 1 to n2+1; t is from 0/(n2-1) ~ (n2-1)/(n2-1) pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mul = j; mat_v(i+1,j+1)=mul*t^pow; end end obj.mat_design = [mat_p;mat_v]; obj.coef_ABCD = obj.mat_design\[p_param;v_param]; end function obj = utilize_pos_vel(obj, t_series) % Utilize t = t_series; mat_p_u = zeros(1, obj.n1+obj.n2, length(t)); mat_v_u = zeros(1, obj.n1+obj.n2, length(t)); for i=1:length(t) for j=0:obj.deg pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mat_p_u(1,j+1, i)=t(i)^j; end for j=1:obj.deg pow = j-1; % j is from 1 to deg+1; pow is from 0 to deg mul = j; mat_v_u(1,j+1,i)=mul*t(i)^pow; end end obj.mat_utilize = [mat_p_u;mat_v_u]; obj.s_t = zeros(length(t), 1); obj.s_t_dot = zeros(length(t), 1); for i=1:length(t) obj.s_t(i)=obj.mat_utilize(1,:, i)*obj.coef_ABCD; obj.s_t_dot(i)=obj.mat_utilize(2,:, i)*obj.coef_ABCD; end end function plot_s_sdot_sddot(obj) t = obj.t_series; fig1000=figure(1000); fig1000.Name=('using s, s_dot, s_ddot'); subplot(3,1,1); plot(t,obj.s_t, 'linewidth', 2); ylabel('$s(t)$', 'interpreter', 'latex'); xlabel('$t$', 'interpreter', 'latex'); grid on; set(gca,'FontSize',14); set(gca,'FontName','serif'); set(gca,'FontWeight','bold'); set(gca,'LineWidth',2); subplot(3,1,2); plot(t,obj.s_t_dot, 'linewidth', 2); ylabel('$\dot s(t)$', 'interpreter', 'latex'); xlabel('$t$', 'interpreter', 'latex');grid on; set(gca,'FontSize',14); set(gca,'FontName','serif'); set(gca,'FontWeight','bold'); set(gca,'LineWidth',2); subplot(3,1,3); plot(t,obj.s_t_ddot, 'linewidth', 2); ylabel('$\ddot s(t)$', 'interpreter', 'latex'); xlabel('$t$', 'interpreter', 'latex');grid on; set(gca,'FontSize',14); set(gca,'FontName','serif'); set(gca,'FontWeight','bold'); set(gca,'LineWidth',2); end function plot_s_sdot(obj) t = obj.t_series; fig1001=figure(1001); fig1001.Name=('using s and s_dot only'); subplot(2,1,1); plot(t,obj.s_t, 'linewidth', 2); ylabel('$s(t)$', 'interpreter', 'latex'); xlabel('$t$', 'interpreter', 'latex'); grid on; set(gca,'FontSize',14); set(gca,'FontName','serif'); set(gca,'FontWeight','bold'); set(gca,'LineWidth',2); subplot(2,1,2); plot(t,obj.s_t_dot, 'linewidth', 2); ylabel('$\dot s(t)$', 'interpreter', 'latex'); xlabel('$t$', 'interpreter', 'latex');grid on; set(gca,'FontSize',14); set(gca,'FontName','serif'); set(gca,'FontWeight','bold'); set(gca,'LineWidth',2); end end end ``` </details> --- # References [[1] Robotics vision and control Professor Peter Corke p.44](https://link.springer.com/book/10.1007%2F978-3-642-20144-8) [[2] Introduction to robotics John J. Craig p.207](http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040) [[3] Turning Paths Into Trajectories Using Parabolic Blends Tobias Kunz and Mike Stilman](https://smartech.gatech.edu/bitstream/handle/1853/41948/ParabolicBlends.pdf) [[4] wikipedia SLERP](https://en.wikipedia.org/wiki/Slerp) ###### tags: `robot`