vxTc@ s&ddlmZyddlZWnek r9dZnXedk ryddlmZddlmZddl m Z n/ddlZddl ZdZ dGHdZ ddl Z d Zd Zd Zed Zd ZdZddZdZedkr"ee jdndS(i(tdivisionN(t featherstone(tmatrix(t approx_equalcC stS(N(tTrue(ta1ta2((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pyRss8libtbx.test_utils not available: approx_equal() disabledcC s%d}x|D]}||7}q W|S(Ni((tltresultte((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytsums cC sHtttjdds$ttttjddsHttttjddslttttjddsttttjdd sttttj ddsttttj ddsttttj dddt j ddt jdddsDtdS(Ng?g|@g?geJZ@g333333?g4zI@iiiiiiiii iZitmgX9v?tctItsym_mat3guVN5@( iiiiiiiii (iii(iiiiii(iiiiii(iii(iiig?g?g333333?(RR tfstXrotxtAssertionErrortXrotytXrotztXrottXtranstcrmtcrftmcIRtcoltsym(((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytexercise_basics$$$$$$$cC s(td|djd||fdS(Nstst_featherstone_exercise_%s.mtws-split_long_rows(0); output_precision(%d); %s(topentwrite(tlabelt precisiontcode((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytwrite_matlab_script$scC sg}g}x|jD]~}t|dkr7qn|jg|jD]}t|^qJt|dkr|jtjd|g}qqWt|dkst|S(Nii$telems( t splitlinestlentextendtsplittfloattappendRtsqrR(tsRR#tlinetv((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytextract_sqr_matrices+s, s 0.0025000 0.0000000 0.0000000 0.0000000 -0.0000000 0.0000000 0.0000000 0.3345833 0.0000000 0.0000000 0.0000000 -0.5000000 0.0000000 0.0000000 0.3345833 -0.0000000 0.5000000 0.0000000 0.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 0.0000000 -0.0000000 0.0000000 0.5000000 0.0000000 1.0000000 0.0000000 0.0000000 -0.5000000 0.0000000 0.0000000 0.0000000 1.0000000 0.0025000 0.0000000 0.0000000 0.0000000 -0.0000000 0.0000000 0.0000000 0.3345833 0.0000000 0.0000000 0.0000000 -0.5000000 0.0000000 0.0000000 0.3345833 -0.0000000 0.5000000 0.0000000 0.0000000 0.0000000 -0.0000000 1.0000000 0.0000000 0.0000000 -0.0000000 0.0000000 0.5000000 0.0000000 1.0000000 0.0000000 0.0000000 -0.5000000 0.0000000 0.0000000 0.0000000 1.0000000 cC stddddddtjdd}|jdks@t|jd d gks[t|jd d gksvtt|jdkstt|j dkstt d }t |j|stt |j t stdS( NRtautoTreeR iR!s autoTree(2) tnbiiis^ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 -0 1 0 0 -0 0 0 0 1 0 0 -0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 1 0 1 0 0 -1 0 0 0 1 ( R"RR/tNBRtpitchtparentR%tXtreeR R.Rtexpected_autoTree_I(ttreetexpected((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytexercise_autoTreeGs cC stddddddtjdddd d d d d }d|jdd?d@dAdBdCgdDdEdFdGdHdIgdJdKdLdMdNdOgdPdQdRdSdTdUgdVdWdXdYdZd[gd\d]d^d_d`dagdbdcdddedfdggdhdidjdkdldmgdndodpdqdrdsgdtdudvdwdxdygdzd{d|d}d~dgg D]}tj|^q}dddg}ddddddddddddg ddddddddddddg ddddddddddddg g}x/td||gdd|g|D] \}} } tj d|d|d|d|d|d| } t | t | kst t g| D]} | j d^q)| sNt tjd|d|d|d| d|d| } t | t |kst t g| D]} | j d^q|st qWdS(NRtID_FDabR i R!s% modified version of test_FD.m % test the correctness of FDab, and ID by checking that FDab % is the inverse of ID. % step 1: create a complicated kinematic tree, and adjust some of the % pitches so that it contains helical and prismatic as well as revolute % joints. tree = autoTree( 12, 1.5, 1, 0.95 ); tree.pitch(3) = 0.1; tree.pitch(5) = inf; tree.pitch(7) = -0.1; tree.pitch(9) = inf; % step 2: choose random initial conditions rand("state", [0]); q = pi * (2*rand(12,1) - 1); qd = 2*rand(12,1) - 1; qdd = 2*rand(12,1) - 1; q qd qdd for i = 1:tree.NB f_ext{i} = 2*rand(6,1) - 1; end f_ext grav_accn = 2*rand(3,1) - 1; grav_accn % step 3: use ID to calculate the force required to produce qdd; then use % FDab to calculate the acceleration that this force % produces. tau = ID( tree, q, qd, qdd ); qdd_ab = FDab( tree, q, qd, tau ); % step 4: compare results. In theory, we should have qdd_ab==qdd and % qdd_crb==qdd. However, rounding errors will make them slightly % different. Expect rounding errors in the vicinity of 1e-14 on this % test. tau qdd_ab tau = ID( tree, q, qd, qdd, f_ext ); qdd_ab = FDab( tree, q, qd, tau, f_ext ); tau qdd_ab tau = ID( tree, q, qd, qdd, f_ext, grav_accn ); qdd_ab = FDab( tree, q, qd, tau, f_ext, grav_accn ); tau qdd_ab R0tbfg?tskewittapergffffff?g?iigiig/NP@g?gpT߿gQyG<(gm%g4ȄgPc)@gWz 3@g-z)5gUfT@g3:\?gJÜy@gEafBIg]!?gA(캿g"ɐ6ÿg7O!*#gSA),?gIT'@g2o7@gɬۈugUP@gcĮtѧ?gVΌ"2?g]g4u?g+*տg:?ga.g *j-)gJ`g@gIn @g yY?g‘{gÿgn g-%?gT%jпg?]?tmodeltqtqdtqddtf_extt grav_accnittau(R"RR/R2tInfRRtziptNonetIDR%RRR#tFDab(R6R>R?R@tft f_ext_giventgrav_accn_givent expected_tausRARBt expected_tauRCR tqdd_ab((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytexercise_ID_FDabcsN<$     %     '/ 'ttst_featherstone_tmpdirc sddlmddl}ddljj|sJj|nj||jj dddfd}|dd|d |d dS( Ni(t copy_filet module_nametscitbxt.c s jj||dS(N(tpathtjoin(t file_namettarget(RQtost scitbx_dist(sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytcpTssmatrix/__init__.pysscitbx_matrix.pys rigid_body/proto/featherstone.pys$rigid_body/proto/tst_featherstone.py( t libtbx.utilsRQtlibtbx.load_envRYRUtisdirtmkdirtchdirtenvt dist_path(ttmpdirtlibtbxR[((RQRYRZsV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytexercise_standaloneLs     cC sLt|dkstttttdk rCtndGHdS(NitOK(R%RRR8RORSRFRe(targs((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pytrunZs  t__main__i(t __future__RRSt ImportErrorRFtscitbx.rigid_body.protoRRRtlibtbx.test_utilsRt scitbx_matrixR tsysRR"R.R5R8ROReRht__name__targv(((sV/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/tst_featherstone.pyts2