U
    9%eR!                     @   sR   d dl mZmZmZmZmZmZ d dlmZ d dl	m
Z
 dgZG dd deZdS )    )Body
LagrangianKanesMethodLagrangesMethod	RigidBodyParticle)_Methods)MatrixJointsMethodc                   @   s   e Zd ZdZdd Zedd Zedd Zedd	 Zed
d Z	edd Z
edd Zedd Zedd Zedd Zedd Zdd Zdd Zdd Zdd Zd d! Zd"d# Zefd$d%Zd)d'd(Zd&S )*r
   a%  Method for formulating the equations of motion using a set of interconnected bodies with joints.

    Parameters
    ==========

    newtonion : Body or ReferenceFrame
        The newtonion(inertial) frame.
    *joints : Joint
        The joints in the system

    Attributes
    ==========

    q, u : iterable
        Iterable of the generalized coordinates and speeds
    bodies : iterable
        Iterable of Body objects in the system.
    loads : iterable
        Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
        describing the forces on the system.
    mass_matrix : Matrix, shape(n, n)
        The system's mass matrix
    forcing : Matrix, shape(n, 1)
        The system's forcing vector
    mass_matrix_full : Matrix, shape(2*n, 2*n)
        The "mass matrix" for the u's and q's
    forcing_full : Matrix, shape(2*n, 1)
        The "forcing vector" for the u's and q's
    method : KanesMethod or Lagrange's method
        Method's object.
    kdes : iterable
        Iterable of kde in they system.

    Examples
    ========

    This is a simple example for a one degree of freedom translational
    spring-mass-damper.

    >>> from sympy import symbols
    >>> from sympy.physics.mechanics import Body, JointsMethod, PrismaticJoint
    >>> from sympy.physics.vector import dynamicsymbols
    >>> c, k = symbols('c k')
    >>> x, v = dynamicsymbols('x v')
    >>> wall = Body('W')
    >>> body = Body('B')
    >>> J = PrismaticJoint('J', wall, body, coordinates=x, speeds=v)
    >>> wall.apply_force(c*v*wall.x, reaction_body=body)
    >>> wall.apply_force(k*x*wall.x, reaction_body=body)
    >>> method = JointsMethod(wall, J)
    >>> method.form_eoms()
    Matrix([[-B_mass*Derivative(v(t), t) - c*v(t) - k*x(t)]])
    >>> M = method.mass_matrix_full
    >>> F = method.forcing_full
    >>> rhs = M.LUsolve(F)
    >>> rhs
    Matrix([
    [                     v(t)],
    [(-c*v(t) - k*x(t))/B_mass]])

    Notes
    =====

    ``JointsMethod`` currently only works with systems that do not have any
    configuration or motion constraints.

    c                 G   s\   t |tr|j| _n|| _|| _|  | _|  | _|  | _	| 
 | _|  | _d | _d S N)
isinstancer   frame_joints_generate_bodylist_bodies_generate_loadlist_loads_generate_q_q_generate_u_u_generate_kdes_kdes_method)selfZ	newtonionZjoints r   c/var/www/html/Darija-Ai-API/env/lib/python3.8/site-packages/sympy/physics/mechanics/jointsmethod.py__init__N   s    






zJointsMethod.__init__c                 C   s   | j S )zList of bodies in they system.)r   r   r   r   r   bodies]   s    zJointsMethod.bodiesc                 C   s   | j S )zList of loads on the system.)r   r   r   r   r   loadsb   s    zJointsMethod.loadsc                 C   s   | j S z$List of the generalized coordinates.)r   r   r   r   r   qg   s    zJointsMethod.qc                 C   s   | j S )zList of the generalized speeds.)r   r   r   r   r   ul   s    zJointsMethod.uc                 C   s   | j S r!   )r   r   r   r   r   kdesq   s    zJointsMethod.kdesc                 C   s   | j jS )z)The "forcing vector" for the u's and q's.)methodforcing_fullr   r   r   r   r&   v   s    zJointsMethod.forcing_fullc                 C   s   | j jS )z&The "mass matrix" for the u's and q's.)r%   mass_matrix_fullr   r   r   r   r'   {   s    zJointsMethod.mass_matrix_fullc                 C   s   | j jS )zThe system's mass matrix.)r%   mass_matrixr   r   r   r   r(      s    zJointsMethod.mass_matrixc                 C   s   | j jS )zThe system's forcing vector.)r%   forcingr   r   r   r   r)      s    zJointsMethod.forcingc                 C   s   | j S )z3Object of method used to form equations of systems.)r   r   r   r   r   r%      s    zJointsMethod.methodc                 C   s@   g }| j D ]0}|j|kr$||j |j|kr
||j q
|S r   )r   childappendparent)r   r   jointr   r   r   r      s    


zJointsMethod._generate_bodylistc                 C   s    g }| j D ]}||j q
|S r   )r   extendr    )r   	load_listbodyr   r   r   r      s    
zJointsMethod._generate_loadlistc                 C   s>   g }| j D ]*}|jD ]}||kr(td|| qq
t|S )Nz'Coordinates of joints should be unique.)r   Zcoordinates
ValueErrorr+   r	   )r   q_indr-   Z
coordinater   r   r   r      s    

zJointsMethod._generate_qc                 C   s>   g }| j D ]*}|jD ]}||kr(td|| qq
t|S )Nz"Speeds of joints should be unique.)r   Zspeedsr1   r+   r	   )r   u_indr-   speedr   r   r   r      s    

zJointsMethod._generate_uc                 C   s*   t ddg j}| jD ]}||j}q|S )N   r   )r	   Tr   Zcol_joinr$   )r   Zkd_indr-   r   r   r   r      s    
zJointsMethod._generate_kdesc                 C   sr   g }| j D ]b}|jrHt|j|j|j|j|j|jf}|j|_|	| q
t
|j|j|j}|j|_|	| q
|S r   )r   Zis_rigidbodyr   nameZ
masscenterr   ZmassZcentral_inertiaZpotential_energyr+   r   )r   bodylistr0   rbpartr   r   r   _convert_bodies   s    

zJointsMethod._convert_bodiesc                 C   sj   |   }t|tr<t| jf| }||| j| j|| j| _n || j| j| j| j	| j|d| _| j
 }|S )a7  Method to form system's equation of motions.

        Parameters
        ==========

        method : Class
            Class name of method.

        Returns
        ========

        Matrix
            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, Body
        >>> from sympy.physics.mechanics import PrismaticJoint, JointsMethod
        >>> q = dynamicsymbols('q')
        >>> qd = dynamicsymbols('q', 1)
        >>> m, k, b = symbols('m k b')
        >>> wall = Body('W')
        >>> part = Body('P', mass=m)
        >>> part.potential_energy = k * q**2 / S(2)
        >>> J = PrismaticJoint('J', wall, part, coordinates=q, speeds=qd)
        >>> wall.apply_force(b * qd * wall.x, reaction_body=part)
        >>> method = JointsMethod(wall, J)
        >>> method.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.

        >>> method.rhs()
        Matrix([
        [                Derivative(q(t), t)],
        [(-b*Derivative(q(t), t) - k*q(t))/m]])

        )r2   r3   Zkd_eqsZ	forcelistr   )r;   
issubclassr   r   r   r"   r    r   r#   r$   r%   Z
_form_eoms)r   r%   r8   LZsolnr   r   r   	form_eoms   s    -
 
zJointsMethod.form_eomsNc                 C   s   | j j|dS )ax  Returns equations that can be solved numerically.

        Parameters
        ==========

        inv_method : str
            The specific sympy inverse matrix calculation method to use. For a
            list of valid methods, see
            :meth:`~sympy.matrices.matrices.MatrixBase.inv`

        Returns
        ========

        Matrix
            Numerically solvable equations.

        See Also
        ========

        sympy.physics.mechanics.kane.KanesMethod.rhs:
            KanesMethod's rhs function.
        sympy.physics.mechanics.lagrange.LagrangesMethod.rhs:
            LagrangesMethod's rhs function.

        )
inv_method)r%   rhs)r   r?   r   r   r   r@      s    zJointsMethod.rhs)N)__name__
__module____qualname____doc__r   propertyr   r    r"   r#   r$   r&   r'   r(   r)   r%   r   r   r   r   r   r;   r   r>   r@   r   r   r   r   r
   	   s<   D









			7N)Zsympy.physics.mechanicsr   r   r   r   r   r   Zsympy.physics.mechanics.methodr   Zsympy.core.backendr	   __all__r
   r   r   r   r   <module>   s    