ó v™xTc@ sedZddlmZyddlZWnek r?dZnXedk r„ddlZddlmZddlm Z d„Z nddl Zd„Z d„Z ddl Z d e fd „ƒYZeƒZd „Zd „Zd „Zd„Zd„Zd„Zd„Zd„Zde fd„ƒYZd„Zd„Zddd„Zddded„ZdS(s®Python version of a subset of Roy Featherstone's spatial_v1 matlab code: http://axiom.anu.edu.au/~roy/spatial/ Version 1: January 2008 (latest bug fix: 7 October 2008) The subset of converted files covers all dependencies of: ID.m FDab.m The original matlab comments are preserved as Python docstrings. See also: Rigid Body Dynamics Algorithms. Roy Featherstone, Springer, New York, 2007. ISBN-10: 0387743146 iÿÿÿÿ(tdivisionN(tmatrix(tifloorcC s=tjtjjjd|jƒddddƒjƒjƒƒS(Ntmtrelative_epsilongê-™—q=tabsolute_epsilon( Rtsqrtscitbxtlinalgt eigensystemtreal_symmetrictas_flex_double_matrixtgeneralized_inverse_as_packed_utmatrix_packed_u_as_symmetric(R((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytgeneralized_inverse s  cC sd„}|tj|ƒƒS(NcS s(|dkrt|dƒSt|dƒS(Nigà?(tint(tx((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytiround.s (tmathtfloor(RR((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pyR-s cC s |jƒS(N(tinverse(R((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pyR3stInfTypecB seZRS((t__name__t __module__(((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pyR8sc%C s™tj|ƒ}tj|ƒ}tjddddddd||dddd| |ddddddddddddd||dddd| |f$ƒS(s % Xrotx spatial coordinate transform (X-axis rotation). % Xrotx(theta) calculates the coordinate transform matrix from A to B % coordinates for spatial motion vectors, where coordinate frame B is % rotated by an angle theta (radians) relative to frame A about their % common X axis. ii(RtcostsinRR(tthetatcts((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytXrotx;sc%C s™tj|ƒ}tj|ƒ}tj|d| ddddddddd|d|dddddd|d| ddddddddd|d|f$ƒS(s % Xroty spatial coordinate transform (Y-axis rotation). % Xroty(theta) calculates the coordinate transform matrix from A to B % coordinates for spatial motion vectors, where coordinate frame B is % rotated by an angle theta (radians) relative to frame A about their % common Y axis. ii(RRRRR(RRR((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytXrotyMsc%C s™tj|ƒ}tj|ƒ}tj||dddd| |ddddddddddddd||dddd| |dddddddf$ƒS(s % Xrotz spatial coordinate transform (Z-axis rotation). % Xrotz(theta) calculates the coordinate transform matrix from A to B % coordinates for spatial motion vectors, where coordinate frame B is % rotated by an angle theta (radians) relative to frame A about their % common Z axis. ii(RRRRR(RRR((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytXrotz_sc %C sš|\ }}}}}}}}} tj|||ddd|||ddd||| dddddd|||ddd|||ddd||| f$ƒS(s= Featherstone (2007) Tab. 2.2 Added in Python version. i(RR( tEtatbRtdtetftgthti((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytXrotqs!c%C s‹|\}}}tjddddddddddddddddddd|| ddd| d|ddd|| ddddf$ƒS(s % Xtrans spatial coordinate transform (translation of origin). % Xtrans(r) calculates the coordinate transform matrix from A to B % coordinates for spatial motion vectors, in which frame B is translated by % an amount r (3D vector) relative to frame A. ii(RR(trtr1tr2tr3((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytXtranssc%C sš|\}}}}}}tjd| |ddd|d| ddd| |ddddd| |d| ||d| |d| | |d| |df$ƒS(s¶ % crm spatial cross-product operator (motion). % crm(v) calculates the 6x6 matrix such that the expression crm(v)*m is the % cross product of the spatial motion vectors v and m. i(RR(tvtv1tv2tv3tv4tv5tv6((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytcrmscC st|ƒjƒ S(sÐ % crf spatial cross-product operator (force). % crf(v) calculates the 6x6 matrix such that the expression crf(v)*f is the % cross product of the spatial motion vector v with the spatial force % vector f. (R6t transpose(R/((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytcrfžsc C sˆ|\}}}tjd| ||d| | |df ƒ}tj||||jƒ||||jƒ|tjdƒfƒjƒS(sû % mcI spatial rigid-body inertia from mass, CoM and rotational inertia. % mcI(m,c,I) calculates the spatial inertia matrix of a rigid body from its % mass, centre of mass (3D vector) and rotational inertia (3x3 matrix) % about its centre of mass. ii(RRR7tidentitytresolve_partitions(RRtItc1tc2tc3tC((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytmcI§s  tautoTreecB seZdddd„ZRS(iic C st||_dg||_d g||_g|_g|_g}x,t|ƒD]}t|dtj |ƒ|ƒd|j|<|j|dkrµ|jj t dddgƒƒn4|jj t |ƒt ||j|ddgƒƒ|j ||ƒ|d|}||t jdddgƒ}|||dt jdd d gƒ} |jj t||| ƒƒqNWd S( s€ % autoTree Create System Models of Kinematic Trees % autoTree(nb,bf,skew,taper) creates system models of kinematic trees % having revolute joints. nb and bf specify the number of bodies in the % tree, and the branching factor, respectively. The latter is the average % number of children of a nonterminal node, and must be >=1. bf=1 produces % an unbranched tree; bf=2 produces a binary tree; and non-integer values % produce trees in which the number of children alternates between % floor(bf) and ceil(bf) in such a way that the average is bf. Trees are % constructed (and numbered) breadth-first. Link i is a thin-walled % cylindrical tube of length l(i), radius l(i)/20, and mass m(i), lying % between 0 and l(i) on the x axis of its local coordinate system. The % values of l(i) and m(i) are determined by the tapering coefficient: % l(i)=taper^(i-1) and m(i)=taper^(3*(i-1)). Thus, if taper=1 then % m(i)=l(i)=1 for all i. The inboard joint axis of link i lies on the % local z axis, and its outboard axis passes through the point (l(i),0,0) % and is rotated about the x axis by an angle of skew radians relative to % the inboard axis. If the link has more than one outboard joint then they % all have the same axis. If skew=0 then the mechanism is planar. The % final one, two or three arguments can be omitted, in which case they % assume default values of taper=1, skew=0 and bf=1. iiiÿÿÿÿigà?ig{®Gázd?g= ×£p=ð?i Ng§ tÚ@§µ?g§ tÚ@§µ?(tNBtpitchtNonetparenttXtreeR;txrangeRRtceiltappendR.RRtcoltdiagR@( tselftnbtbftskewttapertlen_R(tmasstCoMtIcm((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pyt__init__¹s    ,"4 ((RRRU(((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pyRA·scC s÷t|tttfƒs(|jd|ƒS|dkrdt|ƒ}tjddddddgƒ}n‰|tkr©t dd|gƒ}tjddddddgƒ}nDt|ƒt dd||gƒ}tjddddd|gƒ}||fS(s_ % jcalc Calculate joint transform and motion subspace. % [Xj,S]=jcalc(pitch,q) calculates the joint transform and motion subspace % matrices for a revolute (pitch==0), prismatic (pitch==inf) or helical % (pitch==any other value) joint. For revolute and helical joints, q is % the joint angle. For prismatic joints, q is the linear displacement. tqii( t isinstanceRtfloatRtXj_SRRRJtInfR.(RCRVtXjtS((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytjcalcâs  $ $#!cC si|dkr+tjddddddgƒSt|ƒ}t|ƒdksOt‚tjdddg|ƒS(Nig…ëQ¸ž#Ài(RDRRJtlisttlentAssertionError(t grav_accn((sR/public2/ftp/pub/ccp4/6.5/ccp4-6.5/lib/py2/scitbx/rigid_body/proto/featherstone.pytgrav_accn_as_a_grav÷s   cC s±t|ƒ}dg|j}dg|j}dg|j} dg|j} dg|j} x•t|jƒD]„} t|j| || ƒ\} || <|| dkr½|| }|| }n$|| || }|| || }| |j| || <|j| dkr-|| | <|| | || | s:                +  5