/* Generated 13-Jan-2003 14:06:15 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 ROADMAP (compass.sd) Bodies Inb No Name body Joint type Coords q Multipliers --- --------- ---- ----------- ---------------- ----------------------- -1 $ground | 0 left_leg -1 Planar 0 1 2 | 1 right_leg 0 Pin 3 | User Constraints 0 user_0 | 0 1 user_1 | 1 2 user_2 | 2 3 user_3 | 3 4 user_4 | 4 */ #include #include typedef struct { int ground_,nbod_,ndof_,ncons_,nloop_,nldof_,nloopc_,nball_,nlball_,npres_, nuser_; int jtype_[2],inb_[2],outb_[2],njntdof_[2],njntc_[2],njntp_[2],firstq_[2], ballq_[2],firstm_[2],firstp_[2]; int trans_[4],firstu_[5]; } sdgtopo_t; #define ground (sdgtopo.ground_) #define nbod (sdgtopo.nbod_) #define ndof (sdgtopo.ndof_) #define ncons (sdgtopo.ncons_) #define nloop (sdgtopo.nloop_) #define nldof (sdgtopo.nldof_) #define nloopc (sdgtopo.nloopc_) #define nball (sdgtopo.nball_) #define nlball (sdgtopo.nlball_) #define npres (sdgtopo.npres_) #define nuser (sdgtopo.nuser_) #define jtype (sdgtopo.jtype_) #define inb (sdgtopo.inb_) #define outb (sdgtopo.outb_) #define njntdof (sdgtopo.njntdof_) #define njntc (sdgtopo.njntc_) #define njntp (sdgtopo.njntp_) #define firstq (sdgtopo.firstq_) #define ballq (sdgtopo.ballq_) #define firstm (sdgtopo.firstm_) #define firstp (sdgtopo.firstp_) #define trans (sdgtopo.trans_) #define firstu (sdgtopo.firstu_) typedef struct { double grav_[3],mk_[2],ik_[2][3][3],pin_[4][3]; double rk_[2][3],ri_[2][3],pres_[4],stabvel_,stabpos_; int mfrcflg_,roustate_,vpkflg_,inerflg_,mmflg_,mmlduflg_,wwflg_,ltauflg_, fs0flg_,ii_,mmap_[4]; int gravq_[3],mkq_[2],ikq_[2][3][3],pinq_[4][3],rkq_[2][3],riq_[2][3],presq_ [4],stabvelq_,stabposq_; double mtot_,psmkg_,rhead_[2][3],rcom_[2][3],mkrcomt_[2][3][3],psikg_[3][3], psrcomg_[3],psrkg_[3],psrig_[3],psmk_[4],psik_[4][3][3],psrcom_[4][3], psrk_[4][3],psri_[4][3]; } sdginput_t; #define grav (sdginput.grav_) #define mk (sdginput.mk_) #define ik (sdginput.ik_) #define pin (sdginput.pin_) #define rk (sdginput.rk_) #define ri (sdginput.ri_) #define pres (sdginput.pres_) #define stabvel (sdginput.stabvel_) #define stabpos (sdginput.stabpos_) #define rhead (sdginput.rhead_) #define rcom (sdginput.rcom_) #define psrcomg (sdginput.psrcomg_) #define psrcom (sdginput.psrcom_) #define mkrcomt (sdginput.mkrcomt_) #define psmk (sdginput.psmk_) #define psik (sdginput.psik_) #define psrk (sdginput.psrk_) #define psri (sdginput.psri_) #define psmkg (sdginput.psmkg_) #define psikg (sdginput.psikg_) #define psrkg (sdginput.psrkg_) #define psrig (sdginput.psrig_) #define mtot (sdginput.mtot_) #define mfrcflg (sdginput.mfrcflg_) #define roustate (sdginput.roustate_) #define vpkflg (sdginput.vpkflg_) #define inerflg (sdginput.inerflg_) #define mmflg (sdginput.mmflg_) #define mmlduflg (sdginput.mmlduflg_) #define wwflg (sdginput.wwflg_) #define ltauflg (sdginput.ltauflg_) #define fs0flg (sdginput.fs0flg_) #define ii (sdginput.ii_) #define mmap (sdginput.mmap_) #define gravq (sdginput.gravq_) #define mkq (sdginput.mkq_) #define ikq (sdginput.ikq_) #define pinq (sdginput.pinq_) #define rkq (sdginput.rkq_) #define riq (sdginput.riq_) #define presq (sdginput.presq_) #define stabvelq (sdginput.stabvelq_) #define stabposq (sdginput.stabposq_) typedef struct { double curtim_,q_[4],qn_[4],u_[4],cnk_[4][3][3],cnb_[2][3][3]; double rnk_[4][3],vnk_[4][3],wk_[4][3],rnb_[2][3],vnb_[2][3],wb_[2][3], wbrcom_[2][3],com_[3],rnkg_[3]; double Cik_[4][3][3],rikt_[4][3][3],Iko_[4][3][3],mkrk_[4][3][3],Cib_[2][3][ 3]; double Wkk_[4][3],Vkk_[4][3],dik_[4][3],rpp_[4][3],rpk_[4][3],rik_[4][3], rik2_[4][3]; double rpri_[4][3],Wik_[4][3],Vik_[4][3],Wirk_[4][3],rkWkk_[4][3],Wkrpk_[4][ 3],VikWkr_[4][3]; double perr_[5],verr_[5],aerr_[5],mult_[5],ufk_[2][3],utk_[2][3],mfk_[2][3], mtk_[2][3]; double utau_[4],mtau_[4],uacc_[4],uvel_[4],upos_[4]; double s2_,c2_,s3_,c3_; } sdgstate_t; #define curtim (sdgstate.curtim_) #define q (sdgstate.q_) #define qn (sdgstate.qn_) #define u (sdgstate.u_) #define cnk (sdgstate.cnk_) #define cnb (sdgstate.cnb_) #define rnkg (sdgstate.rnkg_) #define rnk (sdgstate.rnk_) #define rnb (sdgstate.rnb_) #define vnk (sdgstate.vnk_) #define vnb (sdgstate.vnb_) #define wk (sdgstate.wk_) #define wb (sdgstate.wb_) #define com (sdgstate.com_) #define Cik (sdgstate.Cik_) #define Cib (sdgstate.Cib_) #define rikt (sdgstate.rikt_) #define Iko (sdgstate.Iko_) #define mkrk (sdgstate.mkrk_) #define Wkk (sdgstate.Wkk_) #define Vkk (sdgstate.Vkk_) #define dik (sdgstate.dik_) #define rpp (sdgstate.rpp_) #define rpk (sdgstate.rpk_) #define rik (sdgstate.rik_) #define rik2 (sdgstate.rik2_) #define rpri (sdgstate.rpri_) #define Wik (sdgstate.Wik_) #define Vik (sdgstate.Vik_) #define Wirk (sdgstate.Wirk_) #define rkWkk (sdgstate.rkWkk_) #define Wkrpk (sdgstate.Wkrpk_) #define VikWkr (sdgstate.VikWkr_) #define wbrcom (sdgstate.wbrcom_) #define perr (sdgstate.perr_) #define verr (sdgstate.verr_) #define aerr (sdgstate.aerr_) #define mult (sdgstate.mult_) #define ufk (sdgstate.ufk_) #define utk (sdgstate.utk_) #define utau (sdgstate.utau_) #define mfk (sdgstate.mfk_) #define mtk (sdgstate.mtk_) #define mtau (sdgstate.mtau_) #define uacc (sdgstate.uacc_) #define uvel (sdgstate.uvel_) #define upos (sdgstate.upos_) #define s2 (sdgstate.s2_) #define c2 (sdgstate.c2_) #define s3 (sdgstate.s3_) #define c3 (sdgstate.c3_) typedef struct { double fs0_[4],qdot_[4],Otk_[4][3],Atk_[4][3],AiOiWi_[4][3],Fstar_[4][3]; double Tstar_[4][3],Fstark_[4][3],Tstark_[4][3],IkWk_[4][3],WkIkWk_[4][3], gk_[4][3],IkbWk_[2][3],WkIkbWk_[2][3]; double w0w0_[2],w1w1_[2],w2w2_[2],w0w1_[2],w0w2_[2],w1w2_[2]; double w00w11_[2],w00w22_[2],w11w22_[2],ww_[5][5],qraux_[5]; double DD_[4],PH1_[4][3],PH2_[4][3],HL1_[4][3],HL2_[4][3],G1_[4][3],G2_[4][3 ]; double P11_[4][3][3],Pd_[4][3][3],P22_[4][3][3],L11_[4][3][3],L21_[4][3][3], L22_[4][3][3],D11_[4][3][3],D22_[4][3][3]; double N11_[4][3][3],N21_[4][3][3],N22_[4][3][3],psi11_[4][3][3],psi12_[4][3 ][3],psi21_[4][3][3],psi22_[4][3][3]; double psiD11_[4][3][3],psiD12_[4][3][3],psiD21_[4][3][3],psiD22_[4][3][3]; double sL11_[3][3],sL21_[3][3],sL22_[3][3],sD1_[3][3],sD2_[3][3]; double sD1INV_[3][3],sD2INV_[3][3],sL11D1_[3][3],sL22D2_[3][3],sD1L21_[3][3] ; double ping_[4][3],hngpt_[4][3]; int wmap_[5],multmap_[5],jpvt_[5],wsiz_,wrank_; } sdglhs_t; #define qdot (sdglhs.qdot_) #define Otk (sdglhs.Otk_) #define Atk (sdglhs.Atk_) #define AiOiWi (sdglhs.AiOiWi_) #define Fstar (sdglhs.Fstar_) #define Tstar (sdglhs.Tstar_) #define fs0 (sdglhs.fs0_) #define Fstark (sdglhs.Fstark_) #define Tstark (sdglhs.Tstark_) #define IkWk (sdglhs.IkWk_) #define IkbWk (sdglhs.IkbWk_) #define WkIkWk (sdglhs.WkIkWk_) #define WkIkbWk (sdglhs.WkIkbWk_) #define gk (sdglhs.gk_) #define w0w0 (sdglhs.w0w0_) #define w1w1 (sdglhs.w1w1_) #define w2w2 (sdglhs.w2w2_) #define w0w1 (sdglhs.w0w1_) #define w0w2 (sdglhs.w0w2_) #define w1w2 (sdglhs.w1w2_) #define w00w11 (sdglhs.w00w11_) #define w00w22 (sdglhs.w00w22_) #define w11w22 (sdglhs.w11w22_) #define ww (sdglhs.ww_) #define qraux (sdglhs.qraux_) #define PH1 (sdglhs.PH1_) #define PH2 (sdglhs.PH2_) #define P11 (sdglhs.P11_) #define Pd (sdglhs.Pd_) #define P22 (sdglhs.P22_) #define L11 (sdglhs.L11_) #define L21 (sdglhs.L21_) #define L22 (sdglhs.L22_) #define D11 (sdglhs.D11_) #define D22 (sdglhs.D22_) #define N11 (sdglhs.N11_) #define N21 (sdglhs.N21_) #define N22 (sdglhs.N22_) #define HL1 (sdglhs.HL1_) #define HL2 (sdglhs.HL2_) #define psi11 (sdglhs.psi11_) #define psi12 (sdglhs.psi12_) #define psi21 (sdglhs.psi21_) #define psi22 (sdglhs.psi22_) #define psiD11 (sdglhs.psiD11_) #define psiD12 (sdglhs.psiD12_) #define psiD21 (sdglhs.psiD21_) #define psiD22 (sdglhs.psiD22_) #define sL11 (sdglhs.sL11_) #define sL21 (sdglhs.sL21_) #define sL22 (sdglhs.sL22_) #define sD1 (sdglhs.sD1_) #define sD2 (sdglhs.sD2_) #define sD1INV (sdglhs.sD1INV_) #define sD2INV (sdglhs.sD2INV_) #define sL11D1 (sdglhs.sL11D1_) #define sL22D2 (sdglhs.sL22D2_) #define sD1L21 (sdglhs.sD1L21_) #define DD (sdglhs.DD_) #define G1 (sdglhs.G1_) #define G2 (sdglhs.G2_) #define ping (sdglhs.ping_) #define hngpt (sdglhs.hngpt_) #define wmap (sdglhs.wmap_) #define multmap (sdglhs.multmap_) #define jpvt (sdglhs.jpvt_) #define wsiz (sdglhs.wsiz_) #define wrank (sdglhs.wrank_) typedef struct { double fs_[4],udot_[4],tauc_[4],dyad_[2][3][3],fc_[4][3],tc_[4][3]; double ank_[4][3],onk_[4][3],Onkb_[4][3],AOnkri_[4][3],Ankb_[4][3],AnkAtk_[4 ][3],anb_[2][3],onb_[2][3],dyrcom_[2][3]; double ffk_[4][3],ttk_[4][3],fccikt_[4][3],ffkb_[2][3],ttkb_[2][3]; } sdgrhs_t; #define fs (sdgrhs.fs_) #define udot (sdgrhs.udot_) #define ank (sdgrhs.ank_) #define anb (sdgrhs.anb_) #define onk (sdgrhs.onk_) #define onb (sdgrhs.onb_) #define Onkb (sdgrhs.Onkb_) #define AOnkri (sdgrhs.AOnkri_) #define Ankb (sdgrhs.Ankb_) #define AnkAtk (sdgrhs.AnkAtk_) #define dyrcom (sdgrhs.dyrcom_) #define ffk (sdgrhs.ffk_) #define ttk (sdgrhs.ttk_) #define fccikt (sdgrhs.fccikt_) #define ffkb (sdgrhs.ffkb_) #define ttkb (sdgrhs.ttkb_) #define dyad (sdgrhs.dyad_) #define fc (sdgrhs.fc_) #define tc (sdgrhs.tc_) #define tauc (sdgrhs.tauc_) typedef struct { double temp_[3000],tmat1_[3][3],tmat2_[3][3],tvec1_[3],tvec2_[3],tvec3_[3], tvec4_[3],tvec5_[3]; double tsc1_,tsc2_,tsc3_; } sdgtemp_t; #define temp (sdgtemp.temp_) #define tmat1 (sdgtemp.tmat1_) #define tmat2 (sdgtemp.tmat2_) #define tvec1 (sdgtemp.tvec1_) #define tvec2 (sdgtemp.tvec2_) #define tvec3 (sdgtemp.tvec3_) #define tvec4 (sdgtemp.tvec4_) #define tvec5 (sdgtemp.tvec5_) #define tsc1 (sdgtemp.tsc1_) #define tsc2 (sdgtemp.tsc2_) #define tsc3 (sdgtemp.tsc3_) sdgtopo_t sdgtopo = { /* Topological information */ /* ground */ 1, /* nbod */ 2, /* ndof */ 4, /* ncons */ 5, /* nloop */ 0, /* nldof */ 0, /* nloopc */ 0, /* nball */ 0, /* nlball */ 0, /* npres */ 0, /* nuser */ 5, /* jtype[0] */ 8, /* jtype[1] */ 1, /* inb[0] */ -1, /* inb[1] */ 0, /* outb[0] */ 0, /* outb[1] */ 1, /* njntdof[0] */ 3, /* njntdof[1] */ 1, /* njntc[0] */ 0, /* njntc[1] */ 0, /* njntp[0] */ 0, /* njntp[1] */ 0, /* firstq[0] */ 0, /* firstq[1] */ 3, /* ballq[0] */ -104, /* ballq[1] */ -104, /* firstm[0] */ -1, /* firstm[1] */ -1, /* firstp[0] */ -1, /* firstp[1] */ -1, /* trans[0] */ 1, /* trans[1] */ 1, /* trans[2] */ 0, /* trans[3] */ 0, /* firstu[0] */ 0, /* firstu[1] */ 1, /* firstu[2] */ 2, /* firstu[3] */ 3, /* firstu[4] */ 4, }; sdginput_t sdginput = { /* Model parameters from the input file */ /* gravity */ /* grav[0] */ 0., /* grav[1] */ -9.81, /* grav[2] */ 0., /* mass */ /* mk[0] */ 37.527, /* mk[1] */ 37.527, /* inertia */ /* ik[0][0][0] */ 1.642, /* ik[0][0][1] */ 0., /* ik[0][0][2] */ 0., /* ik[0][1][0] */ 0., /* ik[0][1][1] */ .1, /* ik[0][1][2] */ 0., /* ik[0][2][0] */ 0., /* ik[0][2][1] */ 0., /* ik[0][2][2] */ 1.642, /* ik[1][0][0] */ 1.642, /* ik[1][0][1] */ 0., /* ik[1][0][2] */ 0., /* ik[1][1][0] */ 0., /* ik[1][1][1] */ .1, /* ik[1][1][2] */ 0., /* ik[1][2][0] */ 0., /* ik[1][2][1] */ 0., /* ik[1][2][2] */ 1.642, /* tree hinge axis vectors */ /* pin[0][0] */ 1., /* pin[0][1] */ 0., /* pin[0][2] */ 0., /* pin[1][0] */ 0., /* pin[1][1] */ 1., /* pin[1][2] */ 0., /* pin[2][0] */ 0., /* pin[2][1] */ 0., /* pin[2][2] */ 1., /* pin[3][0] */ 0., /* pin[3][1] */ 0., /* pin[3][2] */ 1., /* tree bodytojoint vectors */ /* rk[0][0] */ 0., /* rk[0][1] */ .1, /* rk[0][2] */ 0., /* rk[1][0] */ 0., /* rk[1][1] */ .1, /* rk[1][2] */ 0., /* tree inbtojoint vectors */ /* ri[0][0] */ 0., /* ri[0][1] */ 0., /* ri[0][2] */ 0., /* ri[1][0] */ 0., /* ri[1][1] */ .1, /* ri[1][2] */ 0., /* tree prescribed motion */ /* pres[0] */ 0., /* pres[1] */ 0., /* pres[2] */ 0., /* pres[3] */ 0., /* stabilization parameters */ /* stabvel */ 0., /* stabpos */ 0., /* miscellaneous */ /* mfrcflg */ 0, /* roustate */ 0, /* vpkflg */ 0, /* inerflg */ 0, /* mmflg */ 0, /* mmlduflg */ 0, /* wwflg */ 0, /* ltauflg */ 0, /* fs0flg */ 0, /* ii */ 0, /* mmap[0] */ 0, /* mmap[1] */ 1, /* mmap[2] */ 2, /* mmap[3] */ 3, /* Which parameters were "?" (1) or "?" (3) */ /* gravq[0] */ 3, /* gravq[1] */ 3, /* gravq[2] */ 3, /* mkq[0] */ 3, /* mkq[1] */ 3, /* ikq[0][0][0] */ 0, /* ikq[0][0][1] */ 0, /* ikq[0][0][2] */ 0, /* ikq[0][1][0] */ 0, /* ikq[0][1][1] */ 0, /* ikq[0][1][2] */ 0, /* ikq[0][2][0] */ 0, /* ikq[0][2][1] */ 0, /* ikq[0][2][2] */ 3, /* ikq[1][0][0] */ 0, /* ikq[1][0][1] */ 0, /* ikq[1][0][2] */ 0, /* ikq[1][1][0] */ 0, /* ikq[1][1][1] */ 0, /* ikq[1][1][2] */ 0, /* ikq[1][2][0] */ 0, /* ikq[1][2][1] */ 0, /* ikq[1][2][2] */ 3, /* pinq[0][0] */ 0, /* pinq[0][1] */ 0, /* pinq[0][2] */ 0, /* pinq[1][0] */ 0, /* pinq[1][1] */ 0, /* pinq[1][2] */ 0, /* pinq[2][0] */ 0, /* pinq[2][1] */ 0, /* pinq[2][2] */ 0, /* pinq[3][0] */ 0, /* pinq[3][1] */ 0, /* pinq[3][2] */ 0, /* rkq[0][0] */ 0, /* rkq[0][1] */ 3, /* rkq[0][2] */ 0, /* rkq[1][0] */ 0, /* rkq[1][1] */ 3, /* rkq[1][2] */ 0, /* riq[0][0] */ 0, /* riq[0][1] */ 0, /* riq[0][2] */ 0, /* riq[1][0] */ 0, /* riq[1][1] */ 3, /* riq[1][2] */ 0, /* presq[0] */ 0, /* presq[1] */ 0, /* presq[2] */ 0, /* presq[3] */ 0, /* stabvelq */ 3, /* stabposq */ 3, /* End of values from input file */ }; sdgstate_t sdgstate; sdglhs_t sdglhs; sdgrhs_t sdgrhs; sdgtemp_t sdgtemp; sdinit() { /* Initialization routine This routine must be called before the first call to sdstate(), after supplying values for any `?' parameters in the input. */ double sumsq,norminv; int i,j,k; /* Check that all `?' parameters have been assigned values */ for (k = 0; k < 3; k++) { if (gravq[k] == 1) { sdseterr(7,25); } } for (k = 0; k < 2; k++) { if (mkq[k] == 1) { sdseterr(7,26); } for (i = 0; i < 3; i++) { if (rkq[k][i] == 1) { sdseterr(7,29); } if (riq[k][i] == 1) { sdseterr(7,30); } for (j = 0; j < 3; j++) { if (ikq[k][i][j] == 1) { sdseterr(7,27); } } } } for (k = 0; k < 4; k++) { for (i = 0; i < 3; i++) { if (pinq[k][i] == 1) { sdseterr(7,28); } } } /* Normalize pin vectors if necessary */ /* Zero out ping and hngpt */ for (i = 0; i < 4; i++) { for (j = 0; j < 3; j++) { ping[i][j] = 0.; hngpt[i][j] = 0.; } } /* Compute pseudobody-related constants */ rcom[0][0] = 0.; rcom[0][1] = 0.; rcom[0][2] = 0.; rcom[1][0] = 0.; rcom[1][1] = 0.; rcom[1][2] = 0.; dik[3][1] = (ri[1][1]-rk[0][1]); /* Compute mass properties-related constants */ mtot = (mk[0]+mk[1]); mkrk[2][0][2] = (mk[0]*rk[0][1]); mkrk[2][2][0] = -(mk[0]*rk[0][1]); mkrk[3][0][2] = (mk[1]*rk[1][1]); mkrk[3][2][0] = -(mk[1]*rk[1][1]); Iko[2][0][0] = (1.642+(mkrk[2][0][2]*rk[0][1])); Iko[2][2][2] = (ik[0][2][2]-(mkrk[2][2][0]*rk[0][1])); Iko[3][0][0] = (1.642+(mkrk[3][0][2]*rk[1][1])); Iko[3][2][2] = (ik[1][2][2]-(mkrk[3][2][0]*rk[1][1])); sdserialno(&i); if (i != 70405) { sdseterr(7,41); } roustate = 1; } sdstate(timein,qin,uin) double timein,qin[4],uin[4]; { /* Compute kinematic information and store it in sdgstate. Generated 13-Jan-2003 14:06:15 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 */ int i,j,qchg,uchg; if ((roustate != 1) && (roustate != 2) && (roustate != 3)) { sdseterr(8,22); return; } if (roustate == 1) { for (i = 0; i < 4; i++) { if (presq[i] == 1) { sdseterr(8,31); } } } /* See if time or any qs have changed since last call */ if ((roustate != 1) && (timein == curtim)) { qchg = 0; for (i = 0; i < 4; i++) { if (qin[i] != q[i]) { qchg = 1; break; } } } else { qchg = 1; } /* If time and qs are unchanged, check us */ if (qchg == 0) { uchg = 0; for (i = 0; i < 4; i++) { if (uin[i] != u[i]) { uchg = 1; break; } } } else { uchg = 1; } curtim = timein; roustate = 2; if (qchg == 0) { goto skipqs; } /* Position-related variables need to be computed */ vpkflg = 0; mmflg = 0; mmlduflg = 0; wwflg = 0; for (i = 0; i < 4; i++) { q[i] = qin[i]; } /* Compute sines and cosines of q */ s2 = sin(q[2]); c2 = cos(q[2]); s3 = sin(q[3]); c3 = cos(q[3]); /* Compute across-axis direction cosines Cik */ /* Compute across-joint direction cosines Cib */ /* Compute gravity */ gk[2][0] = ((grav[0]*c2)+(grav[1]*s2)); gk[2][1] = ((grav[1]*c2)-(grav[0]*s2)); gk[3][0] = ((gk[2][0]*c3)+(gk[2][1]*s3)); gk[3][1] = ((gk[2][1]*c3)-(gk[2][0]*s3)); /* Compute cnk & cnb (direction cosines in N) */ cnk[3][0][0] = ((c2*c3)-(s2*s3)); cnk[3][0][1] = -((s2*c3)+(s3*c2)); cnk[3][1][0] = ((s2*c3)+(s3*c2)); cnk[3][1][1] = ((c2*c3)-(s2*s3)); cnb[0][0][0] = c2; cnb[0][0][1] = -s2; cnb[0][0][2] = 0.; cnb[0][1][0] = s2; cnb[0][1][1] = c2; cnb[0][1][2] = 0.; cnb[0][2][0] = 0.; cnb[0][2][1] = 0.; cnb[0][2][2] = 1.; cnb[1][0][0] = cnk[3][0][0]; cnb[1][0][1] = cnk[3][0][1]; cnb[1][0][2] = 0.; cnb[1][1][0] = cnk[3][1][0]; cnb[1][1][1] = cnk[3][1][1]; cnb[1][1][2] = 0.; cnb[1][2][0] = 0.; cnb[1][2][1] = 0.; cnb[1][2][2] = 1.; /* Compute q-related auxiliary variables */ rik[3][0] = (ri[1][1]*s3); rik[3][1] = ((ri[1][1]*c3)-rk[1][1]); rikt[3][2][0] = -(dik[3][1]*c3); rikt[3][2][1] = (dik[3][1]*s3); /* Compute rnk & rnb (mass center locations in N) */ rnk[2][0] = (q[0]+(rk[0][1]*s2)); rnk[2][1] = (q[1]-(rk[0][1]*c2)); rnk[3][0] = ((rnk[2][0]-(ri[1][1]*s2))-(cnk[3][0][1]*rk[1][1])); rnk[3][1] = ((rnk[2][1]+(ri[1][1]*c2))-(cnk[3][1][1]*rk[1][1])); rnb[0][0] = rnk[2][0]; rnb[0][1] = rnk[2][1]; rnb[0][2] = 0.; rnb[1][0] = rnk[3][0]; rnb[1][1] = rnk[3][1]; rnb[1][2] = 0.; /* Compute com (system mass center location in N) */ com[0] = ((1./mtot)*((mk[0]*rnk[2][0])+(mk[1]*rnk[3][0]))); com[1] = ((1./mtot)*((mk[0]*rnk[2][1])+(mk[1]*rnk[3][1]))); com[2] = 0.; /* Compute constraint position errors */ skipqs: ; sduperr(curtim,q,&perr[0]); if (uchg == 0) { goto skipus; } /* Velocity-related variables need to be computed */ inerflg = 0; for (i = 0; i < 4; i++) { u[i] = uin[i]; } /* Compute u-related auxiliary variables */ /* Compute wk & wb (angular velocities) */ wk[3][2] = (u[2]+u[3]); wb[0][0] = 0.; wb[0][1] = 0.; wb[0][2] = u[2]; wb[1][0] = 0.; wb[1][1] = 0.; wb[1][2] = wk[3][2]; /* Compute auxiliary variables involving wk */ Wirk[3][0] = -(ri[1][1]*u[2]); Wkrpk[2][0] = (rk[0][1]*u[2]); Wkrpk[3][0] = (rk[1][1]*wk[3][2]); IkWk[2][2] = (ik[0][2][2]*u[2]); IkWk[3][2] = (ik[1][2][2]*wk[3][2]); /* Compute temporaries for use in SDRHS */ w2w2[0] = (u[2]*u[2]); w2w2[1] = (wk[3][2]*wk[3][2]); /* Compute vnk & vnb (mass center linear velocities in N) */ vnk[2][0] = (u[0]+(Wkrpk[2][0]*c2)); vnk[2][1] = (u[1]+(Wkrpk[2][0]*s2)); vnk[3][0] = ((cnk[3][0][0]*Wkrpk[3][0])+(vnk[2][0]+(Wirk[3][0]*c2))); vnk[3][1] = ((cnk[3][1][0]*Wkrpk[3][0])+(vnk[2][1]+(Wirk[3][0]*s2))); vnb[0][0] = vnk[2][0]; vnb[0][1] = vnk[2][1]; vnb[0][2] = 0.; vnb[1][0] = vnk[3][0]; vnb[1][1] = vnk[3][1]; vnb[1][2] = 0.; /* Compute qdot (kinematical equations) */ qdot[0] = u[0]; qdot[1] = u[1]; qdot[2] = u[2]; qdot[3] = u[3]; /* Compute constraint velocity errors */ skipus: ; sduverr(curtim,q,u,&verr[0]); /* Initialize applied forces and torques to zero */ for (i = 0; i < 2; i++) { for (j = 0; j < 3; j++) { ufk[i][j] = 0.; utk[i][j] = 0.; } } for (i = 0; i < 4; i++) { utau[i] = 0.; } ltauflg = 0; fs0flg = 0; /* Used 0.05 seconds CPU time, 8192 additional bytes of memory. Equations contain 28 adds/subtracts/negates 45 multiplies 2 divides 95 assignments */ } sdqdot(oqdot) double oqdot[4]; { /* Return position coordinate derivatives for tree joints. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(63,23); return; } for (i = 0; i <= 3; i++) { oqdot[i] = qdot[i]; } } sdu2qdot(uin,oqdot) double uin[4],oqdot[4]; { /* Convert velocities to qdots. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(64,23); return; } for (i = 0; i <= 3; i++) { oqdot[i] = uin[i]; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 4 assignments */ } sdpsstate(lqin) double lqin[1]; { if (roustate != 2) { sdseterr(9,23); return; } } sddoping() { if (vpkflg == 0) { /* Compute ping (jt pins in ground frame) */ ping[0][0] = 1.; ping[1][1] = 1.; ping[2][2] = 1.; ping[3][2] = 1.; /* Compute hngpt (hinge pts in ground frame) */ hngpt[0][0] = q[0]; hngpt[1][0] = q[0]; hngpt[1][1] = q[1]; hngpt[2][0] = (rnk[2][0]-(rk[0][1]*s2)); hngpt[2][1] = (rnk[2][1]+(rk[0][1]*c2)); hngpt[3][0] = (rnk[3][0]+(cnk[3][0][1]*rk[1][1])); hngpt[3][1] = (rnk[3][1]+(cnk[3][1][1]*rk[1][1])); vpkflg = 1; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 4 adds/subtracts/negates 4 multiplies 0 divides 11 assignments */ } sddoltau() { /* Compute effect of loop hinge torques */ /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 0 assignments */ } sddoiner() { /* Compute inertial accelerations and related temps */ if (inerflg == 0) { /* Compute Otk (inertial angular acceleration) */ /* Compute Atk (inertial linear acceleration) */ Atk[2][1] = (u[2]*Wkrpk[2][0]); AiOiWi[3][1] = (Atk[2][1]+(u[2]*Wirk[3][0])); Atk[3][0] = (AiOiWi[3][1]*s3); Atk[3][1] = ((AiOiWi[3][1]*c3)+(wk[3][2]*Wkrpk[3][0])); inerflg = 1; } /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 2 adds/subtracts/negates 5 multiplies 0 divides 4 assignments */ } sddofs0() { /* Compute effect of all applied loads */ if (fs0flg == 0) { sddoltau(); sddoiner(); /* Compute Fstar (forces) */ Fstar[2][0] = -(ufk[0][0]+(gk[2][0]*mk[0])); Fstar[2][1] = ((mk[0]*(Atk[2][1]-gk[2][1]))-ufk[0][1]); Fstar[2][2] = -(ufk[0][2]+(grav[2]*mk[0])); Fstar[3][0] = ((mk[1]*(Atk[3][0]-gk[3][0]))-ufk[1][0]); Fstar[3][1] = ((mk[1]*(Atk[3][1]-gk[3][1]))-ufk[1][1]); Fstar[3][2] = -(ufk[1][2]+(grav[2]*mk[1])); /* Compute Tstar (torques) */ /* Compute fs0 (RHS ignoring constraints) */ fs0[3] = (utau[3]-((Fstar[3][0]*rk[1][1])-utk[1][2])); Fstark[2][0] = (Fstar[2][0]+((Fstar[3][0]*c3)-(Fstar[3][1]*s3))); Fstark[2][1] = (Fstar[2][1]+((Fstar[3][0]*s3)+(Fstar[3][1]*c3))); Fstark[2][2] = (Fstar[2][2]+Fstar[3][2]); tvec1[0] = ((Fstar[3][2]*rik[3][1])-utk[1][0]); tvec1[1] = -(utk[1][1]+(Fstar[3][2]*rik[3][0])); tvec1[2] = (((Fstar[3][1]*rik[3][0])-(Fstar[3][0]*rik[3][1]))-utk[1][2]) ; Tstark[2][0] = (((tvec1[0]*c3)-(tvec1[1]*s3))-utk[0][0]); Tstark[2][1] = (((tvec1[0]*s3)+(tvec1[1]*c3))-utk[0][1]); Tstark[2][2] = (tvec1[2]-utk[0][2]); fs0[2] = (utau[2]-(Tstark[2][2]+(Fstark[2][0]*rk[0][1]))); Fstark[1][0] = ((Fstark[2][0]*c2)-(Fstark[2][1]*s2)); Fstark[1][1] = ((Fstark[2][0]*s2)+(Fstark[2][1]*c2)); Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][2]*rk[0][1])); tvec1[2] = (Tstark[2][2]+(Fstark[2][0]*rk[0][1])); Tstark[1][0] = ((tvec1[0]*c2)-(Tstark[2][1]*s2)); Tstark[1][1] = ((Tstark[2][1]*c2)+(tvec1[0]*s2)); Tstark[1][2] = tvec1[2]; fs0[1] = (utau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; fs0[0] = (utau[0]-Fstark[0][0]); fs0flg = 1; } /* Used 0.02 seconds CPU time, 8192 additional bytes of memory. Equations contain 41 adds/subtracts/negates 32 multiplies 0 divides 35 assignments */ } sddomm(routine) int routine; { if (mmflg == 0) { /* Compute gains (DD, G) */ if (Iko[3][2][2] < 1e-13) { sdseterr(routine,47); } else { DD[3] = (1./Iko[3][2][2]); } G1[3][0] = (DD[3]*mkrk[3][0][2]); P11[3][0][0] = (mk[1]-(G1[3][0]*mkrk[3][0][2])); sD1INV[0][0] = 0.; if (P11[3][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[3][0][0]); } sD1INV[1][1] = 0.; if (mk[1] >= 1e-13) { sD1INV[1][1] = (1./mk[1]); } sD1INV[2][2] = 0.; if (mk[1] >= 1e-13) { sD1INV[2][2] = (1./mk[1]); } sL21[0][2] = (mkrk[3][2][0]*sD1INV[2][2]); sL22D2[0][0] = (Iko[3][0][0]-(mkrk[3][2][0]*sL21[0][2])); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } N21[3][0][2] = (dik[3][1]+(sL21[0][2]*c3)); N21[3][1][2] = (sL21[0][2]*s3); psiD11[3][0][0] = (P11[3][0][0]*c3); psiD11[3][0][1] = -(mk[1]*s3); psiD11[3][1][0] = (P11[3][0][0]*s3); psiD11[3][1][1] = (mk[1]*c3); psiD21[3][0][2] = (mk[1]*N21[3][0][2]); psiD21[3][1][2] = (mk[1]*N21[3][1][2]); psiD21[3][2][0] = (P11[3][0][0]*rikt[3][2][0]); psiD21[3][2][1] = (mk[1]*rikt[3][2][1]); psiD22[3][0][0] = (sL22D2[0][0]*c3); psiD22[3][1][0] = (sL22D2[0][0]*s3); P11[2][0][0] = (mk[0]+((psiD11[3][0][0]*c3)-(psiD11[3][0][1]*s3))); P11[2][0][1] = ((psiD11[3][0][0]*s3)+(psiD11[3][0][1]*c3)); P11[2][1][0] = P11[2][0][1]; P11[2][1][1] = (mk[0]+((psiD11[3][1][0]*s3)+(psiD11[3][1][1]*c3))); P11[2][2][2] = (mk[0]+mk[1]); Pd[2][0][2] = (mkrk[2][0][2]+((psiD11[3][0][0]*rikt[3][2][0])+( psiD11[3][0][1]*rikt[3][2][1]))); Pd[2][1][2] = ((psiD11[3][1][0]*rikt[3][2][0])+(psiD11[3][1][1]* rikt[3][2][1])); Pd[2][2][0] = (mkrk[2][2][0]+(mk[1]*N21[3][0][2])); Pd[2][2][1] = (mk[1]*N21[3][1][2]); P22[2][0][0] = (Iko[2][0][0]+((N21[3][0][2]*psiD21[3][0][2])+((.1*(s3*s3 ))+(psiD22[3][0][0]*c3)))); P22[2][0][1] = ((N21[3][1][2]*psiD21[3][0][2])+((psiD22[3][0][0]*s3)-(.1 *(s3*c3)))); P22[2][1][0] = P22[2][0][1]; P22[2][1][1] = (.1+((N21[3][1][2]*psiD21[3][1][2])+((.1*(c3*c3))+( psiD22[3][1][0]*s3)))); P22[2][2][2] = (Iko[2][2][2]+((psiD21[3][2][0]*rikt[3][2][0])+( psiD21[3][2][1]*rikt[3][2][1]))); if (P22[2][2][2] < 1e-13) { sdseterr(routine,47); } else { DD[2] = (1./P22[2][2][2]); } G1[2][0] = (DD[2]*Pd[2][0][2]); G1[2][1] = (DD[2]*Pd[2][1][2]); P11[2][0][0] = (P11[2][0][0]-(G1[2][0]*Pd[2][0][2])); P11[2][0][1] = (P11[2][0][1]-(G1[2][1]*Pd[2][0][2])); P11[2][1][1] = (P11[2][1][1]-(G1[2][1]*Pd[2][1][2])); sD1INV[0][0] = 0.; if (P11[2][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[2][0][0]); } sL11[1][0] = (P11[2][0][1]*sD1INV[0][0]); sL11D1[1][1] = (P11[2][1][1]-(P11[2][0][1]*sL11[1][0])); sD1INV[1][1] = 0.; if (sL11D1[1][1] >= 1e-13) { sD1INV[1][1] = (1./sL11D1[1][1]); } sD1INV[2][2] = 0.; if (P11[2][2][2] >= 1e-13) { sD1INV[2][2] = (1./P11[2][2][2]); } sL21[0][2] = (Pd[2][2][0]*sD1INV[2][2]); sL21[1][2] = (Pd[2][2][1]*sD1INV[2][2]); sL22D2[0][0] = (P22[2][0][0]-(Pd[2][2][0]*sL21[0][2])); sL22D2[1][0] = (P22[2][0][1]-(Pd[2][2][0]*sL21[1][2])); sL22D2[1][1] = (P22[2][1][1]-(Pd[2][2][1]*sL21[1][2])); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[1][0] = (sD2INV[0][0]*sL22D2[1][0]); sL22D2[1][1] = (sL22D2[1][1]-(sL22[1][0]*sL22D2[1][0])); sD2INV[1][1] = 0.; if (sL22D2[1][1] >= 1e-13) { sD2INV[1][1] = (1./sL22D2[1][1]); } N11[2][0][0] = (c2-(sL11[1][0]*s2)); N11[2][1][0] = (s2+(sL11[1][0]*c2)); N21[2][0][2] = ((sL21[0][2]*c2)-(sL21[1][2]*s2)); N21[2][1][2] = ((sL21[0][2]*s2)+(sL21[1][2]*c2)); N22[2][0][0] = (c2-(sL22[1][0]*s2)); N22[2][1][0] = (s2+(sL22[1][0]*c2)); psiD11[2][0][0] = (N11[2][0][0]*P11[2][0][0]); psiD11[2][0][1] = -(sL11D1[1][1]*s2); psiD11[2][1][0] = (N11[2][1][0]*P11[2][0][0]); psiD11[2][1][1] = (sL11D1[1][1]*c2); psiD21[2][0][2] = (N21[2][0][2]*P11[2][2][2]); psiD21[2][1][2] = (N21[2][1][2]*P11[2][2][2]); psiD22[2][0][0] = (N22[2][0][0]*sL22D2[0][0]); psiD22[2][0][1] = -(sL22D2[1][1]*s2); psiD22[2][1][0] = (N22[2][1][0]*sL22D2[0][0]); psiD22[2][1][1] = (sL22D2[1][1]*c2); P11[1][0][0] = ((N11[2][0][0]*psiD11[2][0][0])-(psiD11[2][0][1]*s2)); P11[1][0][1] = ((N11[2][1][0]*psiD11[2][0][0])+(psiD11[2][0][1]*c2)); P11[1][1][0] = P11[1][0][1]; P11[1][1][1] = ((N11[2][1][0]*psiD11[2][1][0])+(psiD11[2][1][1]*c2)); P11[1][2][2] = P11[2][2][2]; Pd[1][2][0] = (N21[2][0][2]*P11[2][2][2]); Pd[1][2][1] = (N21[2][1][2]*P11[2][2][2]); P22[1][0][0] = ((N21[2][0][2]*psiD21[2][0][2])+((N22[2][0][0]* psiD22[2][0][0])-(psiD22[2][0][1]*s2))); P22[1][0][1] = ((N21[2][1][2]*psiD21[2][0][2])+((N22[2][1][0]* psiD22[2][0][0])+(psiD22[2][0][1]*c2))); P22[1][1][0] = P22[1][0][1]; P22[1][1][1] = ((N21[2][1][2]*psiD21[2][1][2])+((N22[2][1][0]* psiD22[2][1][0])+(psiD22[2][1][1]*c2))); if (P11[1][1][1] < 1e-13) { sdseterr(routine,47); } else { DD[1] = (1./P11[1][1][1]); } G1[1][0] = (DD[1]*P11[1][0][1]); P11[1][0][0] = (P11[1][0][0]-(G1[1][0]*P11[1][0][1])); sD1INV[0][0] = 0.; if (P11[1][0][0] >= 1e-13) { sD1INV[0][0] = (1./P11[1][0][0]); } sD1INV[2][2] = 0.; if (P11[1][2][2] >= 1e-13) { sD1INV[2][2] = (1./P11[1][2][2]); } sL21[0][2] = (Pd[1][2][0]*sD1INV[2][2]); sL21[1][2] = (Pd[1][2][1]*sD1INV[2][2]); sL22D2[0][0] = (P22[1][0][0]-(Pd[1][2][0]*sL21[0][2])); sL22D2[1][0] = (P22[1][0][1]-(Pd[1][2][0]*sL21[1][2])); sL22D2[1][1] = (P22[1][1][1]-(Pd[1][2][1]*sL21[1][2])); sD2INV[0][0] = 0.; if (sL22D2[0][0] >= 1e-13) { sD2INV[0][0] = (1./sL22D2[0][0]); } sL22[1][0] = (sD2INV[0][0]*sL22D2[1][0]); sL22D2[1][1] = (sL22D2[1][1]-(sL22[1][0]*sL22D2[1][0])); sD2INV[1][1] = 0.; if (sL22D2[1][1] >= 1e-13) { sD2INV[1][1] = (1./sL22D2[1][1]); } N21[1][0][2] = (q[1]+sL21[0][2]); psiD21[1][0][2] = (N21[1][0][2]*P11[1][2][2]); psiD21[1][1][2] = (P11[1][2][2]*sL21[1][2]); psiD21[1][2][0] = -(P11[1][0][0]*q[1]); psiD22[1][1][0] = (sL22[1][0]*sL22D2[0][0]); P11[0][0][0] = P11[1][0][0]; P11[0][2][2] = P11[1][2][2]; Pd[0][0][2] = -(P11[1][0][0]*q[1]); Pd[0][2][0] = (N21[1][0][2]*P11[1][2][2]); Pd[0][2][1] = (P11[1][2][2]*sL21[1][2]); P22[0][0][0] = (sL22D2[0][0]+(N21[1][0][2]*psiD21[1][0][2])); P22[0][0][1] = ((psiD21[1][0][2]*sL21[1][2])+(sL22[1][0]*sL22D2[0][0])); P22[0][1][0] = P22[0][0][1]; P22[0][1][1] = ((psiD21[1][1][2]*sL21[1][2])+(sL22D2[1][1]+( psiD22[1][1][0]*sL22[1][0]))); P22[0][2][2] = -(psiD21[1][2][0]*q[1]); if (P11[0][0][0] < 1e-13) { sdseterr(routine,47); } else { DD[0] = (1./P11[0][0][0]); } mmflg = 1; } /* Used 0.11 seconds CPU time, 16384 additional bytes of memory. Equations contain 62 adds/subtracts/negates 113 multiplies 17 divides 125 assignments */ } sdlhs(routine) int routine; { /* Compute all remaining state- and force-dependent quantities */ roustate = 2; sddomm(routine); sddofs0(); } sdmassmat(mmat) double mmat[4][4]; { /* Calculate the system mass matrix */ int i,j; double udotin[4],mmrow[4],biastrq[4]; if ((roustate != 2) && (roustate != 3)) { sdseterr(57,23); return; } for (i = 0; i < 4; i++) { udotin[i] = 0.; } sdcomptrq(udotin,biastrq); for (i = 0; i < 4; i++) { udotin[i] = 1.; sdcomptrq(udotin,mmrow); udotin[i] = 0.; for (j = i; j <= 3; j++) { mmat[i][j] = mmrow[j]-biastrq[j]; mmat[j][i] = mmat[i][j]; } } /* Check for singular mass matrix */ for (i = 0; i < 4; i++) { if (mmat[i][i] <= 1e-13) { sdseterr(57,47); } } } sdfrcmat(fmat) double fmat[4]; { /* Return the system force matrix (RHS), excluding constraints */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(58,23); return; } sddofs0(); for (i = 0; i < 4; i++) { fmat[i] = fs0[i]; } } sdmfrc(imult) double imult[5]; { /* Calculate forces due to constraint multipliers. */ int i,j; double umult[5]; /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = imult[0]; umult[1] = imult[1]; umult[2] = imult[2]; umult[3] = imult[3]; umult[4] = imult[4]; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 21 assignments */ } sdequivht(tau) double tau[4]; { /* Compute tree hinge torques to match effect of applied loads */ double fstareq[4][3],tstareq[4][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(56,23); return; } /* Compute fstareq (forces) */ fstareq[2][0] = -(ufk[0][0]+(gk[2][0]*mk[0])); fstareq[2][1] = -(ufk[0][1]+(gk[2][1]*mk[0])); fstareq[2][2] = -(ufk[0][2]+(grav[2]*mk[0])); fstareq[3][0] = -(ufk[1][0]+(gk[3][0]*mk[1])); fstareq[3][1] = -(ufk[1][1]+(gk[3][1]*mk[1])); fstareq[3][2] = -(ufk[1][2]+(grav[2]*mk[1])); /* Compute tstareq (torques) */ /* Compute taus (RHS ignoring constraints and inertial forces) */ tau[3] = (utau[3]-((fstareq[3][0]*rk[1][1])-utk[1][2])); Fstark[2][0] = (fstareq[2][0]+((fstareq[3][0]*c3)-(fstareq[3][1]*s3))); Fstark[2][1] = (fstareq[2][1]+((fstareq[3][0]*s3)+(fstareq[3][1]*c3))); Fstark[2][2] = (fstareq[2][2]+fstareq[3][2]); tvec1[0] = ((fstareq[3][2]*rik[3][1])-utk[1][0]); tvec1[1] = -(utk[1][1]+(fstareq[3][2]*rik[3][0])); tvec1[2] = (((fstareq[3][1]*rik[3][0])-(fstareq[3][0]*rik[3][1]))-utk[1][2]) ; Tstark[2][0] = (((tvec1[0]*c3)-(tvec1[1]*s3))-utk[0][0]); Tstark[2][1] = (((tvec1[0]*s3)+(tvec1[1]*c3))-utk[0][1]); Tstark[2][2] = (tvec1[2]-utk[0][2]); tau[2] = (utau[2]-(Tstark[2][2]+(Fstark[2][0]*rk[0][1]))); Fstark[1][0] = ((Fstark[2][0]*c2)-(Fstark[2][1]*s2)); Fstark[1][1] = ((Fstark[2][0]*s2)+(Fstark[2][1]*c2)); Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][2]*rk[0][1])); tvec1[2] = (Tstark[2][2]+(Fstark[2][0]*rk[0][1])); Tstark[1][0] = ((tvec1[0]*c2)-(Tstark[2][1]*s2)); Tstark[1][1] = ((Tstark[2][1]*c2)+(tvec1[0]*s2)); Tstark[1][2] = tvec1[2]; tau[1] = (utau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; tau[0] = (utau[0]-Fstark[0][0]); /* Op counts below do not include called subroutines */ /* Used 0.02 seconds CPU time, 0 additional bytes of memory. Equations contain 41 adds/subtracts/negates 32 multiplies 0 divides 35 assignments */ } sdfulltrq(udotin,multin,trqout) double udotin[4],multin[5],trqout[4]; { /* Compute hinge torques which would produce indicated udots */ double fstarr[4][3],tstarr[4][3],Otkr[4][3],Atir[4][3],Atkr[4][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(61,23); return; } /* Compute multiplier-generated forces */ sdmfrc(multin); /* Account for inertial accelerations and supplied udots */ Otkr[3][2] = (udotin[2]+udotin[3]); Atkr[2][0] = ((rk[0][1]*udotin[2])+((udotin[0]*c2)+(udotin[1]*s2))); Atkr[2][1] = ((u[2]*Wkrpk[2][0])+((udotin[1]*c2)-(udotin[0]*s2))); Atir[3][0] = (Atkr[2][0]-(ri[1][1]*udotin[2])); Atir[3][1] = (Atkr[2][1]+(u[2]*Wirk[3][0])); Atkr[3][0] = ((Otkr[3][2]*rk[1][1])+((Atir[3][0]*c3)+(Atir[3][1]*s3))); Atkr[3][1] = ((wk[3][2]*Wkrpk[3][0])+((Atir[3][1]*c3)-(Atir[3][0]*s3))); /* Accumulate all forces and torques */ fstarr[2][0] = ((mfk[0][0]+ufk[0][0])+(mk[0]*(gk[2][0]-Atkr[2][0]))); fstarr[2][1] = ((mfk[0][1]+ufk[0][1])+(mk[0]*(gk[2][1]-Atkr[2][1]))); fstarr[2][2] = ((grav[2]*mk[0])+(mfk[0][2]+ufk[0][2])); fstarr[3][0] = ((mfk[1][0]+ufk[1][0])+(mk[1]*(gk[3][0]-Atkr[3][0]))); fstarr[3][1] = ((mfk[1][1]+ufk[1][1])+(mk[1]*(gk[3][1]-Atkr[3][1]))); fstarr[3][2] = ((grav[2]*mk[1])+(mfk[1][2]+ufk[1][2])); tstarr[2][0] = (mtk[0][0]+utk[0][0]); tstarr[2][1] = (mtk[0][1]+utk[0][1]); tstarr[2][2] = ((mtk[0][2]+utk[0][2])-(ik[0][2][2]*udotin[2])); tstarr[3][0] = (mtk[1][0]+utk[1][0]); tstarr[3][1] = (mtk[1][1]+utk[1][1]); tstarr[3][2] = ((mtk[1][2]+utk[1][2])-(ik[1][2][2]*Otkr[3][2])); /* Now calculate the torques */ trqout[3] = -((mtau[3]+utau[3])+(tstarr[3][2]+(fstarr[3][0]*rk[1][1]))); Fstark[2][0] = (fstarr[2][0]+((fstarr[3][0]*c3)-(fstarr[3][1]*s3))); Fstark[2][1] = (fstarr[2][1]+((fstarr[3][0]*s3)+(fstarr[3][1]*c3))); Fstark[2][2] = (fstarr[2][2]+fstarr[3][2]); tvec1[0] = (tstarr[3][0]+(fstarr[3][2]*rik[3][1])); tvec1[1] = (tstarr[3][1]-(fstarr[3][2]*rik[3][0])); tvec1[2] = (tstarr[3][2]+((fstarr[3][1]*rik[3][0])-(fstarr[3][0]*rik[3][1])) ); Tstark[2][0] = (tstarr[2][0]+((tvec1[0]*c3)-(tvec1[1]*s3))); Tstark[2][1] = (tstarr[2][1]+((tvec1[0]*s3)+(tvec1[1]*c3))); Tstark[2][2] = (tstarr[2][2]+tvec1[2]); trqout[2] = -((mtau[2]+utau[2])+(Tstark[2][2]+(Fstark[2][0]*rk[0][1]))); Fstark[1][0] = ((Fstark[2][0]*c2)-(Fstark[2][1]*s2)); Fstark[1][1] = ((Fstark[2][0]*s2)+(Fstark[2][1]*c2)); Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][2]*rk[0][1])); tvec1[2] = (Tstark[2][2]+(Fstark[2][0]*rk[0][1])); Tstark[1][0] = ((tvec1[0]*c2)-(Tstark[2][1]*s2)); Tstark[1][1] = ((Tstark[2][1]*c2)+(tvec1[0]*s2)); Tstark[1][2] = tvec1[2]; trqout[1] = -(Fstark[1][1]+(mtau[1]+utau[1])); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; trqout[0] = -(Fstark[0][0]+(mtau[0]+utau[0])); /* Op counts below do not include called subroutines */ /* Used 0.03 seconds CPU time, 0 additional bytes of memory. Equations contain 71 adds/subtracts/negates 48 multiplies 0 divides 48 assignments */ } sdcomptrq(udotin,trqout) double udotin[4],trqout[4]; { /* Compute hinge torques to produce these udots, ignoring constraints */ int i; double multin[5]; if ((roustate != 2) && (roustate != 3)) { sdseterr(60,23); return; } for (i = 0; i < 5; i++) { multin[i] = 0.; } sdfulltrq(udotin,multin,trqout); } sdmulttrq(multin,trqout) double multin[5],trqout[4]; { /* Compute hinge trqs which would be produced by these mults. */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(65,23); return; } sdmfrc(multin); sdfsmult(); for (i = 0; i < 4; i++) { trqout[i] = fs[i]; } } sdfs0() { /* Compute Fs (ignoring multiplier forces) */ fs[0] = fs0[0]; fs[1] = fs0[1]; fs[2] = fs0[2]; fs[3] = fs0[3]; /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 0 adds/subtracts/negates 0 multiplies 0 divides 4 assignments */ } sdfsmult() { /* Compute Fs (multiplier-generated forces only) */ fs[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); Fstark[2][0] = (((mfk[1][1]*s3)-(mfk[1][0]*c3))-mfk[0][0]); Fstark[2][1] = -(mfk[0][1]+((mfk[1][0]*s3)+(mfk[1][1]*c3))); Fstark[2][2] = -(mfk[0][2]+mfk[1][2]); tvec1[0] = -(mtk[1][0]+(mfk[1][2]*rik[3][1])); tvec1[1] = ((mfk[1][2]*rik[3][0])-mtk[1][1]); tvec1[2] = (((mfk[1][0]*rik[3][1])-(mfk[1][1]*rik[3][0]))-mtk[1][2]); Tstark[2][0] = (((tvec1[0]*c3)-(tvec1[1]*s3))-mtk[0][0]); Tstark[2][1] = (((tvec1[0]*s3)+(tvec1[1]*c3))-mtk[0][1]); Tstark[2][2] = (tvec1[2]-mtk[0][2]); fs[2] = (mtau[2]-(Tstark[2][2]+(Fstark[2][0]*rk[0][1]))); Fstark[1][0] = ((Fstark[2][0]*c2)-(Fstark[2][1]*s2)); Fstark[1][1] = ((Fstark[2][0]*s2)+(Fstark[2][1]*c2)); Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][2]*rk[0][1])); tvec1[2] = (Tstark[2][2]+(Fstark[2][0]*rk[0][1])); Tstark[1][0] = ((tvec1[0]*c2)-(Tstark[2][1]*s2)); Tstark[1][1] = ((Tstark[2][1]*c2)+(tvec1[0]*s2)); Tstark[1][2] = tvec1[2]; fs[1] = (mtau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; fs[0] = (mtau[0]-Fstark[0][0]); /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 31 adds/subtracts/negates 26 multiplies 0 divides 29 assignments */ } sdfsfull() { /* Compute Fs (including all forces) */ sdfsmult(); fs[0] = (fs[0]+fs0[0]); fs[1] = (fs[1]+fs0[1]); fs[2] = (fs[2]+fs0[2]); fs[3] = (fs[3]+fs0[3]); /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 4 adds/subtracts/negates 0 multiplies 0 divides 4 assignments */ } sdfsgenmult() { /* Compute Fs (generic multiplier-generated forces) */ fs[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); Fstark[2][0] = (((mfk[1][1]*s3)-(mfk[1][0]*c3))-mfk[0][0]); Fstark[2][1] = -(mfk[0][1]+((mfk[1][0]*s3)+(mfk[1][1]*c3))); Fstark[2][2] = -(mfk[0][2]+mfk[1][2]); tvec1[0] = -(mtk[1][0]+(mfk[1][2]*rik[3][1])); tvec1[1] = ((mfk[1][2]*rik[3][0])-mtk[1][1]); tvec1[2] = (((mfk[1][0]*rik[3][1])-(mfk[1][1]*rik[3][0]))-mtk[1][2]); Tstark[2][0] = (((tvec1[0]*c3)-(tvec1[1]*s3))-mtk[0][0]); Tstark[2][1] = (((tvec1[0]*s3)+(tvec1[1]*c3))-mtk[0][1]); Tstark[2][2] = (tvec1[2]-mtk[0][2]); fs[2] = (mtau[2]-(Tstark[2][2]+(Fstark[2][0]*rk[0][1]))); Fstark[1][0] = ((Fstark[2][0]*c2)-(Fstark[2][1]*s2)); Fstark[1][1] = ((Fstark[2][0]*s2)+(Fstark[2][1]*c2)); Fstark[1][2] = Fstark[2][2]; tvec1[0] = (Tstark[2][0]-(Fstark[2][2]*rk[0][1])); tvec1[2] = (Tstark[2][2]+(Fstark[2][0]*rk[0][1])); Tstark[1][0] = ((tvec1[0]*c2)-(Tstark[2][1]*s2)); Tstark[1][1] = ((Tstark[2][1]*c2)+(tvec1[0]*s2)); Tstark[1][2] = tvec1[2]; fs[1] = (mtau[1]-Fstark[1][1]); Fstark[0][0] = Fstark[1][0]; Fstark[0][1] = Fstark[1][1]; Fstark[0][2] = Fstark[1][2]; tvec1[0] = (Tstark[1][0]+(Fstark[1][2]*q[1])); tvec1[2] = (Tstark[1][2]-(Fstark[1][0]*q[1])); Tstark[0][0] = tvec1[0]; Tstark[0][1] = Tstark[1][1]; Tstark[0][2] = tvec1[2]; fs[0] = (mtau[0]-Fstark[0][0]); /* Used 0.01 seconds CPU time, 0 additional bytes of memory. Equations contain 31 adds/subtracts/negates 26 multiplies 0 divides 29 assignments */ } sdfsgenfull() { /* Compute Fs (incl generic mult & other forces) */ sdfsgenmult(); fs[0] = (fs[0]+fs0[0]); fs[1] = (fs[1]+fs0[1]); fs[2] = (fs[2]+fs0[2]); fs[3] = (fs[3]+fs0[3]); /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 4 adds/subtracts/negates 0 multiplies 0 divides 4 assignments */ } sdrhs() { /* Generated 13-Jan-2003 14:06:16 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 */ /* Compute hinge torques for tree hinges */ tauc[0] = (mtau[0]+utau[0]); tauc[1] = (mtau[1]+utau[1]); tauc[2] = (mtau[2]+utau[2]); tauc[3] = (mtau[3]+utau[3]); sddoiner(); /* Compute onk & onb (angular accels in N) */ Onkb[3][2] = (udot[2]+udot[3]); onb[0][0] = 0.; onb[0][1] = 0.; onb[0][2] = udot[2]; onb[1][0] = 0.; onb[1][1] = 0.; onb[1][2] = Onkb[3][2]; /* Compute acceleration dyadics */ dyad[0][0][0] = -w2w2[0]; dyad[0][0][1] = -udot[2]; dyad[0][0][2] = 0.; dyad[0][1][0] = udot[2]; dyad[0][1][1] = -w2w2[0]; dyad[0][1][2] = 0.; dyad[0][2][0] = 0.; dyad[0][2][1] = 0.; dyad[0][2][2] = 0.; dyad[1][0][0] = -w2w2[1]; dyad[1][0][1] = -Onkb[3][2]; dyad[1][0][2] = 0.; dyad[1][1][0] = Onkb[3][2]; dyad[1][1][1] = -w2w2[1]; dyad[1][1][2] = 0.; dyad[1][2][0] = 0.; dyad[1][2][1] = 0.; dyad[1][2][2] = 0.; /* Compute ank & anb (mass center linear accels in N) */ Ankb[2][0] = ((rk[0][1]*udot[2])+((udot[0]*c2)+(udot[1]*s2))); Ankb[2][1] = ((udot[1]*c2)-(udot[0]*s2)); AOnkri[3][0] = (Ankb[2][0]-(ri[1][1]*udot[2])); Ankb[3][0] = ((Onkb[3][2]*rk[1][1])+((Ankb[2][1]*s3)+(AOnkri[3][0]*c3))); Ankb[3][1] = ((Ankb[2][1]*c3)-(AOnkri[3][0]*s3)); AnkAtk[2][1] = (Ankb[2][1]+Atk[2][1]); ank[2][0] = ((Ankb[2][0]*c2)-(AnkAtk[2][1]*s2)); ank[2][1] = ((AnkAtk[2][1]*c2)+(Ankb[2][0]*s2)); AnkAtk[3][0] = (Ankb[3][0]+Atk[3][0]); AnkAtk[3][1] = (Ankb[3][1]+Atk[3][1]); ank[3][0] = ((AnkAtk[3][0]*cnk[3][0][0])+(AnkAtk[3][1]*cnk[3][0][1])); ank[3][1] = ((AnkAtk[3][0]*cnk[3][1][0])+(AnkAtk[3][1]*cnk[3][1][1])); anb[0][0] = ank[2][0]; anb[0][1] = ank[2][1]; anb[0][2] = 0.; anb[1][0] = ank[3][0]; anb[1][1] = ank[3][1]; anb[1][2] = 0.; /* Compute constraint acceleration errors */ roustate = 3; sduaerr(curtim,q,u,udot,&aerr[0]); /* Used 0.02 seconds CPU time, 0 additional bytes of memory. Equations contain 25 adds/subtracts/negates 19 multiplies 0 divides 47 assignments */ } sdpseudo(lqout,luout) double lqout[1],luout[1]; { /* Return pseudo-coordinates for loop joints. */ /* There are no loop joints in this system. */ } sdpsqdot(lqdout) double lqdout[1]; { /* Return pseudo-coordinate derivatives for loop joints. */ /* There are no loop joints in this system. */ } sdpsudot(ludout) double ludout[1]; { /* Return pseudo-coordinate accelerations for loop joints. */ /* There are no loop joints in this system. */ } sdperr(errs) double errs[5]; { /* Return position constraint errors. */ if ((roustate != 2) && (roustate != 3)) { sdseterr(26,23); return; } errs[0] = perr[0]; errs[1] = perr[1]; errs[2] = perr[2]; errs[3] = perr[3]; errs[4] = perr[4]; } sdverr(errs) double errs[5]; { /* Return velocity constraint errors. */ if ((roustate != 2) && (roustate != 3)) { sdseterr(27,23); return; } errs[0] = verr[0]; errs[1] = verr[1]; errs[2] = verr[2]; errs[3] = verr[3]; errs[4] = verr[4]; } sdaerr(errs) double errs[5]; { /* Return acceleration constraint errors. */ if (roustate != 3) { sdseterr(35,24); return; } errs[0] = aerr[0]; errs[1] = aerr[1]; errs[2] = aerr[2]; errs[3] = aerr[3]; errs[4] = aerr[4]; } int sdindx(joint,axis) int joint,axis; { int offs,gotit; if (sdchkjaxis(36,joint,axis) != 0) { return 0; } gotit = 0; if (jtype[joint] == 4) { if (axis == 3) { offs = ballq[joint]; gotit = 1; } } else { if ((jtype[joint] == 6) || (jtype[joint] == 21)) { if (axis == 6) { offs = ballq[joint]; gotit = 1; } } } if (gotit == 0) { offs = firstq[joint]+axis; } return offs; } sdpresacc(joint,axis,prval) int joint,axis; double prval; { } sdpresvel(joint,axis,prval) int joint,axis; double prval; { } sdprespos(joint,axis,prval) int joint,axis; double prval; { } sdgetht(joint,axis,torque) int joint,axis; double *torque; { if (sdchkjaxis(30,joint,axis) != 0) { return; } if (roustate != 3) { sdseterr(30,24); return; } *torque = tauc[sdindx(joint,axis)]; } sdhinget(joint,axis,torque) int joint,axis; double torque; { if (sdchkjaxis(10,joint,axis) != 0) { return; } if (roustate != 2) { sdseterr(10,23); return; } if (mfrcflg != 0) { mtau[sdindx(joint,axis)] = mtau[sdindx(joint,axis)]+torque; } else { fs0flg = 0; utau[sdindx(joint,axis)] = utau[sdindx(joint,axis)]+torque; } } sdpointf(body,point,force) int body; double point[3],force[3]; { double torque[3]; if (sdchkbnum(11,body) != 0) { return; } if (roustate != 2) { sdseterr(11,23); return; } if (body == -1) { return; } torque[0] = point[1]*force[2]-point[2]*force[1]; torque[1] = point[2]*force[0]-point[0]*force[2]; torque[2] = point[0]*force[1]-point[1]*force[0]; if (mfrcflg != 0) { mfk[body][0] = mfk[body][0]+force[0]; mtk[body][0] = mtk[body][0]+torque[0]; mfk[body][1] = mfk[body][1]+force[1]; mtk[body][1] = mtk[body][1]+torque[1]; mfk[body][2] = mfk[body][2]+force[2]; mtk[body][2] = mtk[body][2]+torque[2]; } else { fs0flg = 0; ufk[body][0] = ufk[body][0]+force[0]; utk[body][0] = utk[body][0]+torque[0]; ufk[body][1] = ufk[body][1]+force[1]; utk[body][1] = utk[body][1]+torque[1]; ufk[body][2] = ufk[body][2]+force[2]; utk[body][2] = utk[body][2]+torque[2]; } } sdbodyt(body,torque) int body; double torque[3]; { if (sdchkbnum(12,body) != 0) { return; } if (roustate != 2) { sdseterr(12,23); return; } if (body == -1) { return; } if (mfrcflg != 0) { mtk[body][0] = mtk[body][0]+torque[0]; mtk[body][1] = mtk[body][1]+torque[1]; mtk[body][2] = mtk[body][2]+torque[2]; } else { fs0flg = 0; utk[body][0] = utk[body][0]+torque[0]; utk[body][1] = utk[body][1]+torque[1]; utk[body][2] = utk[body][2]+torque[2]; } } sddoww(routine) int routine; { double pp[5][4],dpp[4][5]; int i,j,c; double sum; double dfk[2][3],dtk[2][3],dtau[4],dltci[1][3],dltc[1][3],dlfci[1][3],dlfc[1 ][3]; double dTinb[1][3],dToutb[1][3],dltaufi[1][3],dltaufo[1][3],dltauti[1][3], dltauto[1][3]; double umult[5]; double deps[4],dZ1[4][3],dZ2[4][3]; roustate = 2; if (wwflg == 0) { /* Compute constraint effects */ sddomm(routine); /* Constraint 0 (user constraint) */ /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = 1.; umult[1] = 0.; umult[2] = 0.; umult[3] = 0.; umult[4] = 0.; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; deps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[3][0] = ((deps[3]*G1[3][0])-mfk[1][0]); dZ2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); dZ2[3][2] = (deps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[2][0] = (((dZ1[3][0]*c3)+(mfk[1][1]*s3))-mfk[0][0]); dZ1[2][1] = (((dZ1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); dZ1[2][2] = -(mfk[0][2]+mfk[1][2]); dZ2[2][0] = ((((dZ2[3][0]*c3)+(mtk[1][1]*s3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); dZ2[2][1] = (((dZ2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); dZ2[2][2] = ((dZ2[3][2]+((dZ1[3][0]*rikt[3][2][0])-(mfk[1][1]* rikt[3][2][1])))-(mtk[0][2]+(mfk[0][0]*rk[0][1]))); deps[2] = (mtau[2]-dZ2[2][2]); dZ1[2][0] = (dZ1[2][0]+(deps[2]*G1[2][0])); dZ1[2][1] = (dZ1[2][1]+(deps[2]*G1[2][1])); dZ2[2][2] = (deps[2]+dZ2[2][2]); dZ1[1][0] = ((dZ1[2][0]*c2)-(dZ1[2][1]*s2)); dZ1[1][1] = ((dZ1[2][0]*s2)+(dZ1[2][1]*c2)); dZ1[1][2] = dZ1[2][2]; dZ2[1][0] = ((dZ2[2][0]*c2)-(dZ2[2][1]*s2)); dZ2[1][1] = ((dZ2[2][0]*s2)+(dZ2[2][1]*c2)); dZ2[1][2] = dZ2[2][2]; deps[1] = (mtau[1]-dZ1[1][1]); dZ1[1][0] = (dZ1[1][0]+(deps[1]*G1[1][0])); dZ1[1][1] = (deps[1]+dZ1[1][1]); dZ1[0][0] = dZ1[1][0]; dZ1[0][1] = dZ1[1][1]; dZ1[0][2] = dZ1[1][2]; dZ2[0][0] = (dZ2[1][0]+(dZ1[1][2]*q[1])); dZ2[0][1] = dZ2[1][1]; dZ2[0][2] = (dZ2[1][2]-(dZ1[1][0]*q[1])); deps[0] = (mtau[0]-dZ1[0][0]); for (i = 0; i <= 3; i++) { pp[0][i] = deps[i]; dpp[i][0] = DD[i]*deps[i]; } wmap[0] = 0; /* Constraint 1 (user constraint) */ /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = 0.; umult[1] = 1.; umult[2] = 0.; umult[3] = 0.; umult[4] = 0.; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; deps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[3][0] = ((deps[3]*G1[3][0])-mfk[1][0]); dZ2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); dZ2[3][2] = (deps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[2][0] = (((dZ1[3][0]*c3)+(mfk[1][1]*s3))-mfk[0][0]); dZ1[2][1] = (((dZ1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); dZ1[2][2] = -(mfk[0][2]+mfk[1][2]); dZ2[2][0] = ((((dZ2[3][0]*c3)+(mtk[1][1]*s3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); dZ2[2][1] = (((dZ2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); dZ2[2][2] = ((dZ2[3][2]+((dZ1[3][0]*rikt[3][2][0])-(mfk[1][1]* rikt[3][2][1])))-(mtk[0][2]+(mfk[0][0]*rk[0][1]))); deps[2] = (mtau[2]-dZ2[2][2]); dZ1[2][0] = (dZ1[2][0]+(deps[2]*G1[2][0])); dZ1[2][1] = (dZ1[2][1]+(deps[2]*G1[2][1])); dZ2[2][2] = (deps[2]+dZ2[2][2]); dZ1[1][0] = ((dZ1[2][0]*c2)-(dZ1[2][1]*s2)); dZ1[1][1] = ((dZ1[2][0]*s2)+(dZ1[2][1]*c2)); dZ1[1][2] = dZ1[2][2]; dZ2[1][0] = ((dZ2[2][0]*c2)-(dZ2[2][1]*s2)); dZ2[1][1] = ((dZ2[2][0]*s2)+(dZ2[2][1]*c2)); dZ2[1][2] = dZ2[2][2]; deps[1] = (mtau[1]-dZ1[1][1]); dZ1[1][0] = (dZ1[1][0]+(deps[1]*G1[1][0])); dZ1[1][1] = (deps[1]+dZ1[1][1]); dZ1[0][0] = dZ1[1][0]; dZ1[0][1] = dZ1[1][1]; dZ1[0][2] = dZ1[1][2]; dZ2[0][0] = (dZ2[1][0]+(dZ1[1][2]*q[1])); dZ2[0][1] = dZ2[1][1]; dZ2[0][2] = (dZ2[1][2]-(dZ1[1][0]*q[1])); deps[0] = (mtau[0]-dZ1[0][0]); for (i = 0; i <= 3; i++) { pp[1][i] = deps[i]; dpp[i][1] = DD[i]*deps[i]; } wmap[1] = 1; /* Constraint 2 (user constraint) */ /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = 0.; umult[1] = 0.; umult[2] = 1.; umult[3] = 0.; umult[4] = 0.; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; deps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[3][0] = ((deps[3]*G1[3][0])-mfk[1][0]); dZ2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); dZ2[3][2] = (deps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[2][0] = (((dZ1[3][0]*c3)+(mfk[1][1]*s3))-mfk[0][0]); dZ1[2][1] = (((dZ1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); dZ1[2][2] = -(mfk[0][2]+mfk[1][2]); dZ2[2][0] = ((((dZ2[3][0]*c3)+(mtk[1][1]*s3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); dZ2[2][1] = (((dZ2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); dZ2[2][2] = ((dZ2[3][2]+((dZ1[3][0]*rikt[3][2][0])-(mfk[1][1]* rikt[3][2][1])))-(mtk[0][2]+(mfk[0][0]*rk[0][1]))); deps[2] = (mtau[2]-dZ2[2][2]); dZ1[2][0] = (dZ1[2][0]+(deps[2]*G1[2][0])); dZ1[2][1] = (dZ1[2][1]+(deps[2]*G1[2][1])); dZ2[2][2] = (deps[2]+dZ2[2][2]); dZ1[1][0] = ((dZ1[2][0]*c2)-(dZ1[2][1]*s2)); dZ1[1][1] = ((dZ1[2][0]*s2)+(dZ1[2][1]*c2)); dZ1[1][2] = dZ1[2][2]; dZ2[1][0] = ((dZ2[2][0]*c2)-(dZ2[2][1]*s2)); dZ2[1][1] = ((dZ2[2][0]*s2)+(dZ2[2][1]*c2)); dZ2[1][2] = dZ2[2][2]; deps[1] = (mtau[1]-dZ1[1][1]); dZ1[1][0] = (dZ1[1][0]+(deps[1]*G1[1][0])); dZ1[1][1] = (deps[1]+dZ1[1][1]); dZ1[0][0] = dZ1[1][0]; dZ1[0][1] = dZ1[1][1]; dZ1[0][2] = dZ1[1][2]; dZ2[0][0] = (dZ2[1][0]+(dZ1[1][2]*q[1])); dZ2[0][1] = dZ2[1][1]; dZ2[0][2] = (dZ2[1][2]-(dZ1[1][0]*q[1])); deps[0] = (mtau[0]-dZ1[0][0]); for (i = 0; i <= 3; i++) { pp[2][i] = deps[i]; dpp[i][2] = DD[i]*deps[i]; } wmap[2] = 2; /* Constraint 3 (user constraint) */ /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = 0.; umult[1] = 0.; umult[2] = 0.; umult[3] = 1.; umult[4] = 0.; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; deps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[3][0] = ((deps[3]*G1[3][0])-mfk[1][0]); dZ2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); dZ2[3][2] = (deps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[2][0] = (((dZ1[3][0]*c3)+(mfk[1][1]*s3))-mfk[0][0]); dZ1[2][1] = (((dZ1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); dZ1[2][2] = -(mfk[0][2]+mfk[1][2]); dZ2[2][0] = ((((dZ2[3][0]*c3)+(mtk[1][1]*s3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); dZ2[2][1] = (((dZ2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); dZ2[2][2] = ((dZ2[3][2]+((dZ1[3][0]*rikt[3][2][0])-(mfk[1][1]* rikt[3][2][1])))-(mtk[0][2]+(mfk[0][0]*rk[0][1]))); deps[2] = (mtau[2]-dZ2[2][2]); dZ1[2][0] = (dZ1[2][0]+(deps[2]*G1[2][0])); dZ1[2][1] = (dZ1[2][1]+(deps[2]*G1[2][1])); dZ2[2][2] = (deps[2]+dZ2[2][2]); dZ1[1][0] = ((dZ1[2][0]*c2)-(dZ1[2][1]*s2)); dZ1[1][1] = ((dZ1[2][0]*s2)+(dZ1[2][1]*c2)); dZ1[1][2] = dZ1[2][2]; dZ2[1][0] = ((dZ2[2][0]*c2)-(dZ2[2][1]*s2)); dZ2[1][1] = ((dZ2[2][0]*s2)+(dZ2[2][1]*c2)); dZ2[1][2] = dZ2[2][2]; deps[1] = (mtau[1]-dZ1[1][1]); dZ1[1][0] = (dZ1[1][0]+(deps[1]*G1[1][0])); dZ1[1][1] = (deps[1]+dZ1[1][1]); dZ1[0][0] = dZ1[1][0]; dZ1[0][1] = dZ1[1][1]; dZ1[0][2] = dZ1[1][2]; dZ2[0][0] = (dZ2[1][0]+(dZ1[1][2]*q[1])); dZ2[0][1] = dZ2[1][1]; dZ2[0][2] = (dZ2[1][2]-(dZ1[1][0]*q[1])); deps[0] = (mtau[0]-dZ1[0][0]); for (i = 0; i <= 3; i++) { pp[3][i] = deps[i]; dpp[i][3] = DD[i]*deps[i]; } wmap[3] = 3; /* Constraint 4 (user constraint) */ /* Initialize all multiplier forces to zero. */ for (i = 0; i <= 1; i++) { for (j = 0; j <= 2; j++) { mfk[i][j] = 0.; mtk[i][j] = 0.; } } for (i = 0; i <= 3; i++) { mtau[i] = 0.; } /* Compute user-generated multiplier forces */ umult[0] = 0.; umult[1] = 0.; umult[2] = 0.; umult[3] = 0.; umult[4] = 1.; mfrcflg = 1; sduconsfrc(curtim,q,u,umult); mfrcflg = 0; deps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[3][0] = ((deps[3]*G1[3][0])-mfk[1][0]); dZ2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); dZ2[3][2] = (deps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); dZ1[2][0] = (((dZ1[3][0]*c3)+(mfk[1][1]*s3))-mfk[0][0]); dZ1[2][1] = (((dZ1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); dZ1[2][2] = -(mfk[0][2]+mfk[1][2]); dZ2[2][0] = ((((dZ2[3][0]*c3)+(mtk[1][1]*s3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); dZ2[2][1] = (((dZ2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); dZ2[2][2] = ((dZ2[3][2]+((dZ1[3][0]*rikt[3][2][0])-(mfk[1][1]* rikt[3][2][1])))-(mtk[0][2]+(mfk[0][0]*rk[0][1]))); deps[2] = (mtau[2]-dZ2[2][2]); dZ1[2][0] = (dZ1[2][0]+(deps[2]*G1[2][0])); dZ1[2][1] = (dZ1[2][1]+(deps[2]*G1[2][1])); dZ2[2][2] = (deps[2]+dZ2[2][2]); dZ1[1][0] = ((dZ1[2][0]*c2)-(dZ1[2][1]*s2)); dZ1[1][1] = ((dZ1[2][0]*s2)+(dZ1[2][1]*c2)); dZ1[1][2] = dZ1[2][2]; dZ2[1][0] = ((dZ2[2][0]*c2)-(dZ2[2][1]*s2)); dZ2[1][1] = ((dZ2[2][0]*s2)+(dZ2[2][1]*c2)); dZ2[1][2] = dZ2[2][2]; deps[1] = (mtau[1]-dZ1[1][1]); dZ1[1][0] = (dZ1[1][0]+(deps[1]*G1[1][0])); dZ1[1][1] = (deps[1]+dZ1[1][1]); dZ1[0][0] = dZ1[1][0]; dZ1[0][1] = dZ1[1][1]; dZ1[0][2] = dZ1[1][2]; dZ2[0][0] = (dZ2[1][0]+(dZ1[1][2]*q[1])); dZ2[0][1] = dZ2[1][1]; dZ2[0][2] = (dZ2[1][2]-(dZ1[1][0]*q[1])); deps[0] = (mtau[0]-dZ1[0][0]); for (i = 0; i <= 3; i++) { pp[4][i] = deps[i]; dpp[i][4] = DD[i]*deps[i]; } wmap[4] = 4; /* Produce constraint coefficient matrix WW */ for (c = 0; c <= 4; c++) { for (i = c; i <= 4; i++) { sum = 0.; for (j = 0; j <= 3; j++) { sum = sum+pp[wmap[c]][j]*dpp[j][wmap[i]]; } ww[wmap[c]][wmap[i]] = sum; ww[wmap[i]][wmap[c]] = sum; } } /* Form QR decomposition of WW */ sdqrdcomp(5,5,5,5,wmap,wmap,ww,qraux,jpvt); wwflg = 1; } /* Used 0.09 seconds CPU time, 8192 additional bytes of memory. Equations contain 245 adds/subtracts/negates 230 multiplies 0 divides 400 assignments */ } sdxudot0(routine,oudot0) int routine; double oudot0[4]; { /* Compute unconstrained equations */ int i; double eps[4],Z1[4][3],Z2[4][3],A1[4][3],A2[4][3],K1[4][3],K2[4][3]; sdlhs(routine); /* Solve equations ignoring constraints */ Z1[3][0] = (fs0[3]*G1[3][0]); Z1[2][0] = (Z1[3][0]*c3); Z1[2][1] = (Z1[3][0]*s3); Z2[2][2] = (fs0[3]+(rikt[3][2][0]*Z1[3][0])); eps[2] = (fs0[2]-Z2[2][2]); Z1[2][0] = (Z1[2][0]+(eps[2]*G1[2][0])); Z1[2][1] = (Z1[2][1]+(eps[2]*G1[2][1])); Z2[2][2] = (eps[2]+Z2[2][2]); Z1[1][0] = ((Z1[2][0]*c2)-(Z1[2][1]*s2)); Z1[1][1] = ((Z1[2][0]*s2)+(Z1[2][1]*c2)); Z2[1][2] = Z2[2][2]; eps[1] = (fs0[1]-Z1[1][1]); Z1[1][0] = (Z1[1][0]+(eps[1]*G1[1][0])); Z1[1][1] = (eps[1]+Z1[1][1]); Z1[0][0] = Z1[1][0]; Z1[0][1] = Z1[1][1]; Z2[0][2] = (Z2[1][2]-(q[1]*Z1[1][0])); eps[0] = (fs0[0]-Z1[0][0]); udot[0] = (DD[0]*eps[0]); udot[1] = ((DD[1]*eps[1])-(G1[1][0]*udot[0])); K1[2][0] = ((udot[0]*c2)+(udot[1]*s2)); K1[2][1] = ((udot[1]*c2)-(udot[0]*s2)); udot[2] = ((DD[2]*eps[2])-((G1[2][0]*K1[2][0])+(G1[2][1]*K1[2][1]))); K1[3][0] = ((rikt[3][2][0]*udot[2])+((K1[2][0]*c3)+(K1[2][1]*s3))); K1[3][1] = ((rikt[3][2][1]*udot[2])+((K1[2][1]*c3)-(K1[2][0]*s3))); udot[3] = ((DD[3]*fs0[3])-(udot[2]+(G1[3][0]*K1[3][0]))); A2[3][2] = (udot[2]+udot[3]); for (i = 0; i <= 3; i++) { oudot0[i] = udot[i]; } /* Used 0.02 seconds CPU time, 0 additional bytes of memory. Equations contain 24 adds/subtracts/negates 30 multiplies 0 divides 31 assignments */ } sdudot0(oudot0) double oudot0[4]; { if ((roustate != 2) && (roustate != 3)) { sdseterr(66,23); return; } sdxudot0(66,oudot0); } sdsetudot(iudot) double iudot[4]; { /* Assign udots and advance to stage Dynamics Ready */ int i; if ((roustate != 2) && (roustate != 3)) { sdseterr(68,23); return; } for (i = 0; i <= 3; i++) { udot[i] = iudot[i]; } sdrhs(); } sdxudotm(routine,imult,oudotm) int routine; double imult[5],oudotm[4]; { /* Compute udots due only to multipliers */ int i; double eps[4],Z1[4][3],Z2[4][3],A1[4][3],A2[4][3],K1[4][3],K2[4][3]; sdlhs(routine); sdmfrc(imult); eps[3] = (mtau[3]+(mtk[1][2]+(mfk[1][0]*rk[1][1]))); Z1[3][0] = ((eps[3]*G1[3][0])-mfk[1][0]); Z2[3][0] = -(mtk[1][0]-(mfk[1][2]*rk[1][1])); Z2[3][2] = (eps[3]-(mtk[1][2]+(mfk[1][0]*rk[1][1]))); Z1[2][0] = (((mfk[1][1]*s3)+(Z1[3][0]*c3))-mfk[0][0]); Z1[2][1] = (((Z1[3][0]*s3)-(mfk[1][1]*c3))-mfk[0][1]); Z1[2][2] = -(mfk[0][2]+mfk[1][2]); Z2[2][0] = ((((mtk[1][1]*s3)+(Z2[3][0]*c3))-(dik[3][1]*mfk[1][2]))-( mtk[0][0]-(mfk[0][2]*rk[0][1]))); Z2[2][1] = (((Z2[3][0]*s3)-(mtk[1][1]*c3))-mtk[0][1]); Z2[2][2] = ((Z2[3][2]+((rikt[3][2][0]*Z1[3][0])-(mfk[1][1]*rikt[3][2][1])))- (mtk[0][2]+(mfk[0][0]*rk[0][1]))); eps[2] = (mtau[2]-Z2[2][2]); Z1[2][0] = (Z1[2][0]+(eps[2]*G1[2][0])); Z1[2][1] = (Z1[2][1]+(eps[2]*G1[2][1])); Z2[2][2] = (eps[2]+Z2[2][2]); Z1[1][0] = ((Z1[2][0]*c2)-(Z1[2][1]*s2)); Z1[1][1] = ((Z1[2][0]*s2)+(Z1[2][1]*c2)); Z1[1][2] = Z1[2][2]; Z2[1][0] = ((Z2[2][0]*c2)-(Z2[2][1]*s2)); Z2[1][1] = ((Z2[2][0]*s2)+(Z2[2][1]*c2)); Z2[1][2] = Z2[2][2]; eps[1] = (mtau[1]-Z1[1][1]); Z1[1][0] = (Z1[1][0]+(eps[1]*G1[1][0])); Z1[1][1] = (eps[1]+Z1[1][1]); Z1[0][0] = Z1[1][0]; Z1[0][1] = Z1[1][1]; Z1[0][2] = Z1[1][2]; Z2[0][0] = (Z2[1][0]+(q[1]*Z1[1][2])); Z2[0][1] = Z2[1][1]; Z2[0][2] = (Z2[1][2]-(q[1]*Z1[1][0])); eps[0] = (mtau[0]-Z1[0][0]); udot[0] = (DD[0]*eps[0]); udot[1] = ((DD[1]*eps[1])-(G1[1][0]*udot[0])); K1[2][0] = ((udot[0]*c2)+(udot[1]*s2)); K1[2][1] = ((udot[1]*c2)-(udot[0]*s2)); udot[2] = ((DD[2]*eps[2])-((G1[2][0]*K1[2][0])+(G1[2][1]*K1[2][1]))); K1[3][0] = ((rikt[3][2][0]*udot[2])+((K1[2][0]*c3)+(K1[2][1]*s3))); K1[3][1] = ((rikt[3][2][1]*udot[2])+((K1[2][1]*c3)-(K1[2][0]*s3))); udot[3] = ((DD[3]*eps[3])-(udot[2]+(G1[3][0]*K1[3][0]))); A2[3][2] = (udot[2]+udot[3]); for (i = 0; i <= 3; i++) { oudotm[i] = udot[i]; } /* Used 0.03 seconds CPU time, 0 additional bytes of memory. Equations contain 49 adds/subtracts/negates 48 multiplies 0 divides 43 assignments */ } sdudotm(imult,oudotm) double imult[5],oudotm[4]; { if ((roustate != 2) && (roustate != 3)) { sdseterr(67,23); return; } sdxudotm(67,imult,oudotm); } sdderiv(oqdot,oudot) double oqdot[4],oudot[4]; { /* This is the derivative section for a 2-body ground-based system with 4 hinge degree(s) of freedom. There are 5 constraints. */ double workr[5],bb[5],b0[5],v0[5],p0[5]; int iwork[5]; int i,j; double udot0[4],udot1[4]; double eps[4],Z1[4][3],Z2[4][3],A1[4][3],A2[4][3],K1[4][3],K2[4][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(17,23); return; } if (stabvelq == 1) { sdseterr(17,32); } if (stabposq == 1) { sdseterr(17,33); } wsiz = 5; /* Compute unconstrained equations */ sdxudot0(17,udot0); sdrhs(); sdaerr(b0); if (stabvel != 0.) { sdverr(v0); } if (stabpos != 0.) { sdperr(p0); } /* Stabilize constraints using Baumgarte's method */ for (i = 0; i <= 4; i++) { bb[i] = -b0[i]; } if (stabvel != 0.) { for (i = 0; i <= 4; i++) { bb[i] = bb[i]-stabvel*v0[i]; } } if (stabpos != 0.) { for (i = 0; i <= 4; i++) { bb[i] = bb[i]-stabpos*p0[i]; } } /* Compute and decompose constraint matrix WW */ sddoww(17); /* Numerically solve for constraint multipliers */ sdqrbslv(5,5,5,5,wmap,wmap,1e-13,workr,iwork,ww,qraux,jpvt,bb,mult,&wrank); for (i = 0; i <= 4; i++) { multmap[i] = 0; } for (i = 0; i < wrank; i++) { multmap[jpvt[i]] = 1; } j = 0; for (i = 0; i <= 4; i++) { if (multmap[i] != 0) { multmap[j] = wmap[i]; j = j+1; } } /* Compute final udots */ sdxudotm(17,mult,udot1); for (i = 0; i <= 3; i++) { udot[i] = udot0[i]+udot1[i]; } sdrhs(); for (i = 0; i <= 3; i++) { oqdot[i] = qdot[i]; } for (i = 0; i <= 3; i++) { oudot[i] = udot[i]; } /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 19 adds/subtracts/negates 10 multiplies 0 divides 27 assignments */ } /* Compute residuals for use with DAE integrator */ sdresid(eqdot,eudot,emults,resid) double eqdot[4],eudot[4],emults[5],resid[13]; { int i; double uderrs[4],p0[5]; if ((roustate != 2) && (roustate != 3)) { sdseterr(16,23); return; } if (stabposq == 1) { sdseterr(16,33); } sdfulltrq(eudot,emults,uderrs); for (i = 0; i < 4; i++) { resid[i] = eqdot[i]-qdot[i]; } for (i = 0; i < 4; i++) { resid[4+i] = uderrs[i]; } sdverr(&resid[8]); if (stabpos != 0.) { sdperr(p0); for (i = 0; i < 5; i++) { resid[8+i] = resid[8+i]+stabpos*p0[i]; } } for (i = 0; i < 4; i++) { udot[i] = eudot[i]; } for (i = 0; i < 5; i++) { mult[i] = emults[i]; } sdrhs(); /* Used 0.00 seconds CPU time, 0 additional bytes of memory. Equations contain 9 adds/subtracts/negates 5 multiplies 0 divides 22 assignments */ } sdmult(omults,owrank,omultmap) double omults[5]; int *owrank,omultmap[5]; { int i; if (roustate != 3) { sdseterr(34,24); return; } for (i = 0; i < 5; i++) { omults[i] = mult[i]; if (i <= wrank-1) { omultmap[i] = multmap[i]; } else { omultmap[i] = -1; } } *owrank = wrank; } sdreac(force,torque) double force[2][3],torque[2][3]; { /* Generated 13-Jan-2003 14:06:16 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 */ if (roustate != 3) { sdseterr(31,24); return; } /* Compute reaction forces for non-weld tree joints */ ffkb[0][0] = (mfk[0][0]+ufk[0][0]); ffkb[0][1] = (mfk[0][1]+ufk[0][1]); ffkb[0][2] = (mfk[0][2]+ufk[0][2]); ffkb[1][0] = (mfk[1][0]+ufk[1][0]); ffkb[1][1] = (mfk[1][1]+ufk[1][1]); ffkb[1][2] = (mfk[1][2]+ufk[1][2]); ttkb[0][0] = (mtk[0][0]+utk[0][0]); ttkb[0][1] = (mtk[0][1]+utk[0][1]); ttkb[0][2] = (mtk[0][2]+utk[0][2]); ttkb[1][0] = (mtk[1][0]+utk[1][0]); ttkb[1][1] = (mtk[1][1]+utk[1][1]); ttkb[1][2] = (mtk[1][2]+utk[1][2]); fc[3][0] = ((mk[1]*(AnkAtk[3][0]-gk[3][0]))-ffkb[1][0]); fc[3][1] = ((mk[1]*(AnkAtk[3][1]-gk[3][1]))-ffkb[1][1]); fc[3][2] = -(ffkb[1][2]+(grav[2]*mk[1])); tc[3][0] = -(ttkb[1][0]+(fc[3][2]*rk[1][1])); tc[3][1] = -ttkb[1][1]; tc[3][2] = ((ik[1][2][2]*Onkb[3][2])-(ttkb[1][2]-(fc[3][0]*rk[1][1]))); fccikt[3][0] = ((fc[3][0]*c3)-(fc[3][1]*s3)); fccikt[3][1] = ((fc[3][0]*s3)+(fc[3][1]*c3)); fccikt[3][2] = fc[3][2]; ffk[2][0] = (ffkb[0][0]-fccikt[3][0]); ffk[2][1] = (ffkb[0][1]-fccikt[3][1]); ffk[2][2] = (ffkb[0][2]-fccikt[3][2]); ttk[2][0] = (ttkb[0][0]-((fccikt[3][2]*ri[1][1])+((tc[3][0]*c3)-(tc[3][1]*s3 )))); ttk[2][1] = (ttkb[0][1]-((tc[3][0]*s3)+(tc[3][1]*c3))); ttk[2][2] = (ttkb[0][2]-(tc[3][2]-(fccikt[3][0]*ri[1][1]))); fc[2][0] = ((mk[0]*(Ankb[2][0]-gk[2][0]))-ffk[2][0]); fc[2][1] = ((mk[0]*(AnkAtk[2][1]-gk[2][1]))-ffk[2][1]); fc[2][2] = -(ffk[2][2]+(grav[2]*mk[0])); tc[2][0] = -(ttk[2][0]+(fc[2][2]*rk[0][1])); tc[2][1] = -ttk[2][1]; tc[2][2] = ((ik[0][2][2]*udot[2])-(ttk[2][2]-(fc[2][0]*rk[0][1]))); fccikt[2][0] = ((fc[2][0]*c2)-(fc[2][1]*s2)); fccikt[2][1] = ((fc[2][0]*s2)+(fc[2][1]*c2)); fccikt[2][2] = fc[2][2]; ffk[1][0] = -fccikt[2][0]; ffk[1][1] = -fccikt[2][1]; ffk[1][2] = -fccikt[2][2]; ttk[1][0] = -((tc[2][0]*c2)-(tc[2][1]*s2)); ttk[1][1] = -((tc[2][0]*s2)+(tc[2][1]*c2)); ttk[1][2] = -tc[2][2]; fc[1][0] = -ffk[1][0]; fc[1][1] = -ffk[1][1]; fc[1][2] = -ffk[1][2]; tc[1][0] = -ttk[1][0]; tc[1][1] = -ttk[1][1]; tc[1][2] = -ttk[1][2]; fccikt[1][0] = fc[1][0]; fccikt[1][1] = fc[1][1]; fccikt[1][2] = fc[1][2]; ffk[0][0] = -fccikt[1][0]; ffk[0][1] = -fccikt[1][1]; ffk[0][2] = -fccikt[1][2]; ttk[0][0] = -(tc[1][0]+(fccikt[1][2]*q[1])); ttk[0][1] = -tc[1][1]; ttk[0][2] = -(tc[1][2]-(fccikt[1][0]*q[1])); fc[0][0] = -ffk[0][0]; fc[0][1] = -ffk[0][1]; fc[0][2] = -ffk[0][2]; tc[0][0] = -ttk[0][0]; tc[0][1] = -ttk[0][1]; tc[0][2] = -ttk[0][2]; force[0][0] = fc[2][0]; torque[0][0] = tc[2][0]; force[0][1] = fc[2][1]; torque[0][1] = tc[2][1]; force[0][2] = fc[2][2]; torque[0][2] = tc[2][2]; force[1][0] = fc[3][0]; torque[1][0] = tc[3][0]; force[1][1] = fc[3][1]; torque[1][1] = tc[3][1]; force[1][2] = fc[3][2]; torque[1][2] = tc[3][2]; /* Compute reaction forces for tree weld joints */ /* Used 0.02 seconds CPU time, 0 additional bytes of memory. Equations contain 76 adds/subtracts/negates 32 multiplies 0 divides 75 assignments */ } sdmom(lm,am,ke) double lm[3],am[3],*ke; { /* Compute system linear and angular momentum, and kinetic energy. Generated 13-Jan-2003 14:06:16 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 */ double lk[2][3],hnk[2][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(19,23); return; } lk[0][0] = (mk[0]*vnk[2][0]); lk[0][1] = (mk[0]*vnk[2][1]); lk[1][0] = (mk[1]*vnk[3][0]); lk[1][1] = (mk[1]*vnk[3][1]); hnk[0][2] = (ik[0][2][2]*u[2]); hnk[1][2] = (ik[1][2][2]*wk[3][2]); lm[0] = (lk[0][0]+lk[1][0]); lm[1] = (lk[0][1]+lk[1][1]); lm[2] = 0.; am[0] = 0.; am[1] = 0.; am[2] = (((hnk[0][2]+((lk[0][1]*rnk[2][0])-(lk[0][0]*rnk[2][1])))+(hnk[1][2] +((lk[1][1]*rnk[3][0])-(lk[1][0]*rnk[3][1]))))-((com[0]*lm[1])-(com[1]* lm[0]))); *ke = (.5*(((hnk[0][2]*u[2])+((lk[0][0]*vnk[2][0])+(lk[0][1]*vnk[2][1])))+(( hnk[1][2]*wk[3][2])+((lk[1][0]*vnk[3][0])+(lk[1][1]*vnk[3][1]))))); /* Used 0.01 seconds CPU time, 8192 additional bytes of memory. Equations contain 14 adds/subtracts/negates 19 multiplies 0 divides 13 assignments */ } sdsys(mtoto,cm,icm) double *mtoto,cm[3],icm[3][3]; { /* Compute system total mass, and instantaneous center of mass and inertia matrix. Generated 13-Jan-2003 14:06:16 by SD/FAST, Order(N) formulation (sdfast B.2.6 #70405) on machine ID 690c54f5 Copyright (c) 1990-1996 Symbolic Dynamics, Inc. Copyright (c) 1990-1996 Parametric Technology Corp. RESTRICTED RIGHTS LEGEND: Use, duplication, or disclosure by the U.S. Government is subject to restrictions as set forth in subparagraph (c)(1)(ii) of the Rights in Technical Data and Computer Software clause at DFARS 52.227-7013 and similar clauses in the FAR and NASA FAR Supplement. Symbolic Dynamics, Inc., Mountain View, CA 94041 */ double ikcnkt[4][3][3]; if ((roustate != 2) && (roustate != 3)) { sdseterr(20,23); return; } *mtoto = mtot; cm[0] = com[0]; cm[1] = com[1]; cm[2] = 0.; icm[0][0] = (.1+((((1.542*(c2*c2))+(mk[0]*(rnk[2][1]*rnk[2][1])))+((mk[1]*( rnk[3][1]*rnk[3][1]))+((.1*(cnk[3][0][1]*cnk[3][0][1]))+(1.642*( cnk[3][0][0]*cnk[3][0][0])))))-(mtot*(com[1]*com[1])))); icm[0][1] = ((mtot*(com[0]*com[1]))+(((1.542*(s2*c2))-(mk[0]*(rnk[2][0]* rnk[2][1])))+(((.1*(cnk[3][0][1]*cnk[3][1][1]))+(1.642*(cnk[3][0][0]* cnk[3][1][0])))-(mk[1]*(rnk[3][0]*rnk[3][1]))))); icm[0][2] = 0.; icm[1][0] = icm[0][1]; icm[1][1] = (.1+((((1.542*(s2*s2))+(mk[0]*(rnk[2][0]*rnk[2][0])))+((mk[1]*( rnk[3][0]*rnk[3][0]))+((.1*(cnk[3][1][1]*cnk[3][1][1]))+(1.642*( cnk[3][1][0]*cnk[3][1][0])))))-(mtot*(com[0]*com[0])))); icm[1][2] = 0.; icm[2][0] = icm[0][2]; icm[2][1] = icm[1][2]; icm[2][2] = (((ik[0][2][2]+(mk[0]*((rnk[2][0]*rnk[2][0])+(rnk[2][1]* rnk[2][1]))))+(ik[1][2][2]+(mk[1]*((rnk[3][0]*rnk[3][0])+(rnk[3][1]* rnk[3][1])))))-(mtot*((com[0]*com[0])+(com[1]*com[1])))); /* Used 0.02 seconds CPU time, 0 additional bytes of memory. Equations contain 24 adds/subtracts/negates 45 multiplies 0 divides 13 assignments */ } sdpos(body,pt,loc) int body; double pt[3],loc[3]; { /* Return inertial frame location of a point on a body. */ double pv[3]; if (sdchkbnum(21,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(21,23); return; } if (body == -1) { loc[0] = pt[0]; loc[1] = pt[1]; loc[2] = pt[2]; } else { pv[0] = rnb[body][0]+pt[0]*cnb[body][0][0]+pt[1]*cnb[body][0][1]+pt[2]* cnb[body][0][2]; pv[1] = rnb[body][1]+pt[0]*cnb[body][1][0]+pt[1]*cnb[body][1][1]+pt[2]* cnb[body][1][2]; pv[2] = rnb[body][2]+pt[0]*cnb[body][2][0]+pt[1]*cnb[body][2][1]+pt[2]* cnb[body][2][2]; loc[0] = pv[0]; loc[1] = pv[1]; loc[2] = pv[2]; } } sdvel(body,pt,velo) int body; double pt[3],velo[3]; { /* Return inertial frame velocity of a point on a body. */ double pv[3]; if (sdchkbnum(22,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(22,23); return; } if (body == -1) { velo[0] = 0.; velo[1] = 0.; velo[2] = 0.; } else { pv[0] = wb[body][1]*pt[2]-wb[body][2]*pt[1]; pv[1] = wb[body][2]*pt[0]-wb[body][0]*pt[2]; pv[2] = wb[body][0]*pt[1]-wb[body][1]*pt[0]; velo[0] = vnb[body][0]+pv[0]*cnb[body][0][0]+pv[1]*cnb[body][0][1]+pv[2] *cnb[body][0][2]; velo[1] = vnb[body][1]+pv[0]*cnb[body][1][0]+pv[1]*cnb[body][1][1]+pv[2] *cnb[body][1][2]; velo[2] = vnb[body][2]+pv[0]*cnb[body][2][0]+pv[1]*cnb[body][2][1]+pv[2] *cnb[body][2][2]; } } sdorient(body,dircos) int body; double dircos[3][3]; { /* Return orientation of body w.r.t. ground frame. */ if (sdchkbnum(23,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(23,23); return; } if (body == -1) { dircos[0][0] = 1.; dircos[0][1] = 0.; dircos[0][2] = 0.; dircos[1][0] = 0.; dircos[1][1] = 1.; dircos[1][2] = 0.; dircos[2][0] = 0.; dircos[2][1] = 0.; dircos[2][2] = 1.; } else { dircos[0][0] = cnb[body][0][0]; dircos[0][1] = cnb[body][0][1]; dircos[0][2] = cnb[body][0][2]; dircos[1][0] = cnb[body][1][0]; dircos[1][1] = cnb[body][1][1]; dircos[1][2] = cnb[body][1][2]; dircos[2][0] = cnb[body][2][0]; dircos[2][1] = cnb[body][2][1]; dircos[2][2] = cnb[body][2][2]; } } sdangvel(body,avel) int body; double avel[3]; { /* Return angular velocity of the body. */ if (sdchkbnum(24,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(24,23); return; } if (body == -1) { avel[0] = 0.; avel[1] = 0.; avel[2] = 0.; } else { avel[0] = wb[body][0]; avel[1] = wb[body][1]; avel[2] = wb[body][2]; } } sdtrans(frbod,ivec,tobod,ovec) int frbod; double ivec[3]; int tobod; double ovec[3]; { /* Transform ivec from frbod frame to tobod frame. */ double pv[3]; if (sdchkbnum(25,frbod) != 0) { return; } if (sdchkbnum(25,tobod) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(25,23); return; } if (frbod == tobod) { sdvcopy(ivec,ovec); return; } if (frbod == -1) { sdvcopy(ivec,pv); ovec[0] = pv[0]*cnb[tobod][0][0]+pv[1]*cnb[tobod][1][0]+pv[2]*cnb[tobod ][2][0]; ovec[1] = pv[0]*cnb[tobod][0][1]+pv[1]*cnb[tobod][1][1]+pv[2]*cnb[tobod ][2][1]; ovec[2] = pv[0]*cnb[tobod][0][2]+pv[1]*cnb[tobod][1][2]+pv[2]*cnb[tobod ][2][2]; return; } if (tobod == -1) { sdvcopy(ivec,pv); ovec[0] = pv[0]*cnb[frbod][0][0]+pv[1]*cnb[frbod][0][1]+pv[2]*cnb[frbod ][0][2]; ovec[1] = pv[0]*cnb[frbod][1][0]+pv[1]*cnb[frbod][1][1]+pv[2]*cnb[frbod ][1][2]; ovec[2] = pv[0]*cnb[frbod][2][0]+pv[1]*cnb[frbod][2][1]+pv[2]*cnb[frbod ][2][2]; return; } pv[0] = ivec[0]*cnb[frbod][0][0]+ivec[1]*cnb[frbod][0][1]+ivec[2]*cnb[frbod ][0][2]; pv[1] = ivec[0]*cnb[frbod][1][0]+ivec[1]*cnb[frbod][1][1]+ivec[2]*cnb[frbod ][1][2]; pv[2] = ivec[0]*cnb[frbod][2][0]+ivec[1]*cnb[frbod][2][1]+ivec[2]*cnb[frbod ][2][2]; ovec[0] = pv[0]*cnb[tobod][0][0]+pv[1]*cnb[tobod][1][0]+pv[2]*cnb[tobod][2][ 0]; ovec[1] = pv[0]*cnb[tobod][0][1]+pv[1]*cnb[tobod][1][1]+pv[2]*cnb[tobod][2][ 1]; ovec[2] = pv[0]*cnb[tobod][0][2]+pv[1]*cnb[tobod][1][2]+pv[2]*cnb[tobod][2][ 2]; } sdrel2cart(coord,body,point,linchg,rotchg) int coord,body; double point[3],linchg[3],rotchg[3]; { /* Return derivative of pt loc and body orient w.r.t. hinge rate */ int x,i,gnd; double lin[3],pv[3]; double pink[3],ptvec[3]; if ((coord < 0) || (coord > 3)) { sdseterr(59,45); return; } if (sdchkbnum(59,body) != 0) { return; } if ((roustate != 2) && (roustate != 3)) { sdseterr(59,23); return; } gnd = -1; sdvset(0.,0.,0.,linchg); sdvset(0.,0.,0.,rotchg); i = body; for (;;) { if (i == gnd) { return; } x = firstq[i]; if (x <= coord) { if (coord >= x+njntdof[i]) { return; } break; } i = inb[i]; } sddoping(); for (i = 0; i < 3; i++) { pink[i] = ping[coord][i]; lin[i] = hngpt[coord][i]; } sdtrans(gnd,pink,body,pink); if (trans[coord] != 0) { sdvcopy(pink,linchg); } else { sdvcopy(pink,rotchg); sdpos(body,point,ptvec); sdvsub(ptvec,lin,ptvec); sdtrans(gnd,ptvec,body,ptvec); sdvcross(pink,ptvec,linchg); } } sdacc(body,pt,accel) int body; double pt[3],accel[3]; { /* Return linear acceleration a point of the specified body. */ double pv[3]; if (sdchkbnum(32,body) != 0) { return; } if (roustate != 3) { sdseterr(32,24); return; } if (body == -1) { accel[0] = 0.; accel[1] = 0.; accel[2] = 0.; } else { pv[0] = pt[0]*dyad[body][0][0]+pt[1]*dyad[body][0][1]+pt[2]*dyad[body][0 ][2]; pv[1] = pt[0]*dyad[body][1][0]+pt[1]*dyad[body][1][1]+pt[2]*dyad[body][1 ][2]; pv[2] = pt[0]*dyad[body][2][0]+pt[1]*dyad[body][2][1]+pt[2]*dyad[body][2 ][2]; accel[0] = anb[body][0]+pv[0]*cnb[body][0][0]+pv[1]*cnb[body][0][1]+pv[2 ]*cnb[body][0][2]; accel[1] = anb[body][1]+pv[0]*cnb[body][1][0]+pv[1]*cnb[body][1][1]+pv[2 ]*cnb[body][1][2]; accel[2] = anb[body][2]+pv[0]*cnb[body][2][0]+pv[1]*cnb[body][2][1]+pv[2 ]*cnb[body][2][2]; } } sdangacc(body,aacc) int body; double aacc[3]; { /* Return angular acceleration of the body. */ if (sdchkbnum(33,body) != 0) { return; } if (roustate != 3) { sdseterr(33,24); return; } if (body == -1) { aacc[0] = 0.; aacc[1] = 0.; aacc[2] = 0.; } else { aacc[0] = onb[body][0]; aacc[1] = onb[body][1]; aacc[2] = onb[body][2]; } } sdgrav(gravin) double gravin[3]; { grav[0] = gravin[0]; gravq[0] = 3; grav[1] = gravin[1]; gravq[1] = 3; grav[2] = gravin[2]; gravq[2] = 3; roustate = 0; } sdmass(body,massin) int body; double massin; { if (sdchkbnum(2,body) != 0) { return; } if (body == -1) { sdseterr(2,15); return; } if (mkq[body] != 0) { mk[body] = massin; mkq[body] = 3; } else { sdseterr(2,19); } roustate = 0; } sdiner(body,inerin) int body; double inerin[3][3]; { int anyques; if (sdchkbnum(3,body) != 0) { return; } if (body == -1) { sdseterr(3,15); return; } anyques = 0; if (ikq[body][0][0] != 0) { ik[body][0][0] = inerin[0][0]; ikq[body][0][0] = 3; anyques = 1; } if (ikq[body][0][1] != 0) { ik[body][0][1] = inerin[0][1]; ikq[body][0][1] = 3; ik[body][1][0] = inerin[0][1]; ikq[body][1][0] = 3; anyques = 1; } if (ikq[body][0][2] != 0) { ik[body][0][2] = inerin[0][2]; ikq[body][0][2] = 3; ik[body][2][0] = inerin[0][2]; ikq[body][2][0] = 3; anyques = 1; } if (ikq[body][1][1] != 0) { ik[body][1][1] = inerin[1][1]; ikq[body][1][1] = 3; anyques = 1; } if (ikq[body][1][2] != 0) { ik[body][1][2] = inerin[1][2]; ikq[body][1][2] = 3; ik[body][2][1] = inerin[1][2]; ikq[body][2][1] = 3; anyques = 1; } if (ikq[body][2][2] != 0) { ik[body][2][2] = inerin[2][2]; ikq[body][2][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(3,19); } roustate = 0; } sdbtj(joint,btjin) int joint; double btjin[3]; { int anyques; if (sdchkjnum(4,joint) != 0) { return; } anyques = 0; if (rkq[joint][0] != 0) { rk[joint][0] = btjin[0]; rkq[joint][0] = 3; anyques = 1; } if (rkq[joint][1] != 0) { rk[joint][1] = btjin[1]; rkq[joint][1] = 3; anyques = 1; } if (rkq[joint][2] != 0) { rk[joint][2] = btjin[2]; rkq[joint][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(4,19); } roustate = 0; } sditj(joint,itjin) int joint; double itjin[3]; { int anyques; if (sdchkjnum(5,joint) != 0) { return; } anyques = 0; if (riq[joint][0] != 0) { ri[joint][0] = itjin[0]; riq[joint][0] = 3; anyques = 1; } if (riq[joint][1] != 0) { ri[joint][1] = itjin[1]; riq[joint][1] = 3; anyques = 1; } if (riq[joint][2] != 0) { ri[joint][2] = itjin[2]; riq[joint][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(5,19); } roustate = 0; } sdpin(joint,pinno,pinin) int joint,pinno; double pinin[3]; { int anyques,offs; if (sdchkjpin(6,joint,pinno) != 0) { return; } anyques = 0; offs = firstq[joint]+pinno; if (jtype[joint] == 21) { offs = offs+3; } if (jtype[joint] == 11) { offs = offs+1; } if (pinq[offs][0] != 0) { pin[offs][0] = pinin[0]; pinq[offs][0] = 3; anyques = 1; } if (pinq[offs][1] != 0) { pin[offs][1] = pinin[1]; pinq[offs][1] = 3; anyques = 1; } if (pinq[offs][2] != 0) { pin[offs][2] = pinin[2]; pinq[offs][2] = 3; anyques = 1; } if (anyques == 0) { sdseterr(6,19); } roustate = 0; } sdpres(joint,axis,presin) int joint,axis,presin; { int anyques; if (sdchkjaxis(37,joint,axis) != 0) { return; } if ((presin != 0) && (presin != 1)) { sdseterr(37,20); } anyques = 0; if (presq[sdindx(joint,axis)] != 0) { if (presin != 0) { pres[sdindx(joint,axis)] = 1.; } else { pres[sdindx(joint,axis)] = 0.; } presq[sdindx(joint,axis)] = 3; anyques = 1; } if (anyques == 0) { sdseterr(37,19); } wwflg = 0; } sdstab(velin,posin) double velin,posin; { stabvel = velin; stabvelq = 3; stabpos = posin; stabposq = 3; } sdgetgrav(gravout) double gravout[3]; { gravout[0] = grav[0]; gravout[1] = grav[1]; gravout[2] = grav[2]; } sdgetmass(body,massout) int body; double *massout; { if (sdchkbnum(40,body) != 0) { return; } if (body == -1) { sdseterr(40,15); return; } *massout = mk[body]; } sdgetiner(body,inerout) int body; double inerout[3][3]; { if (sdchkbnum(41,body) != 0) { return; } if (body == -1) { sdseterr(41,15); return; } inerout[0][0] = ik[body][0][0]; inerout[0][1] = ik[body][0][1]; inerout[0][2] = ik[body][0][2]; inerout[1][0] = ik[body][1][0]; inerout[1][1] = ik[body][1][1]; inerout[1][2] = ik[body][1][2]; inerout[2][0] = ik[body][2][0]; inerout[2][1] = ik[body][2][1]; inerout[2][2] = ik[body][2][2]; } sdgetbtj(joint,btjout) int joint; double btjout[3]; { if (sdchkjnum(42,joint) != 0) { return; } btjout[0] = rk[joint][0]; btjout[1] = rk[joint][1]; btjout[2] = rk[joint][2]; } sdgetitj(joint,itjout) int joint; double itjout[3]; { if (sdchkjnum(43,joint) != 0) { return; } itjout[0] = ri[joint][0]; itjout[1] = ri[joint][1]; itjout[2] = ri[joint][2]; } sdgetpin(joint,pinno,pinout) int joint,pinno; double pinout[3]; { int offs; if (sdchkjpin(44,joint,pinno) != 0) { return; } offs = firstq[joint]+pinno; if (jtype[joint] == 21) { offs = offs+3; } if (jtype[joint] == 11) { offs = offs+1; } pinout[0] = pin[offs][0]; pinout[1] = pin[offs][1]; pinout[2] = pin[offs][2]; } sdgetpres(joint,axis,presout) int joint,axis,*presout; { if (sdchkjaxis(45,joint,axis) != 0) { return; } if (pres[sdindx(joint,axis)] != 0.) { *presout = 1; } else { *presout = 0; } } sdgetstab(velout,posout) double *velout,*posout; { *velout = stabvel; *posout = stabpos; } sdinfo(info) int info[50]; { info[0] = ground; info[1] = nbod; info[2] = ndof; info[3] = ncons; info[4] = nloop; info[5] = nldof; info[6] = nloopc; info[7] = nball; info[8] = nlball; info[9] = npres; info[10] = nuser; info[11] = 5; /* info entries from 12-49 are reserved */ } sdjnt(joint,info,tran) int joint,info[50],tran[6]; { int i,offs; if (sdchkjnum(48,joint) != 0) { return; } info[0] = jtype[joint]; info[1] = 0; offs = 0; info[2] = inb[joint]; info[3] = outb[joint]; info[4] = njntdof[joint]; info[5] = njntc[joint]; info[6] = njntp[joint]; info[7] = firstq[joint]; info[8] = ballq[joint]; info[9] = firstm[joint]; info[10] = firstp[joint]; /* info entries from 11-49 are reserved */ for (i = 0; i <= 5; i++) { if (i < njntdof[joint]) { tran[i] = trans[offs+firstq[joint]+i]; } else { tran[i] = -1; } } } sdcons(consno,info) int consno,info[50]; { if (sdchkucnum(49,consno) != 0) { return; } info[0] = 1; info[1] = firstu[consno]; /* info entries from 2-49 are reserved */ } int sdchkbnum(routine,bnum) int routine,bnum; { if ((bnum < -1) || (bnum > 1)) { sdseterr(routine,15); return 1; } return 0; } int sdchkjnum(routine,jnum) int routine,jnum; { if ((jnum < 0) || (jnum > 1)) { sdseterr(routine,16); return 1; } return 0; } int sdchkucnum(routine,ucnum) int routine,ucnum; { if ((ucnum < 0) || (ucnum > 4)) { sdseterr(routine,21); return 1; } return 0; } int sdchkjaxis(routine,jnum,axnum) int routine,jnum,axnum; { int maxax; if (sdchkjnum(routine,jnum) != 0) { return 1; } if ((axnum < 0) || (axnum > 6)) { sdseterr(routine,17); return 1; } maxax = njntdof[jnum]-1; if ((jtype[jnum] == 4) || (jtype[jnum] == 6) || (jtype[jnum] == 21)) { maxax = maxax+1; } if (axnum > maxax) { sdseterr(routine,18); return 1; } return 0; } int sdchkjpin(routine,jnum,pinno) int routine,jnum,pinno; { int maxax,pinok; if (sdchkjnum(routine,jnum) != 0) { return 1; } if ((pinno < 0) || (pinno > 5)) { sdseterr(routine,17); return 1; } if (njntdof[jnum] >= 3) { maxax = 2; } else { maxax = njntdof[jnum]-1; } if (jtype[jnum] == 4) { maxax = -1; } if (jtype[jnum] == 7) { maxax = 0; } pinok = 0; if (pinno <= maxax) { pinok = 1; } if (pinok == 0) { sdseterr(routine,18); return 1; } return 0; } /* Convert state to form using 1-2-3 Euler angles for ball joints. */ sdst2ang(st,stang) double st[4],stang[4]; { int i; for (i = 0; i < 4; i++) { stang[i] = st[i]; } } /* Convert 1-2-3 form of state back to Euler parameters for ball joints. */ sdang2st(stang,st) double stang[4],st[4]; { int i; for (i = 0; i < 4; i++) { st[i] = stang[i]; } } /* Normalize Euler parameters in state. */ sdnrmsterr(st,normst,routine) double st[4],normst[4]; int routine; { int i; for (i = 0; i < 4; i++) { normst[i] = st[i]; } } sdnormst(st,normst) double st[4],normst[4]; { sdnrmsterr(st,normst,0); } sdgentime(gentm) int *gentm; { *gentm = 140615; } /* Done. CPU seconds used: 0.54 Memory used: 925696 bytes. Equation complexity: sdstate: 28 adds 45 multiplies 2 divides 95 assignments sdderiv: 728 adds 736 multiplies 59 divides 1184 assignments sdresid: 105 adds 72 multiplies 0 divides 138 assignments sdreac: 76 adds 32 multiplies 0 divides 75 assignments sdmom: 14 adds 19 multiplies 0 divides 13 assignments sdsys: 24 adds 45 multiplies 0 divides 13 assignments */