o o h3@sddlmZmZmZmZddlmZddlmZm Z m Z ddl m Z ddl mZddlmZddlmZmZmZmZmZddlmZdd lmZd gZGd d d e Zd S) )zerosMatrixdiffeye)default_sort_key)ReferenceFramedynamicsymbolspartial_velocity)_Methods)Particle) RigidBody)msubsfind_dynamicsymbols_f_list_parser_validate_coordinates_parse_linear_solver) Linearizer)iterable KanesMethodc@s,eZdZdZ      d8ddZddZd9d d Zd9d d Zd dZddZ d9ddZ dddddZ d:ddZ ddZ d;ddZddZeddZed d!Zed"d#Zed$d%Zed&d'Zed(d)Zed*d+Zed,d-Zed.d/Zed0d1Zed2d3Zed4d5Zed6d7ZdS)>> from sympy import symbols >>> from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame >>> from sympy.physics.mechanics import Point, Particle, KanesMethod >>> q, u = dynamicsymbols('q u') >>> qd, ud = dynamicsymbols('q u', 1) >>> m, c, k = symbols('m c k') >>> N = ReferenceFrame('N') >>> P = Point('P') >>> P.set_vel(N, u * N.x) Next we need to arrange/store information in the way that KanesMethod requires. The kinematic differential equations should be an iterable of expressions. A list of forces/torques must be constructed, where each entry in the list is a (Point, Vector) or (ReferenceFrame, Vector) tuple, where the Vectors represent the Force or Torque. Next a particle needs to be created, and it needs to have a point and mass assigned to it. Finally, a list of all bodies and particles needs to be created. >>> kd = [qd - u] >>> FL = [(P, (-k * q - c * u) * N.x)] >>> pa = Particle('pa', P, m) >>> BL = [pa] Finally we can generate the equations of motion. First we create the KanesMethod object and supply an inertial frame, coordinates, generalized speeds, and the kinematic differential equations. Additional quantities such as configuration and motion constraints, dependent coordinates and speeds, and auxiliary speeds are also supplied here (see the online documentation). Next we form FR* and FR to complete: Fr + Fr* = 0. We have the equations of motion at this point. It makes sense to rearrange them though, so we calculate the mass matrix and the forcing terms, for E.o.M. in the form: [MM] udot = forcing, where MM is the mass matrix, udot is a vector of the time derivatives of the generalized speeds, and forcing is a vector representing "forcing" terms. >>> KM = KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd) >>> (fr, frstar) = KM.kanes_equations(BL, FL) >>> MM = KM.mass_matrix >>> forcing = KM.forcing >>> rhs = MM.inv() * forcing >>> rhs Matrix([[(-c*u(t) - k*q(t))/m]]) >>> KM.linearize(A_and_B=True)[0] Matrix([ [ 0, 1], [-k/m, -c/m]]) Please look at the documentation pages for more information on how to perform linearization and how to deal with dependent coordinates & speeds, and how do deal with bringing non-contributing forces into evidence. NTLUcCs|s tdg}tdg}t|tstd||_d|_d|_| |_| |_| |_ ||_ | ||||| t |j |j||||||| |dS)z&Please read the online documentation. dummy_qdummy_kdz+An inertial ReferenceFrame must be suppliedN)r isinstancer TypeError _inertial_fr_frstar _forcelist _bodylistexplicit_kinematics_constraint_solver_initialize_vectorsrqu_initialize_kindiffeq_matrices_initialize_constraint_matrices)selfframeq_indu_indkd_eqs q_dependentconfiguration_constraints u_dependentvelocity_constraintsacceleration_constraints u_auxiliarybodies forcelistr kd_eqs_solverconstraint_solverr5p/var/www/html/construction_image-detection-poc/venv/lib/python3.10/site-packages/sympy/physics/mechanics/kane.py__init__s*     zKanesMethod.__init__cCsdd}||}t|stdt|stdt|}||_t||g|_|jtj|_ ||}t|s:tdt|sBtdt|}||_ t||g|_ |j tj|_ |||_dS)z,Initialize the coordinate and speed vectors.cS|rt|StSNrxr5r5r6z1KanesMethod._initialize_vectors..z,Generalized coordinates must be an iterable.z*Dependent coordinates must be an iterable.z'Generalized speeds must be an iterable.z%Dependent speeds must be an iterable.N)rrr_qdep_qr"rr_t_qdot_udep_ur#_udot_uaux)r&r(q_depr)u_depu_aux none_handlerr5r5r6r!s(zKanesMethod._initialize_vectorscCst|}t|j}t|j}||}dd}||}t|jt|kr'td|||_||}||}t||kr>td|rJt||krJtd|rt|jd} t|j d} |j duret ||j }t || |_ ||j  |j|_|s|jtj|j|j tj} |j durt | |j } | |_|j|_n|j durt ||j }t || |_||j |j |_|jddd|f} |jdd||f} || |  |_dSt|_ t|_t|_t|_t|_dS)z Initializes constraint matrices.cSr8r9r:r;r5r5r6r=r>z=KanesMethod._initialize_constraint_matrices..zUThere must be an equal number of dependent coordinates and configuration constraints.zKThere must be an equal number of dependent speeds and velocity constraints.zOThere must be an equal number of dependent speeds and acceleration constraints.rN)rlenr#rCr? ValueError_f_hdictfromkeysrE _qdot_u_mapr _f_nhjacobian_k_nhrrrA_f_dnh_k_dnh_Arsr)r&configvelacc linear_solveromprJu_zero udot_zerorTB_indB_depr5r5r6r% sT               z+KanesMethod._initialize_constraint_matricescs~t|}|rt|jt|krtd|j}|j}t|}t|d}t|j d}t|d}| |}| |} | | |} t | |  | fdd|dd|ddD} | rjd} t| | | ||_| ||_| |_|| | } || |}tt||||  |_| ||_| ||_tt||_dSd|_t|_|_t|_|_t|_|_dS)a]Initialize the kinematic differential equation matrices. Parameters ========== kdeqs : sequence of sympy expressions Kinematic differential equations in the form of f(u,q',q,t) where f() = 0. The equations have to be linear in the generalized coordinates and generalized speeds. zRThere must be an equal number of kinematic differential equations and coordinates.rcsg|]}|vr|qSr5r5).0varidy_symsr5r6 tz>KanesMethod._initialize_kindiffeq_matrices..NzThe provided kinematic differential equations are nonlinear in {}. They must be linear in the generalized speeds and derivatives of the generalized coordinates.)rrKr"rLr#rBrrNrOrFrRxreplacerrow_joinformat _f_k_implicit_k_ku_implicit_k_kqdot_implicitziprP_f_k_k_kur_k_kqdot)r&kdeqsrZr#qdotr^ uaux_zero qdot_zerok_kuk_kqdotf_k nonlin_varsmsg f_k_explicit k_ku_explicitr5rdr6r$Ns>     &     z*KanesMethod._initialize_kindiffeq_matricesc s|durt|dkst|stdj}t||\}fdd|D}fddDtj}t}t|d}t|j|t|D]t fdd t|D|<qGj r|tj }|d|df}|||df} |j j | 7}|}|_ |_|S) z"Form the generalized active force.Nrz>Force pairs must be supplied in an non-empty iterable or None.cg|]}t|jqSr5r rPrbir&r5r6rfrgz(KanesMethod._form_fr..cr}r5r~rrr5r6rfrgc3s&|]}||VqdSr9)dot)rbj)f_listrpartialsr5r6 s$z'KanesMethod._form_fr..)rKrrLrrr#rr rangesumrCrVTrr) r&flNvel_listr[bFRr]FRtildeFRoldr5)rrrr&r6_form_frs*   $zKanesMethod._form_frc s<t|stdtjjtjdtjdfddjD}t|d}fddj D}| j fddfd d|D}t j }t||}t|d }fd d } fd d } t|D]&\} } t| tr6| | j} | | j}| | j}| | j}| | j}| || |}| || j|t|| j|||}t|D]j}| || d|}| ||| d |}t|D]*}|||f| ||| d|7<|||f||| d |7<q||||| d|7<||||| d |7<qqo| | j} | | j }| | j }| || |}t|D];}| || d|}t|D]}|||f| ||| d|7<qj||||| d|7<qZqo| t||}tt|||}|tt!j|| }j"r |t j"}|d|df}|||df}|j#j$|}|d|ddf}|||ddf}|j#j$|}|d|ddfj#j$|||ddf}|_%|_&|_'j(| _)|S)z#Form the generalized inertia force.z'Bodies must be supplied in an iterable.rcsg|]}t|qSr5)rr)tr5r6rfsz,KanesMethod._form_frstar..cs*i|]\}}||jqSr5)rrhrP)rbkv)r&rr5r6 s z,KanesMethod._form_frstar..csft|tr|j|jg}nt|tr|jg}ntdfdd|D}t |j S)NzMThe body list may only contain either RigidBody or Particle as list elements.cr}r5r~)rbrXrr5r6rfrgzJKanesMethod._form_frstar..get_partial_velocity..) rr masscenterrXr' ang_vel_inr pointrr r#)bodyvlistr)rr&r5r6get_partial_velocitys  z6KanesMethod._form_frstar..get_partial_velocitycsg|]}|qSr5r5)rbr)rr5r6rfsrcs t|Sr9r expr)rtr5r6r=s z*KanesMethod._form_frstar..cstt|Sr9rr)rtr_r5r6r=sN)*rrrrArrNrOrErFrPitemsupdaterKr#r enumeraterr masscentral_inertiarrXr'rrYrdtrr ang_acc_incrossrrrrCrVrrr_k_dr_f_d)r&bluauxdot uauxdot_zero q_ddot_u_maprr[MMnonMM zero_uauxzero_udot_uauxrrMIrXomegarYinertial_forceinertial_torquertmp_veltmp_angrtempfr_starr] fr_star_ind fr_star_depMMiMMdr5)rrr&rrtr_r6 _form_frstars            *("$   .& 0zKanesMethod._form_frstarcs\|jdus |jdurtd|j}|jr#|jr#|j|jt|j}nt}|jr8|j r8|j|j t|j }nt}t |jd}t |j d}t |j d}t t|j |jgd}t|j||jt|j } t|j||jt|j} t|j|} t|j||j} tt| d} |j}|j}|jr|dt|j }n|}|j}|jr|dt|j }n|}|j}|j}|tj}t t||gd}tt||j ||j ||gtfdd|j|j|j|j |j|jfDrtdttt|j |}|j!t"d|D]}t|tj|vrtd q t#| | | | | |||||||||||d S) aReturns an instance of the Linearizer class, initiated from the data in the KanesMethod class. This may be more desirable than using the linearize class method, as the Linearizer object will allow more efficient recalculation (i.e. about varying operating points). Parameters ========== linear_solver : str, callable Method used to solve the several symbolic linear systems of the form ``A*x=b`` in the linearization process. If a string is supplied, it should be a valid method that can be used with the :meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is supplied, it should have the format ``x = f(A, b)``, where it solves the equations and returns the solution. The default is ``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``. ``LUsolve()`` is fast to compute but will often result in divide-by-zero and thus ``nan`` results. Returns ======= Linearizer An instantiated :class:`sympy.physics.mechanics.linearize.Linearizer`. NNeed to compute Fr, Fr* first.rrc3s|]}t|VqdSr9)rrsym_listr5r6resz,KanesMethod.to_linearizer..zWCannot have dynamicsymbols outside dynamic forcing vector.)keyzpCannot have derivatives of specified quantities when linearizing forcing terms.rZ)$rrrLrMrQrSrr#rTrUrErNrOrBr rorqrprrKr"r?rCrFrrrAsetanyrlistrrsortrr)r&rZf_cf_vf_ar^ud_zeroqd_zero qd_u_zerof_0f_1f_2f_3f_4r"r#q_iq_du_iu_duauxrrtrrr5rr6 to_linearizersZ       zKanesMethod.to_linearizer) new_methodrZcKs(|j|d}|jdi|}||jfS)a Linearize the equations of motion about a symbolic operating point. Parameters ========== new_method Deprecated, does nothing and will be removed. linear_solver : str, callable Method used to solve the several symbolic linear systems of the form ``A*x=b`` in the linearization process. If a string is supplied, it should be a valid method that can be used with the :meth:`sympy.matrices.matrixbase.MatrixBase.solve`. If a callable is supplied, it should have the format ``x = f(A, b)``, where it solves the equations and returns the solution. The default is ``'LU'`` which corresponds to SymPy's ``A.LUsolve(b)``. ``LUsolve()`` is fast to compute but will often result in divide-by-zero and thus ``nan`` results. **kwargs Extra keyword arguments are passed to :meth:`sympy.physics.mechanics.linearize.Linearizer.linearize`. Explanation =========== If kwarg A_and_B is False (default), returns M, A, B, r for the linearized form, M*[q', u']^T = A*[q_ind, u_ind]^T + B*r. If kwarg A_and_B is True, returns A, B, r for the linearized form dx = A*x + B*r, where x = [q_ind, u_ind]^T. Note that this is computationally intensive if there are many symbolic parameters. For this reason, it may be more desirable to use the default A_and_B=False, returning M, A, and B. Values may then be substituted in to these matrices, and the state space form found as A = P.T*M.inv()*A, B = P.T*M.inv()*B, where P = Linearizer.perm_mat. In both cases, r is found as all dynamicsymbols in the equations of motion that are not part of q, u, q', or u'. They are sorted in canonical form. The operating points may be also entered using the ``op_point`` kwarg. This takes a dictionary of {symbol: value}, or a an iterable of such dictionaries. The values may be numeric or symbolic. The more values you can specify beforehand, the faster this computation will run. For more documentation, please see the ``Linearizer`` class. rNr5)r linearizer)r&rrZkwargs linearizerresultr5r5r6rxs 0 zKanesMethod.linearizec Cs|dur|j}|dur|jdur|j}|gkrd}|js td||}||}|jr|js?t|j |j |j|j|j d}n t|j |j |j|j|j|j |j |j|j|j|j|j d}|j|_||_||}||}|||_|||_|||_|j|jfS)a Method to form Kane's equations, Fr + Fr* = 0. Explanation =========== Returns (Fr, Fr*). In the case where auxiliary generalized speeds are present (say, s auxiliary speeds, o generalized speeds, and m motion constraints) the length of the returned vectors will be o - m + s in length. The first o - m equations will be the constrained Kane's equations, then the s auxiliary Kane's equations. These auxiliary equations can be accessed with the auxiliary_eqs property. Parameters ========== bodies : iterable An iterable of all RigidBody's and Particle's in the system. A system must have at least one body. loads : iterable Takes in an iterable of (Particle, Vector) or (ReferenceFrame, Vector) tuples which represent the force at a point or torque on a frame. Must be either a non-empty iterable of tuples or None which corresponds to a system with no constraints. N[Create an instance of KanesMethod with kinematic differential equations to use this method.)r0r4)r0r-r.r/r4)r1rrqAttributeErrorrrrFrCrrr"r rSr#rQrUrErTrP_km_aux_eqcol_joinrr)r&r1loadsfrfrstarkmfraux frstarauxr5r5r6kanes_equationssB          zKanesMethod.kanes_equationscCs||j|j\}}||Sr9)rbodylistr2)r&rrr5r5r6 _form_eomsszKanesMethod._form_eomscCstt|jt|jd}|}t|jD] \}}||||<q|dur9|j|j |t|jddf<|S|jj |dd|j |t|jddf<|S)aReturns the system's equations of motion in first order form. The output is the right hand side of:: x' = |q'| =: f(q, u, r, p, t) |u'| The right hand side is what is needed by most numerical ODE integrators. Parameters ========== inv_method : str The specific sympy inverse matrix calculation method to use. For a list of valid methods, see :meth:`~sympy.matrices.matrixbase.MatrixBase.inv` rNrT)try_block_diag) rrKr"r# kindiffdictrr mass_matrixLUsolveforcinginv)r& inv_methodrhskdesrrr5r5r6rs zKanesMethod.rhscCs|jstd|jS)z%Returns a dictionary mapping q' to u.r)rPrrr5r5r6r szKanesMethod.kindiffdictcCs(|jr|js td|jstd|jS)z,A matrix containing the auxiliary equations.rz'No auxiliary speeds have been declared.)rrrLrFrrr5r5r6 auxiliary_eqss zKanesMethod.auxiliary_eqscCs|jr|jS|jS)zBThe kinematic "mass matrix" $\mathbf{k_{k\dot{q}}}$ of the system.)rrqrmrr5r5r6mass_matrix_kinszKanesMethod.mass_matrix_kincCs6|jr|jt|j|j S|jt|j|j S)z-The kinematic "forcing vector" of the system.)rrprr#rorlrkrr5r5r6 forcing_kin szKanesMethod.forcing_kincCs$|jr|js tdt|j|jgS)zThe mass matrix of the system.r)rrrLrrrUrr5r5r6r(s zKanesMethod.mass_matrixcCs&|jr|js tdt|j|jg S)z!The forcing vector of the system.r)rrrLrrrTrr5r5r6r/s zKanesMethod.forcingcCsP|jr|js tdt|jt|j}}|jt|| t|||j S)zvThe mass matrix of the system, augmented by the kinematic differential equations in explicit or implicit form.r) rrrLrKr#r"rrirrr)r&r[nr5r5r6mass_matrix_full6s zKanesMethod.mass_matrix_fullcCst|j|jgS)zyThe forcing vector of the system, augmented by the kinematic differential equations in explicit or implicit form.)rrrrr5r5r6 forcing_full@szKanesMethod.forcing_fullcC|jSr9)r@rr5r5r6r"Fz KanesMethod.qcCrr9)rDrr5r5r6r#Jrz KanesMethod.ucCrr9rrr5r5r6rNrzKanesMethod.bodylistcCrr9rrr5r5r6r2RrzKanesMethod.forcelistcCrr9rrr5r5r6r1VrzKanesMethod.bodiescCrr9rrr5r5r6rZrzKanesMethod.loads) NNNNNNNNNTrr)r)NNr9)__name__ __module__ __qualname____doc__r7r!r%r$rrrrrrrrpropertyrrrrrrrr"r#rr2r1rr5r5r5r6rs^=   CH c_ 4: !            N)sympyrrrrsympy.core.sortingrsympy.physics.vectorrrr sympy.physics.mechanics.methodr sympy.physics.mechanics.particler !sympy.physics.mechanics.rigidbodyr !sympy.physics.mechanics.functionsr rrrr!sympy.physics.mechanics.linearizersympy.utilities.iterablesr__all__rr5r5r5r6s