o o hA@sddlmZddlmZddlmZddlmZmZm Z ddl m Z ddl m Z ddlmZddlmZmZmZdd lmZdd lmZdd lmZdd lmZmZdd lmZddlm Z ddl!m"Z"m#Z#m$Z$ddl%m&Z&ddl'm(Z(ddgZ)ddZ*GdddeZ+GdddZ,dS)wraps)Basic)ImmutableMatrix)Matrixeyezeros OrderedSet) ActuatorBase)BodyBase) Lagrangian_validate_coordinatesfind_dynamicsymbols)Joint) KanesMethod)LagrangesMethod) _parse_loadgravity)_Methods)Particle)PointReferenceFramedynamicsymbols)iterable) filldedentSymbolicSystemSystemcstfdd}|S)z;Decorator to reset the eom_method if a property is changed.csd|_|g|Ri|SN _eom_method)selfargskwargsmethodr/var/www/html/construction_image-detection-poc/venv/lib/python3.10/site-packages/sympy/physics/mechanics/system.pywrappersz"_reset_eom_method..wrapperr)r%r(r&r$r'_reset_eom_methodsr)c@s<eZdZdZdtddZeddZeddZed d Z ed d Z ed dZ eddZ eddZ e jeddZ eddZejeddZeddZejeddZeddZejeddZeddZedd Zed!d"Zejed#d"Zed$d%Zejed&d%Zed'd(Zejed)d(Zed*d+Zejed,d+Zed-d.Zejed/d.Zed0d1Zejed2d1Zed3d4Zejed5d4Zed6d7Zejed8d7Zed9d:Zejed;d:Zedd?Z ed@dAZ! BdudCdDZ"e EdvdFdGZ#edHdIdJdKZ$edHdIdLdMZ%edNdOZ&edPdQZ'edRdSZ(edTdUZ)edVdWZ*edXdYZ+edZd[Z,ed\d]Z-ed^d_Z.d`daZ/dbdcZ0dddeZ1e2fdfdgZ3dwdhdiZ4edjdkZ5edldmZ6edndoZ7edpdqZ8e2dEfdrdsZ9dS)xraVClass to define a multibody system and form its equations of motion. Explanation =========== A ``System`` instance stores the different objects associated with a model, including bodies, joints, constraints, and other relevant information. With all the relationships between components defined, the ``System`` can be used to form the equations of motion using a backend, such as ``KanesMethod``. The ``System`` has been designed to be compatible with third-party libraries for greater flexibility and integration with other tools. Attributes ========== frame : ReferenceFrame Inertial reference frame of the system. fixed_point : Point A fixed point in the inertial reference frame. x : Vector Unit vector fixed in the inertial reference frame. y : Vector Unit vector fixed in the inertial reference frame. z : Vector Unit vector fixed in the inertial reference frame. q : ImmutableMatrix Matrix of all the generalized coordinates, i.e. the independent generalized coordinates stacked upon the dependent. u : ImmutableMatrix Matrix of all the generalized speeds, i.e. the independent generealized speeds stacked upon the dependent. q_ind : ImmutableMatrix Matrix of the independent generalized coordinates. q_dep : ImmutableMatrix Matrix of the dependent generalized coordinates. u_ind : ImmutableMatrix Matrix of the independent generalized speeds. u_dep : ImmutableMatrix Matrix of the dependent generalized speeds. u_aux : ImmutableMatrix Matrix of auxiliary generalized speeds. kdes : ImmutableMatrix Matrix of the kinematical differential equations as expressions equated to the zero matrix. bodies : tuple of BodyBase subclasses Tuple of all bodies that make up the system. joints : tuple of Joint Tuple of all joints that connect bodies in the system. loads : tuple of LoadBase subclasses Tuple of all loads that have been applied to the system. actuators : tuple of ActuatorBase subclasses Tuple of all actuators present in the system. holonomic_constraints : ImmutableMatrix Matrix with the holonomic constraints as expressions equated to the zero matrix. nonholonomic_constraints : ImmutableMatrix Matrix with the nonholonomic constraints as expressions equated to the zero matrix. velocity_constraints : ImmutableMatrix Matrix with the velocity constraints as expressions equated to the zero matrix. These are by default derived as the time derivatives of the holonomic constraints extended with the nonholonomic constraints. eom_method : subclass of KanesMethod or LagrangesMethod Backend for forming the equations of motion. Examples ======== In the example below a cart with a pendulum is created. The cart moves along the x axis of the rail and the pendulum rotates about the z axis. The length of the pendulum is ``l`` with the pendulum represented as a particle. To move the cart a time dependent force ``F`` is applied to the cart. We first need to import some functions and create some of our variables. >>> from sympy import symbols, simplify >>> from sympy.physics.mechanics import ( ... mechanics_printing, dynamicsymbols, RigidBody, Particle, ... ReferenceFrame, PrismaticJoint, PinJoint, System) >>> mechanics_printing(pretty_print=False) >>> g, l = symbols('g l') >>> F = dynamicsymbols('F') The next step is to create bodies. It is also useful to create a frame for locating the particle with respect to the pin joint later on, as a particle does not have a body-fixed frame. >>> rail = RigidBody('rail') >>> cart = RigidBody('cart') >>> bob = Particle('bob') >>> bob_frame = ReferenceFrame('bob_frame') Initialize the system, with the rail as the Newtonian reference. The body is also automatically added to the system. >>> system = System.from_newtonian(rail) >>> print(system.bodies[0]) rail Create the joints, while immediately also adding them to the system. >>> system.add_joints( ... PrismaticJoint('slider', rail, cart, joint_axis=rail.x), ... PinJoint('pin', cart, bob, joint_axis=cart.z, ... child_interframe=bob_frame, ... child_point=l * bob_frame.y) ... ) >>> system.joints (PrismaticJoint: slider parent: rail child: cart, PinJoint: pin parent: cart child: bob) While adding the joints, the associated generalized coordinates, generalized speeds, kinematic differential equations and bodies are also added to the system. >>> system.q Matrix([ [q_slider], [ q_pin]]) >>> system.u Matrix([ [u_slider], [ u_pin]]) >>> system.kdes Matrix([ [u_slider - q_slider'], [ u_pin - q_pin']]) >>> [body.name for body in system.bodies] ['rail', 'cart', 'bob'] With the kinematics established, we can now apply gravity and the cart force ``F``. >>> system.apply_uniform_gravity(-g * system.y) >>> system.add_loads((cart.masscenter, F * rail.x)) >>> system.loads ((rail_masscenter, - g*rail_mass*rail_frame.y), (cart_masscenter, - cart_mass*g*rail_frame.y), (bob_masscenter, - bob_mass*g*rail_frame.y), (cart_masscenter, F*rail_frame.x)) With the entire system defined, we can now form the equations of motion. Before forming the equations of motion, one can also run some checks that will try to identify some common errors. >>> system.validate_system() >>> system.form_eoms() Matrix([ [bob_mass*l*u_pin**2*sin(q_pin) - bob_mass*l*cos(q_pin)*u_pin' - (bob_mass + cart_mass)*u_slider' + F], [ -bob_mass*g*l*sin(q_pin) - bob_mass*l**2*u_pin' - bob_mass*l*cos(q_pin)*u_slider']]) >>> simplify(system.mass_matrix) Matrix([ [ bob_mass + cart_mass, bob_mass*l*cos(q_pin)], [bob_mass*l*cos(q_pin), bob_mass*l**2]]) >>> system.forcing Matrix([ [bob_mass*l*u_pin**2*sin(q_pin) + F], [ -bob_mass*g*l*sin(q_pin)]]) The complexity of the above example can be increased if we add a constraint to prevent the particle from moving in the horizontal (x) direction. This can be done by adding a holonomic constraint. After which we should also redefine what our (in)dependent generalized coordinates and speeds are. >>> system.add_holonomic_constraints( ... bob.masscenter.pos_from(rail.masscenter).dot(system.x) ... ) >>> system.q_ind = system.get_joint('pin').coordinates >>> system.q_dep = system.get_joint('slider').coordinates >>> system.u_ind = system.get_joint('pin').speeds >>> system.u_dep = system.get_joint('slider').speeds With the updated system the equations of motion can be formed again. >>> system.validate_system() >>> system.form_eoms() Matrix([[-bob_mass*g*l*sin(q_pin) - bob_mass*l**2*u_pin' - bob_mass*l*cos(q_pin)*u_slider' - l*(bob_mass*l*u_pin**2*sin(q_pin) - bob_mass*l*cos(q_pin)*u_pin' - (bob_mass + cart_mass)*u_slider')*cos(q_pin) - l*F*cos(q_pin)]]) >>> simplify(system.mass_matrix) Matrix([ [bob_mass*l**2*sin(q_pin)**2, -cart_mass*l*cos(q_pin)], [ l*cos(q_pin), 1]]) >>> simplify(system.forcing) Matrix([ [-l*(bob_mass*g*sin(q_pin) + bob_mass*l*u_pin**2*sin(2*q_pin)/2 + F*cos(q_pin))], [ l*u_pin**2*sin(q_pin)]]) NcCs |dur td}n t|tstd||_|durtd}n t|ts'td||_|j|jdtddgj|_ tddgj|_ tddgj|_ tddgj|_ tddgj|_ tddgj|_tddgj|_tddgj|_d|_g|_g|_g|_g|_d|_dS)aInitialize the system. Parameters ========== frame : ReferenceFrame, optional The inertial frame of the system. If none is supplied, a new frame will be created. fixed_point : Point, optional A fixed point in the inertial reference frame. If none is supplied, a new fixed_point will be created. Ninertial_framez,Frame must be an instance of ReferenceFrame.inertial_pointz)Fixed point must be an instance of Point.r)r isinstance TypeError_framer _fixed_pointset_velrT_q_ind_q_dep_u_ind_u_dep_u_aux_kdes _hol_coneqs_nonhol_coneqs _vel_constrs_bodies_joints_loads _actuatorsr )r!frame fixed_pointr&r&r'__init__s2     zSystem.__init__cCs0t|tr td||j|jd}|||S)z7Constructs the system with respect to a Newtonian body.z7A Particle has no frame so cannot act as the Newtonian.)r@rA)r-rr.r@ masscenter add_bodies)cls newtoniansystemr&r&r'from_newtonians  zSystem.from_newtoniancC|jS)z,Fixed point in the inertial reference frame.)r0r!r&r&r'rAzSystem.fixed_pointcCrI)z'Inertial reference frame of the system.)r/rJr&r&r'r@!rKz System.framecC|jjSz2Unit vector fixed in the inertial reference frame.)r/xrJr&r&r'rN&zSystem.xcCrLrM)r/yrJr&r&r'rP+rOzSystem.ycCrLrM)r/zrJr&r&r'rQ0rOzSystem.zcC t|jS)z7Tuple of all bodies that have been added to the system.)tupler<rJr&r&r'bodies5 z System.bodiescC&||}||gtdd||_dS)NBodiesrT)_objects_to_list_check_objectsr r<r!rTr&r&r'rT:s  cCrR)z7Tuple of all joints that have been added to the system.)rSr=rJr&r&r'jointsArUz System.jointscCs0||}||gtddg|_|j|dS)NJointsr[)rXrYrr= add_joints)r!r[r&r&r'r[Fs cCrR)z4Tuple of loads that have been applied on the system.)rSr>rJr&r&r'loadsNrUz System.loadscCs||}dd|D|_dS)NcSg|]}t|qSr&r.0loadr&r&r' Wz System.loads..)rXr>r!r^r&r&r'r^Ss cCrR)z)Tuple of actuators present in the system.)rSr?rJr&r&r' actuatorsYrUzSystem.actuatorscCrV)N Actuatorsrg)rXrYr r?r!rgr&r&r'rg^s   cC|j|jS)zbMatrix of all the generalized coordinates with the independent stacked upon the dependent.)r3col_joinr4rJr&r&r'qfzSystem.qcCrj)z]Matrix of all the generalized speeds with the independent stacked upon the dependent.)r5rkr6rJr&r&r'ulrmzSystem.ucCrI)z2Matrix of the independent generalized coordinates.)r3rJr&r&r'q_indrrKz System.q_indcC&|||dg|jd\|_|_dS)NT coordinates)_parse_coordinatesrXq_depr3r4)r!ror&r&r'rowcCrI)z0Matrix of the dependent generalized coordinates.)r4rJr&r&r'rs}rKz System.q_depcC&|||d|jgd\|_|_dS)NFrq)rrrXror3r4)r!rsr&r&r'rsrtcCrI)z-Matrix of the independent generalized speeds.)r5rJr&r&r'u_indrKz System.u_indcCrp)NTspeeds)rrrXu_depr5r6)r!rvr&r&r'rvrtcCrI)z+Matrix of the dependent generalized speeds.)r6rJr&r&r'rxrKz System.u_depcCru)NFrw)rrrXrvr5r6)r!rxr&r&r'rxrtcCrI)z'Matrix of auxiliary generalized speeds.)r7rJr&r&r'u_auxrKz System.u_auxcCs"|||dggdd|_dS)NT u_auxiliaryr)rrrXr7)r!ryr&r&r'rys  cCrI)zKinematical differential equations as expressions equated to the zero matrix. These equations describe the coupling between the generalized coordinates and the generalized speeds.)r8rJr&r&r'kdessz System.kdescC||}||gd|_dS)N kinematic differential equations)rX_parse_expressionsr8r!r{r&r&r'r{  cCrI)zXMatrix with the holonomic constraints as expressions equated to the zero matrix.)r9rJr&r&r'holonomic_constraintszSystem.holonomic_constraintscCr|)Nholonomic constraints)rXr~r9r! constraintsr&r&r'rrcCrI)z[Matrix with the nonholonomic constraints as expressions equated to the zero matrix.)r:rJr&r&r'nonholonomic_constraintsrzSystem.nonholonomic_constraintscCr|)Nnonholonomic constraints)rXr~r:rr&r&r'rrcCs&|jdur|jtj|jS|jS)zMatrix with the velocity constraints as expressions equated to the zero matrix. The velocity constraints are by default derived from the holonomic and nonholonomic constraints unless they are explicitly set. N)r;rdiffr_trkrrJr&r&r'velocity_constraintss zSystem.velocity_constraintscCs0|dur d|_dS||}||gd|_dS)Nzvelocity constraints)r;rXr~rr&r&r'rs  cCrI)z,Backend for forming the equations of motion.rrJr&r&r' eom_methodrKzSystem.eom_methodcCst|s|gSt|ddS)z+Helper to convert passed objects to a list.N)rlist)lstr&r&r'rXszSystem._objects_to_listc Cst|}t}t}|D]}t||s||||vr"||q ||q |r7t|d|d|d|rCt|d|ddS)aHelper to check the objects that are being added to the system. Explanation =========== This method checks that the objects that are being added to the system are of the correct type and have not already been added. If any of the objects are not of the correct type or have already been added, then an error is raised. Parameters ========== objects : iterable The objects that would be added to the system. obj_lst : list The list of objects that are already in the system. expected_type : type The type that the objects should be. obj_name : str The name of the category of objects. This string is used to formulate the error message for the user. type_name : str The name of the type that the objects should be. This string is used to formulate the error message for the user.  z are not .z' have already been added to the system.N)setr-addr. ValueError) objectsobj_lst expected_typeobj_name type_nameseen duplicates wrong_typesobjr&r&r'rYs    zSystem._check_objectsrqc Cs|dd|dd}}t|s|gt|}t||D]\}} | r)||q||qd|jdd|jddd|jdd|jddd|jdd|||i} t di| t dt||j t dt||j fS)z'Helper to parse coordinates and speeds.Nrqrwrzr,r&) rlenzipappendrorsrvrxr7rrr2) r! new_coords independentold_coords_indold_coords_dep coord_type coords_ind coords_depcoordindepcurrentr&r&r'rrs   zSystem._parse_coordinatesFcCs|dd}t|}|r|dd|D}n|}t||t|d|D]}|dkr1td|dq#tdt|t|||jS) z-Helper to parse expressions like constraints.NcSsg|]}| qSr&r&)rbexprr&r&r'rd9sz-System._parse_expressions.. expressionsrzParsed z are zero.r,)rrrYrrrrr2)new_expressionsold_expressionsnamecheck_negatives check_exprsrr&r&r'r~2s"  zSystem._parse_expressionsT)rcG"||||j|jd\|_|_dS)aAdd generalized coordinate(s) to the system. Parameters ========== *coordinates : dynamicsymbols One or more generalized coordinates to be added to the system. independent : bool or list of bool, optional Boolean whether a coordinate is dependent or independent. The default is True, so the coordinates are added as independent by default. rqN)rrrorsr3r4)r!rrqr&r&r'add_coordinatesDszSystem.add_coordinatescGr)aAdd generalized speed(s) to the system. Parameters ========== *speeds : dynamicsymbols One or more generalized speeds to be added to the system. independent : bool or list of bool, optional Boolean whether a speed is dependent or independent. The default is True, so the speeds are added as independent by default. rwN)rrrvrxr5r6)r!rrwr&r&r' add_speedsVszSystem.add_speedscGs||d|jgdd|_dS)zAdd auxiliary speed(s) to the system. Parameters ========== *speeds : dynamicsymbols One or more auxiliary speeds to be added to the system. TrzrN)rrr7)r!rwr&r&r'add_auxiliary_speedsgs   zSystem.add_auxiliary_speedscGs|j||jddd|_dS)zAdd kinematic differential equation(s) to the system. Parameters ========== *kdes : Expr One or more kinematic differential equations. r}TrN)r~r{r8rr&r&r'add_kdesus  zSystem.add_kdescG|j||jddd|_dS)zAdd holonomic constraint(s) to the system. Parameters ========== *constraints : Expr One or more holonomic constraints, which are expressions that should be zero. rTrN)r~r9rr&r&r'add_holonomic_constraints  z System.add_holonomic_constraintscGr)zAdd nonholonomic constraint(s) to the system. Parameters ========== *constraints : Expr One or more nonholonomic constraints, which are expressions that should be zero. rTrN)r~r:rr&r&r'add_nonholonomic_constraintsrz#System.add_nonholonomic_constraintscG$|||jtdd|j|dS)zAdd body(ies) to the system. Parameters ========== bodies : Particle or RigidBody One or more bodies. rWrTN)rYrTr r<extendrZr&r&r'rDs zSystem.add_bodiescGsdd|D}|j|dS)zAdd load(s) to the system. Parameters ========== *loads : Force or Torque One or more loads. cSr_r&r`rar&r&r'rdrez$System.add_loads..N)r>rrfr&r&r' add_loadss zSystem.add_loadscCs|jt|g|jRdS)zApply uniform gravity to all bodies in the system by adding loads. Parameters ========== acceleration : Vector The acceleration due to gravity. N)rrrT)r! accelerationr&r&r'apply_uniform_gravitys zSystem.apply_uniform_gravitycGr)zAdd actuator(s) to the system. Parameters ========== *actuators : subclass of ActuatorBase One or more actuators. rhrgN)rYrgr r?rrir&r&r' add_actuatorss zSystem.add_actuatorscGs|||jtdd|j|ddtdD\}}}}|D]}||j||j||j ||j |j fq| |j }| |j}| |j dd|j dd}| |j}|jt||jt||jddt|D|jt|dS)aAdd joint(s) to the system. Explanation =========== This methods adds one or more joints to the system including its associated objects, i.e. generalized coordinates, generalized speeds, kinematic differential equations and the bodies. Parameters ========== *joints : subclass of Joint One or more joints. Notes ===== For the generalized coordinates, generalized speeds and bodies it is checked whether they are already known by the system instance. If they are, then they are not added. The kinematic differential equations are however always added to the system, so you should not also manually add those on beforehand. r\r[css|]}tVqdSrr )rb_r&r&r' sz$System.add_joints..Ncss|] }|dks|VqdS)rNr&)rbkder&r&r'rs)rYr[rr=rrangeupdaterqrwr{parentchild differencerlrnrTrrSrrrD)r!r[rqrwr{rTjointr&r&r'r]s       $ zSystem.add_jointscC"|jD] }|j|kr|SqdS)a%Retrieve a body from the system by name. Parameters ========== name : str The name of the body to retrieve. Returns ======= RigidBody or Particle The body with the given name, or None if no such body exists. N)r<r)r!rbodyr&r&r'get_body  zSystem.get_bodycCr)a%Retrieve a joint from the system by name. Parameters ========== name : str The name of the joint to retrieve. Returns ======= subclass of Joint The joint with the given name, or None if no such joint exists. N)r=r)r!rrr&r&r' get_jointrzSystem.get_jointcCs|Sr) form_eomsrJr&r&r' _form_eoms0szSystem._form_eomsc KsL|jtdd|jD}|r|nd}t|trShd}||}|r/td|jd|d|j|j |j |j |j |j |j|j|j||jdd |}|di||_nNt|trhd }||}|rotd|jd|d|j|j||j|j|jd |}d |vrt|d g|dR|d <|di||_nt|d|jS)aForm the equations of motion of the system. Parameters ========== eom_method : subclass of KanesMethod or LagrangesMethod Backend class to be used for forming the equations of motion. The default is ``KanesMethod``. Returns ======== ImmutableMatrix Vector of equations of motions. Examples ======== This is a simple example for a one degree of freedom translational spring-mass-damper. >>> from sympy import S, symbols >>> from sympy.physics.mechanics import ( ... LagrangesMethod, dynamicsymbols, PrismaticJoint, Particle, ... RigidBody, System) >>> q = dynamicsymbols('q') >>> qd = dynamicsymbols('q', 1) >>> m, k, b = symbols('m k b') >>> wall = RigidBody('W') >>> system = System.from_newtonian(wall) >>> bob = Particle('P', mass=m) >>> bob.potential_energy = S.Half * k * q**2 >>> system.add_joints(PrismaticJoint('J', wall, bob, q, qd)) >>> system.add_loads((bob.masscenter, b * qd * system.x)) >>> system.form_eoms(LagrangesMethod) Matrix([[-b*Derivative(q(t), t) + k*q(t) + m*Derivative(q(t), (t, 2))]]) We can also solve for the states using the 'rhs' method. >>> system.rhs() Matrix([ [ Derivative(q(t), t)], [(b*Derivative(q(t), t) - k*q(t))/m]]) css"|] }|D]}|VqqdSr)to_loads)rbactrcr&r&r'rbs  z#System.form_eoms..N> r@rorvrTkd_eqs forcelist q_dependentrz u_dependentrconfiguration_constraintszEThe following keyword arguments are not allowed to be overwritten in z: rF) r@rorvrrrrrrzrrTexplicit_kinematics>qsr@rTrr hol_coneqs nonhol_coneqs)r@rrrTrrr r@rT has not been implemented.r&)r^rSrg issubclassr intersectionr__name__r@rorvr{rsrxrrryrTr rrlrr NotImplementedErrorrr)r!rr#r^disallowed_kwargs wrong_kwargsr&r&r'r3sh /         zSystem.form_eomscCs|jj|dS)aCompute the equations of motion in the explicit form. 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` Returns ======== ImmutableMatrix Equations of motion in the explicit form. See Also ======== sympy.physics.mechanics.kane.KanesMethod.rhs: KanesMethod's ``rhs`` function. sympy.physics.mechanics.lagrange.LagrangesMethod.rhs: LagrangesMethod's ``rhs`` function. ) inv_method)rrhs)r!rr&r&r'rsz System.rhscCrL)abThe mass matrix of the system. Explanation =========== The mass matrix $M_d$ and the forcing vector $f_d$ of a system describe the system's dynamics according to the following equations: .. math:: M_d \dot{u} = f_d where $\dot{u}$ is the time derivative of the generalized speeds. )r mass_matrixrJr&r&r'rszSystem.mass_matrixcCrL)aThe mass matrix of the system, augmented by the kinematic differential equations in explicit or implicit form. Explanation =========== The full mass matrix $M_m$ and the full forcing vector $f_m$ of a system describe the dynamics and kinematics according to the following equation: .. math:: M_m \dot{x} = f_m where $x$ is the state vector stacking $q$ and $u$. )rmass_matrix_fullrJr&r&r'rszSystem.mass_matrix_fullcCrL)z!The forcing vector of the system.)rforcingrJr&r&r'rrOzSystem.forcingcCrL)zyThe forcing vector of the system, augmented by the kinematic differential equations in explicit or implicit form.)r forcing_fullrJr&r&r'rszSystem.forcing_fullc sg}|jjd}|jjd}|jjd|jjd}}t|jt|j}} t|t| } } ||krA| t d|d|dt} |j D] } | t| j |qG| rb| t d| dt|trt|j}tt}}|j D]%} | t| j| | t| j|jdd|j ddqv|r| t d|d |r| t d |d ||kr| t d |d |d| | kr| t d| d| d| |kr| t d| d|dnPt|tr6t|j|jtj}|j D]} | t| j|jtjq|r%| t d|d|jr5| t d|jdnt|d|r{d|jfd|jfd|jfd|jfd|j fg}|D]"\}}tfdd|D}|ry| t d|d|dqX|rtd|dS) aValidates the system using some basic checks. Explanation =========== This method validates the system based on the following checks: - The number of dependent generalized coordinates should equal the number of holonomic constraints. - All generalized coordinates defined by the joints should also be known to the system. - If ``KanesMethod`` is used as a ``eom_method``: - All generalized speeds and kinematic differential equations defined by the joints should also be known to the system. - The number of dependent generalized speeds should equal the number of velocity constraints. - The number of generalized coordinates should be less than or equal to the number of generalized speeds. - The number of generalized coordinates should equal the number of kinematic differential equations. - If ``LagrangesMethod`` is used as ``eom_method``: - There should not be any generalized speeds that are not derivatives of the generalized coordinates (this includes the generalized speeds defined by the joints). Parameters ========== eom_method : subclass of KanesMethod or LagrangesMethod Backend class that will be used for forming the equations of motion. There are different checks for the different backends. The default is ``KanesMethod``. check_duplicates : bool Boolean whether the system should be checked for duplicate definitions. The default is False, because duplicates are already checked when adding objects to the system. Notes ===== This method is not guaranteed to be backwards compatible as it may improve over time. The method can become both more and less strict in certain areas. However a well-defined system should always pass all these tests. rz= The number of dependent generalized coordinates zD should be equal to the number of holonomic constraints rz) The generalized coordinates z8 used in joints are not added to the system.Nz( The generalized speeds z< used in joints are not added to the system.z6 The kinematic differential equations z< used in joints are not added to the system.z< The number of dependent generalized speeds zG should be equal to the number of velocity constraints z7 The number of generalized coordinates zR should be less than or equal to the number of generalized speeds z2 The number of generalized speeds zS should be equal to the number of kinematic differential equations z are not supported by this method. Only derivatives of the generalized coordinates are supported. If these symbols are used in your expressions, then this will result in wrong equations of motion.z This method does not support auxiliary speeds. If these symbols are used in your expressions, then this will result in wrong equations of motion. The auxiliary speeds are rzgeneralized coordinateszgeneralized speedszauxiliary speedsrTr[cs"h|] }|vs |r|qSr&)r)rbrNrr&r' Ss"z)System.validate_system..z The rz< exist multiple times within the system. )rshaperrsrxrrlrnrrrr[rrqrrrr{rwrrrrryrrTrjoin)r!rcheck_duplicatesmsgsn_hcn_vcn_q_depn_u_depq_setu_setn_qn_u missing_qrn_kdes missing_kdes missing_u not_qdotsduplicates_to_checkrrrr&rr'validate_systems/                  zSystem.validate_system)NN)rq)Fr):r __module__ __qualname____doc__rB classmethodrHpropertyrAr@rNrPrQrTsetterr)r[r^rgrlrnrorsrvrxryr{rrrr staticmethodrXrYrrr~rrrrrrrDrrrr]rrrrrrrrrrrr&r&r&r'r#s G(                                      +           +  Z    c @seZdZdZddddiddddf ddZeddZeddZed d Zed d Z ed dZ eddZ eddZ eddZ ddZeddZeddZddZddZedd Zed!d"ZdS)#raB"SymbolicSystem is a class that contains all the information about a system in a symbolic format such as the equations of motions and the bodies and loads in the system. There are three ways that the equations of motion can be described for Symbolic System: [1] Explicit form where the kinematics and dynamics are combined x' = F_1(x, t, r, p) [2] Implicit form where the kinematics and dynamics are combined M_2(x, p) x' = F_2(x, t, r, p) [3] Implicit form where the kinematics and dynamics are separate M_3(q, p) u' = F_3(q, u, t, r, p) q' = G(q, u, t, r, p) where x : states, e.g. [q, u] t : time r : specified (exogenous) inputs p : constants q : generalized coordinates u : generalized speeds F_1 : right hand side of the combined equations in explicit form F_2 : right hand side of the combined equations in implicit form F_3 : right hand side of the dynamical equations in implicit form M_2 : mass matrix of the combined equations in implicit form M_3 : mass matrix of the dynamical equations in implicit form G : right hand side of the kinematical differential equations Parameters ========== coord_states : ordered iterable of functions of time This input will either be a collection of the coordinates or states of the system depending on whether or not the speeds are also given. If speeds are specified this input will be assumed to be the coordinates otherwise this input will be assumed to be the states. right_hand_side : Matrix This variable is the right hand side of the equations of motion in any of the forms. The specific form will be assumed depending on whether a mass matrix or coordinate derivatives are given. speeds : ordered iterable of functions of time, optional This is a collection of the generalized speeds of the system. If given it will be assumed that the first argument (coord_states) will represent the generalized coordinates of the system. mass_matrix : Matrix, optional The matrix of the implicit forms of the equations of motion (forms [2] and [3]). The distinction between the forms is determined by whether or not the coordinate derivatives are passed in. If they are given form [3] will be assumed otherwise form [2] is assumed. coordinate_derivatives : Matrix, optional The right hand side of the kinematical equations in explicit form. If given it will be assumed that the equations of motion are being entered in form [3]. alg_con : Iterable, optional The indexes of the rows in the equations of motion that contain algebraic constraints instead of differential equations. If the equations are input in form [3], it will be assumed the indexes are referencing the mass_matrix/right_hand_side combination and not the coordinate_derivatives. output_eqns : Dictionary, optional Any output equations that are desired to be tracked are stored in a dictionary where the key corresponds to the name given for the specific equation and the value is the equation itself in symbolic form coord_idxs : Iterable, optional If coord_states corresponds to the states rather than the coordinates this variable will tell SymbolicSystem which indexes of the states correspond to generalized coordinates. speed_idxs : Iterable, optional If coord_states corresponds to the states rather than the coordinates this variable will tell SymbolicSystem which indexes of the states correspond to generalized speeds. bodies : iterable of Body/Rigidbody objects, optional Iterable containing the bodies of the system loads : iterable of load instances (described below), optional Iterable containing the loads of the system where forces are given by (point of application, force vector) and torques are given by (reference frame acting upon, torque vector). Ex [(point, force), (ref_frame, torque)] Attributes ========== coordinates : Matrix, shape(n, 1) This is a matrix containing the generalized coordinates of the system speeds : Matrix, shape(m, 1) This is a matrix containing the generalized speeds of the system states : Matrix, shape(o, 1) This is a matrix containing the state variables of the system alg_con : List This list contains the indices of the algebraic constraints in the combined equations of motion. The presence of these constraints requires that a DAE solver be used instead of an ODE solver. If the system is given in form [3] the alg_con variable will be adjusted such that it is a representation of the combined kinematics and dynamics thus make sure it always matches the mass matrix entered. dyn_implicit_mat : Matrix, shape(m, m) This is the M matrix in form [3] of the equations of motion (the mass matrix or generalized inertia matrix of the dynamical equations of motion in implicit form). dyn_implicit_rhs : Matrix, shape(m, 1) This is the F vector in form [3] of the equations of motion (the right hand side of the dynamical equations of motion in implicit form). comb_implicit_mat : Matrix, shape(o, o) This is the M matrix in form [2] of the equations of motion. This matrix contains a block diagonal structure where the top left block (the first rows) represent the matrix in the implicit form of the kinematical equations and the bottom right block (the last rows) represent the matrix in the implicit form of the dynamical equations. comb_implicit_rhs : Matrix, shape(o, 1) This is the F vector in form [2] of the equations of motion. The top part of the vector represents the right hand side of the implicit form of the kinemaical equations and the bottom of the vector represents the right hand side of the implicit form of the dynamical equations of motion. comb_explicit_rhs : Matrix, shape(o, 1) This vector represents the right hand side of the combined equations of motion in explicit form (form [1] from above). kin_explicit_rhs : Matrix, shape(m, 1) This is the right hand side of the explicit form of the kinematical equations of motion as can be seen in form [3] (the G matrix). output_eqns : Dictionary If output equations were given they are stored in a dictionary where the key corresponds to the name given for the specific equation and the value is the equation itself in symbolic form bodies : Tuple If the bodies in the system were given they are stored in a tuple for future access loads : Tuple If the loads in the system were given they are stored in a tuple for future access. This includes forces and torques where forces are given by (point of application, force vector) and torques are given by (reference frame acted upon, torque vector). Example ======= As a simple example, the dynamics of a simple pendulum will be input into a SymbolicSystem object manually. First some imports will be needed and then symbols will be set up for the length of the pendulum (l), mass at the end of the pendulum (m), and a constant for gravity (g). :: >>> from sympy import Matrix, sin, symbols >>> from sympy.physics.mechanics import dynamicsymbols, SymbolicSystem >>> l, m, g = symbols('l m g') The system will be defined by an angle of theta from the vertical and a generalized speed of omega will be used where omega = theta_dot. :: >>> theta, omega = dynamicsymbols('theta omega') Now the equations of motion are ready to be formed and passed to the SymbolicSystem object. :: >>> kin_explicit_rhs = Matrix([omega]) >>> dyn_implicit_mat = Matrix([l**2 * m]) >>> dyn_implicit_rhs = Matrix([-g * l * m * sin(theta)]) >>> symsystem = SymbolicSystem([theta], dyn_implicit_rhs, [omega], ... dyn_implicit_mat) Notes ===== m : number of generalized speeds n : number of generalized coordinates o : number of states Nc s|dur6t|_|durd|_nfdd|D} t| |_| dur'd|_n!fdd| D} t| |_nt|_t||_|j|j|_dur_|_||_||_d|_d|_ d|_ n)|durvd|_d|_d|_||_||_ d|_ nd|_d|_d|_d|_d|_ ||_ |durdurfdd|D}||_ ||_ t | ts| durt| } t | ts| durt| } | |_| |_dS)z#Initializes a SymbolicSystem objectNcg|]}|qSr&r&rbi coord_statesr&r'rd2rez+SymbolicSystem.__init__..crr&r&rr r&r'rd8recsg|]}|tqSr&)rr)coordinate_derivativesr&r'rdXs)r_states _coordinates_speedsrk_kin_explicit_rhs_dyn_implicit_rhs_dyn_implicit_mat_comb_implicit_rhs_comb_implicit_mat_comb_explicit_rhs_alg_con output_eqnsr-rSr<r>)r!r right_hand_siderwrr alg_conr coord_idxs speed_idxsrTr^coords speeds_interr&)r r r'rB%sV      zSymbolicSystem.__init__cC|jdur td|jS)z8Returns the column matrix of the generalized coordinatesNz#The coordinates were not specified.)r AttributeErrorrJr&r&r'rqe zSymbolicSystem.coordinatescCr)z/Returns the column matrix of generalized speedsNzThe speeds were not specified.)rrrJr&r&r'rwmrzSymbolicSystem.speedscCrI)z0Returns the column matrix of the state variables)r rJr&r&r'statesurKzSymbolicSystem.statescCrI)zReturns a list with the indices of the rows containing algebraic constraints in the combined form of the equations of motion)rrJr&r&r'rzrzSymbolicSystem.alg_concCr)zReturns the matrix, M, corresponding to the dynamic equations in implicit form, M x' = F, where the kinematical equations are not includedNzJdyn_implicit_mat is not specified for equations of motion form [1] or [2].)rrrJr&r&r'dyn_implicit_mat zSymbolicSystem.dyn_implicit_matcCr)zReturns the column matrix, F, corresponding to the dynamic equations in implicit form, M x' = F, where the kinematical equations are not includedNzJdyn_implicit_rhs is not specified for equations of motion form [1] or [2].)rrrJr&r&r'dyn_implicit_rhsr"zSymbolicSystem.dyn_implicit_rhscCsv|jdur8|jdur4t|j}t|j}t||}t||}t||}||j}|||_|jSt d|jS)zReturns the matrix, M, corresponding to the equations of motion in implicit form (form [2]), M x' = F, where the kinematical equations are includedNzDcomb_implicit_mat is not specified for equations of motion form [1].) rrrrrrrrow_joinrkr)r! num_kin_eqns num_dyn_eqnszeros1zeros2inter1inter2r&r&r'comb_implicit_mats        z SymbolicSystem.comb_implicit_matcCs@|jdur|jdur|j}|j}|||_|jStd|jS)zReturns the column matrix, F, corresponding to the equations of motion in implicit form (form [2]), M x' = F, where the kinematical equations are includedNzGcomb_implicit_mat is not specified for equations of motion in form [1].)rrrrkr)r! kin_inter dyn_interr&r&r'comb_implicit_rhss   z SymbolicSystem.comb_implicit_rhscCsX|jdur tdt|dd}|dur |j|j}||}n|j|j}||_dS)zIf the explicit right hand side of the combined equations of motion is to provided upon initialization, this method will calculate it. This calculation can potentially take awhile to compute.Nz$comb_explicit_rhs is already formed.kin_explicit_rhs) rrgetattrrLUsolverrkrr)r!r)r*outr&r&r'compute_explicit_forms    z$SymbolicSystem.compute_explicit_formcCr)zReturns the right hand side of the equations of motion in explicit form, x' = F, where the kinematical equations are includedNzPPlease run .combute_explicit_form before attempting to access comb_explicit_rhs.)rrrJr&r&r'comb_explicit_rhs z SymbolicSystem.comb_explicit_rhscCr)zYReturns the right hand side of the kinematical equations in explicit form, q' = GNzJkin_explicit_rhs is not specified for equations of motion form [1] or [2].)rrrJr&r&r'r/r5zSymbolicSystem.kin_explicit_rhscCsh|jdur|jdd|jdd}n|jdd}t}|D] }|t|}q ||j}t|S)z_Returns a column matrix containing all of the symbols in the system that depend on timeN)rr+r.runionrr rS)r!eom_expressionsfunctions_of_timerr&r&r'dynamic_symbolss    zSymbolicSystem.dynamic_symbolscCsf|jdur|jdd|jdd}n|jdd}t}|D]}||j}q |tjt |S)zfReturns a column matrix containing all of the symbols in the system that do not depend on timeN) rr+r.rr6 free_symbolsremoverrrS)r!r7 constantsrr&r&r'constant_symbolss    zSymbolicSystem.constant_symbolscCr)z Returns the bodies in the systemNz)bodies were not specified for the system.)r<rrJr&r&r'rTrzSymbolicSystem.bodiescCr)zReturns the loads in the systemNz(loads were not specified for the system.)r>rrJr&r&r'r^ rzSymbolicSystem.loads)rrrrrBrrqrwr rr!r#r+r.r3r4r/r9r=rTr^r&r&r&r'r\sFI @            N)- functoolsrsympy.core.basicrsympy.matrices.immutablersympy.matrices.denserrrsympy.core.containersr sympy.physics.mechanics.actuatorr !sympy.physics.mechanics.body_baser !sympy.physics.mechanics.functionsr rrsympy.physics.mechanics.jointrsympy.physics.mechanics.kaner sympy.physics.mechanics.lagrangersympy.physics.mechanics.loadsrrsympy.physics.mechanics.methodr sympy.physics.mechanics.particlersympy.physics.vectorrrrsympy.utilities.iterablesrsympy.utilities.miscr__all__r)rrr&r&r&r's:             A