''' This file contains a representation of the classical robotic arm DAE, cf. Singularities of the Robotic Arm DAE by Diana Este'vez Schwarz, Rene' Lamour, Roswitha Maerz in DAE Forum 2019 ''' import algopy from algopy import zeros from numpy import sin, cos, exp,zeros as npzeros,array def RoboticArmSolutionCa88(t): """ input t - timepoint, where the solution is computed output x - solution vector x(t) xabl - derivative x'(t) of the components x1,...,x6. x7'(t) and x8'(t) is set to zero. """ expt=exp(t) exp2t=exp(2*t) exp3t=exp(3*t) exp4t=exp(4*t) x1=1. - expt x3=expt-t #x1=1.-t #x3=-x3 cosx3=cos(x3) cos2x3=cos(2*(x3)) cos3x3=cos(3*(x3)) cos4x3=cos(4*(x3)) cos5x3=cos(5*(x3)) cos6x3=cos(6*(x3)) cos7x3=cos(7*(x3)) cos8x3=cos(8*(x3)) cos9x3=cos(9*(x3)) sinx3=sin(x3) sin2x3=sin(2*(x3)) sin3x3=sin(3*(x3)) sin4x3=sin(4*(x3)) sin5x3=sin(5*(x3)) sin6x3=sin(6*(x3)) sin7x3=sin(7*(x3)) sin8x3=sin(8*(x3)) sin9x3=sin(9*(x3)) x2=(84*expt - 72.*t - (91*expt - 96.*t)*cosx3 + 4*expt*(-3. + cos2x3) + 4*expt*cos2x3 -8.*t*cos2x3 - expt*cos3x3 - 15*exp2t*sinx3 + exp2t*sin3x3)/(4.*(9 - 12*cosx3 + cos2x3)) x4=-expt x5=(2*(-1 + expt)*(6*sinx3 - sin2x3)*(-72*expt + 72*t + (91*expt - 96*t)*cosx3 - 8*(x3)*cos2x3 + expt*cos3x3 + \ 15*exp2t*sinx3 - exp2t*sin3x3) + (9 - 12*cosx3 + cos2x3)*(-72 + 84*expt - 15*exp2t*(-1 + expt)*cosx3 - \ (-96 + 91*expt)*cosx3 + 4*expt*(-3 + cos2x3) - 8*cos2x3 + 4*expt*cos2x3 - expt*cos3x3 + 3*exp2t*(-1 + expt)*cos3x3 \ - 30*exp2t*sinx3 + (-1 + expt)*(91*expt - 96*t)*sinx3 - 16*expt*(-1 + expt)*sin2x3 + \ 16*(-1 + expt)*t*sin2x3 + 2*exp2t*sin3x3 + 3*expt*(-1 + expt)*sin3x3))/(4*(9 - 12*cosx3 + cos2x3)**2) x6=-1 + expt U2= (expt*(-262416 + 272136*expt - 310632*exp2t + (374994 - 284084*expt + 355708*exp2t)*cosx3 + 240*(-482 - 376*expt + 221*exp2t)*cos2x3 - \ 20260*cos3x3 +155336*expt*cos3x3 - 150544*exp2t*cos3x3 + 32512*cos4x3 - 58848*expt*cos4x3 + \ 61248*exp2t*cos4x3 - 10004*cos5x3 - 1176*expt*cos5x3 - 768*exp2t*cos5x3 + 480*cos6x3 + \ 8832*expt*cos6x3 - 7728*exp2t*cos6x3 + 103*cos7x3 - 2678*expt*cos7x3 + 2446*exp2t*cos7x3 + 16*cos8x3 +\ 216*expt*cos8x3 - 216*exp2t*cos8x3 - cos9x3 - 6*expt*cos9x3 + 6*exp2t*cos9x3 - 11484*sinx3 + \ 233026*expt*sinx3 - 211732*exp2t*sinx3 + 105866*exp3t*sinx3 - 6672*sin2x3 - \ 148224*expt*sin2x3 + 146832*exp2t*sin2x3 - 73416*exp3t*sin2x3 + 22664*sin3x3 - \ 14108*expt*sin3x3 - 43448*exp2t*sin3x3 + 21724*exp3t*sin3x3 - 12816*sin4x3 + \ 48000*expt*sin4x3 + 5136*exp2t*sin4x3 - 2568*exp3t*sin4x3 + 664*sin5x3 - 16468*expt*sin5x3 + \ 5624*exp2t*sin5x3 - 2812*exp3t*sin5x3 + 1968*sin6x3 - 768*expt*sin6x3 - 4656*exp2t*sin6x3 + \ 2328*exp3t*sin6x3 - 718*sin7x3 + 1297*expt*sin7x3 + 1182*exp2t*sin7x3 - 591*exp3t*sin7x3 + 72*sin8x3 -\ 192*expt*sin8x3 - 72*exp2t*sin8x3 + 36*exp3t*sin8x3 - 2*sin9x3 + 7*expt*sin9x3 + 2*exp2t*sin9x3 - \ exp3t*sin9x3))/(32*(-3 + cos2x3)*(9 - 12*cosx3 + cos2x3)**3) U1=-((105216*expt - 272136*exp2t + 310632*exp3t - 151680*expt*cosx3 + 284084*exp2t*cosx3 - 355708*exp3t*cosx3 + 66144*expt*cos2x3 +\ 90240*exp2t*cos2x3 - 53040*exp3t*cos2x3 - 25648*expt*cos3x3 - 155336*exp2t*cos3x3 + \ 150544*exp3t*cos3x3 + 7104*expt*cos4x3 + 58848*exp2t*cos4x3 - 61248*exp3t*cos4x3 + 1968*expt*cos5x3 + \ 1176*exp2t*cos5x3 + 768*exp3t*cos5x3 - 3168*expt *cos6x3 - 8832*exp2t *cos6x3 + \ 7728*exp3t *cos6x3 + 1272*expt *cos7x3 + 2678*exp2t *cos7x3 - 2446*exp3t *cos7x3 + \ 24*expt*(-8 - 9*expt + 9*exp2t)*cos8x3 - 2*expt*(-4 - 3*expt + 3*exp2t)*cos9x3 - 160116*sinx3 + 11484*expt*sinx3 - \ 213196*exp2t *sinx3 + 211732*exp3t *sinx3 - 105866*exp4t *sinx3 + 175824 *sin2x3 + \ 6672*expt *sin2x3 + 99408*exp2t *sin2x3 - 146832*exp3t *sin2x3 + 73416*exp4t *sin2x3 - 83096 *sin3x3 - \ 22664*expt *sin3x3 + 68112*exp2t *sin3x3 + 43448*exp3t *sin3x3 - 21724*exp4t *sin3x3 + 5904 *sin4x3 + \ 12816*expt *sin4x3 - 73840*exp2t *sin4x3 - 5136*exp3t *sin4x3 + 2568*exp4t *sin4x3 + \ 12440 *sin5x3 - 664*expt *sin5x3 + 16288*exp2t *sin5x3 - 5624*exp3t *sin5x3 + \ 2812*exp4t *sin5x3 - 5616 *sin6x3 - 1968*expt *sin6x3 + 5520*exp2t *sin6x3 + \ 4656*exp3t *sin6x3 - 2328*exp4t *sin6x3 + 958 *sin7x3 + 718*expt *sin7x3 - \ 2890*exp2t *sin7x3 - 1182*exp3t *sin7x3 + 591*exp4t *sin7x3 - 72 *sin8x3 - \ 72*expt *sin8x3 + 376*exp2t *sin8x3 + 72*exp3t *sin8x3 - 36*exp4t *sin8x3 + \ 2 *sin9x3 + 2*expt *sin9x3 - 14*exp2t *sin9x3 - 2*exp3t *sin9x3 + \ exp4t *sin9x3)/(32*(-3 + cos2x3)*(9 - 12*cosx3 + cos2x3)**3)) x1abl= -expt x2abl= (-2*(-1 + expt)*(-6*sinx3 + sin2x3)*(-72*expt + 72*t + (91*expt - 96*t)*cosx3 - 8*(x3)*cos2x3 + \ expt*cos3x3 + 15*exp2t*sinx3 - exp2t*sin3x3) + (9 - 12*cosx3 + cos2x3)*(-72 + 72*expt + \ (96 - 91*expt + 15*exp2t - 15*exp3t)*cosx3 - 8*cos2x3 + 8*expt*cos2x3 + expt*(-1 - 3*expt + 3*exp2t)*\ cos3x3 - 91*expt*sinx3 + 61*exp2t*sinx3 + 96*t*sinx3 - 96*expt*t*sinx3 + 16*expt*sin2x3 - \ 16*exp2t*sin2x3 - 16*t*sin2x3 + 16*expt*t*sin2x3 - 3*expt*sin3x3 + 5*exp2t*sin3x3))/\ (4.*(9 - 12*cosx3 + cos2x3)**2) x3abl=-1. + expt x4abl=-expt x5abl= (expt*(89400 - 45072*expt + 52992*exp2t - 2*(70416 - 26663*expt + 34159*exp2t)*cosx3 + 12*(5723 + 142*expt + 610*exp2t)*\ cos2x3 - 19056*cos3x3 - 17454*expt*cos3x3 + 14118*exp2t*cos3x3 + 1800*cos4x3 + \ 10128*expt*cos4x3 - 9024*exp2t*cos4x3 + 144*cos5x3 - 2714*expt*cos5x3 + \ 2482*exp2t*cos5x3 - 4*cos6x3 + 216*expt*cos6x3 - 216*exp2t*cos6x3 - \ 6*expt*cos7x3 + 6*exp2t*cos7x3 + 1110*sinx3 - 43026*expt*sinx3 + 32074*exp2t*sinx3 - \ 16037*exp3t*sinx3 + 1512*sin2x3 + 34488*expt*sin2x3 - 25320*exp2t*sin2x3 + \ 12660*exp3t*sin2x3 - 3714*sin3x3 - 9106*expt*sin3x3 + 12786*exp2t*sin3x3 - \ 6393*exp3t*sin3x3 + 2400*sin4x3 - 1440*expt*sin4x3 - 5088*exp2t*sin4x3 + \ 2544*exp3t*sin4x3 - 730*sin5x3 + 1158*expt*sin5x3 + 1194*exp2t*sin5x3 - \ 597*exp3t*sin5x3 + 72*sin6x3 - 168*expt*sin6x3 - 72*exp2t*sin6x3 + \ 36*exp3t*sin6x3 - 2*sin7x3 + 6*expt*sin7x3 + 2*exp2t*sin7x3 - exp3t*sin7x3))/\ (16.*(9 - 12*cosx3 + cos2x3)**3) print 'term= ', (9 - 12*cosx3 + cos2x3) x6abl=expt x7abl=0.0 x8abl=0.0 if type(t) is algopy.utpm.utpm.UTPM: X=zeros(8,dtype=t) X[0]=x1 X[1]=x2 X[2]=x3 X[3]=x4 X[4]=x5 X[5]=x6 X[6]=U1 X[7]=U2 else: X=array([x1,x2,x3,x4,x5,x6,U1,U2]) Xabl=(x1abl,x2abl,x3abl,x4abl,x5abl,x6abl,x7abl,x8abl) return X,Xabl def RoboticArmSolutionCa88_1(t): X,Xabl=RoboticArmSolutionCa88(t) return X if __name__=="__main__": #from matplotlib import use # choose your backend #use('WxAgg') #use('Gtk3Agg') #use('TkAgg') #use('Qt4Agg') from numpy import shape,sqrt,arange from matplotlib.pyplot import plot,show,legend,figure,ylabel,xlabel,yscale,yticks tlist=npzeros(100) t0=0. T=2.4 #t0=1.5 #singularity of type sin x3 = 0 #T= 1.6 #t0=-6. #singularity of type cos x3 = 3- sqrt(5) #T= -5. steps=100 h=(T-t0)/steps tlist=arange(t0,T+h,h) #yxout,abl=RoboticArmSolution(tlist) yxout=RoboticArmSolutionCa88_1(tlist) print 'solution ',shape(yxout),yxout figure(0) #plot(tlist,yxout[0],label='x1') plot(tlist,yxout[1],label='x2') #plot(tlist,yxout[2],label='x3') #plot(tlist,yxout[3],label='x4') plot(tlist,yxout[4],label='x5') #plot(tlist,yxout[5],label='x6') plot(tlist,yxout[6],label='u1') plot(tlist,yxout[7],label='u2') legend(loc='upper left', shadow=True) ylabel('solution') xlabel('t') yscale('symlog') yticks([-10**8,-10**4,0,10**4,10**8]) figure(3) plot(tlist,yxout[0],label='x1') #plot(tlist,yxout[1],label='x2') plot(tlist,yxout[2],label='x3') plot(tlist,yxout[3],label='x4') #plot(tlist,yxout[4],label='x5') plot(tlist,yxout[5],label='x6') #plot(tlist,yxout[6],label='u1') #plot(tlist,yxout[7],label='u2') legend(loc='upper left', shadow=True) ylabel('solution') xlabel('t') figure(1) singularities_sin=1./sin(yxout[2]) plot(tlist,singularities_sin,label='1/(sin x3)') legend(loc='upper left', shadow=True) figure(2) singularities_cos=1./(cos(yxout[2])-(3.-sqrt(5))) plot(tlist,singularities_cos,label='1/(cos x3-(3-sqrt(5)))') legend(loc='upper left', shadow=True) show()