
    Edo                         d dl mZmZmZmZ d dlmZ d dlmZm	Z	m
Z
 d dlmZ d dlmZ d dlmZ d dlmZmZmZ d dlmZ d d	lmZ d
gZ G d d
e          ZdS )    )zerosMatrixdiffeye)default_sort_key)ReferenceFramedynamicsymbolspartial_velocity)_Methods)Particle)	RigidBody)msubsfind_dynamicsymbols_f_list_parser)
Linearizer)iterableKanesMethodc                   `   e Zd ZdZ	 	 	 	 ddZd Zd Zd Zd Zd Z	d	 Z
dd
dZddZd ZddZd Zed             Zed             Zed             Zed             Zed             Zed             Zed             Zed             Zed             Zed             Zed             ZdS )r   a9  Kane's method object.

    Explanation
    ===========

    This object is used to do the "book-keeping" as you go through and form
    equations of motion in the way Kane presents in:
    Kane, T., Levinson, D. Dynamics Theory and Applications. 1985 McGraw-Hill

    The attributes are for equations in the form [M] udot = forcing.

    Attributes
    ==========

    q, u : Matrix
        Matrices of the generalized coordinates and speeds
    bodies : iterable
        Iterable of Point and RigidBody objects in the system.
    loads : iterable
        Iterable of (Point, vector) or (ReferenceFrame, vector) tuples
        describing the forces on the system.
    auxiliary : Matrix
        If applicable, the set of auxiliary Kane's
        equations used to solve for non-contributing
        forces.
    mass_matrix : Matrix
        The system's mass matrix
    forcing : Matrix
        The system's forcing vector
    mass_matrix_full : Matrix
        The "mass matrix" for the u's and q's
    forcing_full : Matrix
        The "forcing vector" for the u's and q's

    Examples
    ========

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

    In this example, we first need to do the kinematics.
    This involves creating generalized speeds and coordinates and their
    derivatives.
    Then we create a point and set its velocity in a frame.

        >>> 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 need to be stored in a
    dict.  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.

    Nc                 b   |s t          d          g}t          d          g}t          |t                    st          d          || _        d| _        d| _        || _        || _        | 	                    |||||
           | 
                    |           |                     |||	           dS )z&Please read the online documentation. dummy_qdummy_kdz+An inertial ReferenceFrame must be suppliedN)r	   
isinstancer   	TypeError	_inertial_fr_frstar
_forcelist	_bodylist_initialize_vectors_initialize_kindiffeq_matrices_initialize_constraint_matrices)selfframeq_indu_indkd_eqsq_dependentconfiguration_constraintsu_dependentvelocity_constraintsacceleration_constraintsu_auxiliarybodies	forcelists                <lib/python3.11/site-packages/sympy/physics/mechanics/kane.py__init__zKanesMethod.__init__u   s      	2#I../E$Z001F%00 	KIJJJ#  UK	 	 	++F333,,-F$&>	@ 	@ 	@ 	@ 	@    c                    d } ||          }t          |          st          d          t          |          st          d          t          |          }|| _        t          ||g          | _        | j                            t          j                  | _	         ||          }t          |          st          d          t          |          st          d          t          |          }|| _
        t          ||g          | _        | j                            t          j                  | _         ||          | _        dS )z,Initialize the coordinate and speed vectors.c                 @    | rt          |           nt                      S Nr   xs    r/   <lambda>z1KanesMethod._initialize_vectors.<locals>.<lambda>       a!=VXX r1   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)r   r   r   _qdep_qqr   r	   _t_qdot_udep_uu_udot_uaux)r"   r$   q_depr%   u_depu_auxnone_handlers          r/   r   zKanesMethod._initialize_vectors   s5    >= U## 	LJKKK 	JHIIIu
%((V[[!233
 U## 	GEFFF 	ECDDDu
%((V[[!233
!\%((


r1   c                    t          | j                  }t          | j                  }||z
  }d } ||          }t          | j                  t          |          k    rt	          d           ||          | _         ||          } ||          }t          |          |k    rt	          d          |r"t          |          |k    rt	          d          |rd | j        D             }d | j        D             }	| j        t          || j                  }t          ||          | _	        || j	        z
  
                    | j                  | _        |s| j                            t          j                  | j        z  | j	                            t          j                  z   }
| j        t          |
| j                  }
|
| _        | j        | _        nX| j        t          || j                  }t          ||	          | _        || j        z
  
                    | j                  | _        | j        ddd|f         }| j        dd||f         }|                    |           | _        dS t'                      | _	        t'                      | _        t'                      | _        t'                      | _        t'                      | _        dS )z Initializes constraint matrices.c                 @    | rt          |           nt                      S r4   r5   r6   s    r/   r8   z=KanesMethod._initialize_constraint_matrices.<locals>.<lambda>   r9   r1   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.c                     i | ]}|d S r    .0is     r/   
<dictcomp>z?KanesMethod._initialize_constraint_matrices.<locals>.<dictcomp>   s    +++qa+++r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z?KanesMethod._initialize_constraint_matrices.<locals>.<dictcomp>   s    222!A222r1   N)lenrA   r?   r:   
ValueError_f_hrB   _qdot_u_mapr   _f_nhjacobian_k_nhr   r	   r=   _f_dnh_k_dnhLUsolve_Arsr   )r"   configvelaccomprG   u_zero	udot_zerorY   B_indB_deps                r/   r!   z+KanesMethod._initialize_constraint_matrices   s    KK
OOE== f%%tz??c&kk) 	K J K K K L((	 l3l3s88q= 	A @ A A A 	ECHHM 	E D E E E (	!++DF+++F22tz222I  3C!122sF++DJ
*44TV<<DJ G*//.*;<<tvE*//.*;<<=# ="64+;<<F$"j# 7T%566C#C33"T[0::4:FF Jqqq"1"u%EJqqq!A#v&Eu---DIIIDJDJ ((DK ((DKDIIIr1   c                 v   |rt          | j                  t          |          k    rt          d          | j        }| j        }t          |          }d |D             }d | j        D             }d |D             }|                    |          }|                    |          }|                    |                              |          }	t          |
                    |          
                    |	                    fd|dd         |dd         z   D             }
|
r$d}t          |                    |
                    |                    |	          }	|                    |          }t          t          |                    }t          t          |||z  |	z                        | _        |	                    |          | _        |                    |          | _        || _        dS d| _        t                      | _        t                      | _        t                      | _        dS )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.c                     i | ]}|d S rK   rL   )rN   uis     r/   rP   z>KanesMethod._initialize_kindiffeq_matrices.<locals>.<dictcomp>  s    (((b!(((r1   c                     i | ]}|d S rK   rL   )rN   uais     r/   rP   z>KanesMethod._initialize_kindiffeq_matrices.<locals>.<dictcomp>  s    666Ca666r1   c                     i | ]}|d S rK   rL   )rN   qdis     r/   rP   z>KanesMethod._initialize_kindiffeq_matrices.<locals>.<dictcomp>  s    000Ca000r1   c                     g | ]}|v |	S rL   rL   )rN   varidy_symss     r/   
<listcomp>z>KanesMethod._initialize_kindiffeq_matrices.<locals>.<listcomp>  s"    NNNDdgoN4NNNr1   NzThe provided kinematic differential equations are nonlinear in {}. They must be linear in the generalized speeds and derivatives of the generalized coordinates.)rR   r<   rS   rA   r>   r   rC   rW   xreplacer   row_joinformatr[   r   dictziprU   _f_k_k_ku_k_kqdot)r"   kdeqsrA   qdotrc   	uaux_zero	qdot_zerok_kuk_kqdotf_knonlin_varsmsgrp   s               @r/   r    z*KanesMethod._initialize_kindiffeq_matrices   s     6	%46{{c%jj( L  "K L L L A:D5MME((a(((F664:666I004000I >>!$$DnnT**G..((11)<<C *$--*@*@*I*I#*N*NOOGNNNNAaaaD47NNNNK :& !K!8!8999 //#&&C??4((D#d))nnG#CQo$>$>??DY//DIy11DJ#DMMM#DDIDJ"HHDMMMr1   c                    
 |1t          |          dk    st          |          st          d           j        }t	          ||          \  }
 fd|D             } fd
D             
t           j                  }t          
          }t          |d          }t          | j        |          t          |          D ]/t          
fdt          |          D                       |<   0 j
        rC|t           j
                  z
  }|d|df         }|||df         }	| j        j        |	z  z  }|}| _        | _        |S )z"Form the generalized active force.Nr   z>Force pairs must be supplied in an non-empty iterable or None.c                 :    g | ]}t          |j                  S rL   r   rU   rN   rO   r"   s     r/   rq   z(KanesMethod._form_fr.<locals>.<listcomp>;  s&    AAA1E!T-..AAAr1   c                 :    g | ]}t          |j                  S rL   r   r   s     r/   rq   z(KanesMethod._form_fr.<locals>.<listcomp><  s&    ===%4+,,===r1      c              3   F   K   | ]}|                  |         z  V  d S r4   rL   )rN   jf_listrO   partialss     r/   	<genexpr>z'KanesMethod._form_fr.<locals>.<genexpr>D  s4      EEqA2EEEEEEr1   )rR   r   rS   r   r   rA   r   r
   rangesumr?   r\   Tr   r   )r"   flNvel_listr`   bFRrb   FRtildeFRoldr   rO   r   s   `         @@@r/   _form_frzKanesMethod._form_fr2  s    	/s2ww!| 	/8B<< 	/ . / / / N)"a00&AAAAAAA====f=== KKKK1a[[#Hdfa88q 	F 	FAEEEEEEE!HHEEEEEBqEE : 	C
OO#A!QiGqsAvJEty{U**GB	r1   c           
      
    !"# t          |          st          d          t          j        ! j        d  j        D             #d  j        D             "!fd j        D             }d |D             }!fd j                                        D             }|	                     j                    fd  fd|D             }t           j                  }t          ||          }t          |d	          }"fd
}	"#fd}
t          |          D ] \  }}t          |t                    r  |	|j                  } |	|j                  } |	|j                                                } |	|j                                                } |
|j                                                }|                    !          |z  ||z  z   } |	|                    |j                  |z  t3          ||j                                      z  #          z   |||z  z  z             }t7          |          D ]} |	||         d         |                   } |	|||         d	         |         z            }t7          |          D ]S}|||fxx         ||||         d         |         z  z  z  cc<   |||fxx         |||         d	         |         z  z  cc<   T||xx         |||         d         |         z  z  cc<   ||xx         |||         d	         |         z  z  cc<    |	|j                  } |	|j                                                } |
|j                                                }|                    !          |z  ||z  z   }t7          |          D ]} |	||         d         |                   }t7          |          D ],}|||fxx         ||||         d         |         z  z  z  cc<   -||xx         |||         d         |         z  z  cc<   " |	t3          ||                    }t3          t3          ||          #|"          }|t3          t;           j                  |          z  |z    } j        ro|t           j                  z
  }|d|df         }|||df         }| j        j         |z  z   }|d|ddf         }|||ddf         }| j        j         |z  z   }| _!        | _"        | _#        t3           j$         j"        z   #            _%        |S )z#Form the generalized inertia force.z'Bodies must be supplied in an iterable.c                     i | ]}|d S rK   rL   rM   s     r/   rP   z,KanesMethod._form_frstar.<locals>.<dictcomp>[      ...aQ...r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z,KanesMethod._form_frstar.<locals>.<dictcomp>\  r   r1   c                 0    g | ]}t          |          S rL   r   )rN   rO   ts     r/   rq   z,KanesMethod._form_frstar.<locals>.<listcomp>]  s!    222!41::222r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z,KanesMethod._form_frstar.<locals>.<dictcomp>^  s    ...1...r1   c                 h    i | ].\  }}|                               |                               /S rL   r   )rN   kvr   s      r/   rP   z,KanesMethod._form_frstar.<locals>.<dictcomp>`  s?     * * *!Qq		166!99 * * *r1   c                 `   t          | t                    r5| j                                      | j                                      g}n@t          | t                    r| j                                      g}nt          d          fd|D             }t          |j
                  S )NzMThe body list may only contain either RigidBody or Particle as list elements.c                 :    g | ]}t          |j                  S rL   r   )rN   r^   r"   s     r/   rq   zJKanesMethod._form_frstar.<locals>.get_partial_velocity.<locals>.<listcomp>r  s&    ???#sD,--???r1   )r   r   
masscenterr^   r#   
ang_vel_inr   pointr   r
   rA   )bodyvlistr   r   r"   s      r/   get_partial_velocityz6KanesMethod._form_frstar.<locals>.get_partial_velocityj  s    $	** K,,Q//1F1Fq1I1IJD(++ K**, !J K K K???????A#Atvq111r1   c                 &    g | ]} |          S rL   rL   )rN   r   r   s     r/   rq   z,KanesMethod._form_frstar.<locals>.<listcomp>t  s%    >>>4((..>>>r1   r   c                 $    t          |           S r4   r   )exprr|   s    r/   r8   z*KanesMethod._form_frstar.<locals>.<lambda>{  s    tY!7!7 r1   c                 @    t          t          |                     S r4   r   )r   r|   rd   s    r/   r8   z*KanesMethod._form_frstar.<locals>.<lambda>|  s    eE$	,B,BI&N&N r1   r   N)&r   r   r	   r=   r   rB   rC   rU   itemsupdaterR   rA   r   	enumerater   r   masscentral_inertiar   r^   r#   r   r_   r   dtr   
ang_acc_inr   r   r   r?   r\   r   r   r   _k_dr   _f_d)$r"   bluauxdotuauxdot_zeroq_ddot_u_mapr   r`   MMnonMM	zero_uauxzero_udot_uauxrO   r   MIr^   omegar_   inertial_forceinertial_torquer   tmp_veltmp_angr   tempfr_starrb   fr_star_indfr_star_depMMiMMdr   r   r   r|   rd   s$   `                              @@@@@r/   _form_frstarzKanesMethod._form_frstarR  s    || 	GEFFFN..4:...	..4:...	2222tz222..g...* * * * &&((* * *D,---		2 		2 		2 		2 		2 		2 ?>>>2>>> KK1a[[a7777	NNNNN }} 	C 	CGAt$	** CIdi((Id233i 3 3A 6 677!	$*"7"7":":;;$nT_%8%8%;%;<<"#&&))c/AG";"+)QTT$*-=-=-E!dj33A666	BB-Ca%i(-* #+ #+ q 	D 	DA'iAq(9::G'iHQKN1,=(=>>G"1XX B B1a4A8A;q>!3D)D$EE1a4Wx{1~a/@%@A!HHH!Q1B BBHHH!HHH(1+a.2C CCHHHH	D Idi((i
q 1 122$nTZ^^A%6%677"#&&))c/AG";q C CA$9Xa[^A%677D"1XX C C1a4AAq0A)A$BB!HHH!Q1B BBHHHH	C YuR..//eE<00<4 4vdj11<@@@5HI : 	+C
OO#A!"1"a%.K!!A#q&/K!TY[;%>?GRaRU)CQqS!!!V*C	c)*B	48dl2I>>>	r1   c                    | j         | j        t          d          | j        }| j        r,| j        r%| j        | j        t          | j                  z  z   }nt                      }| j        r,| j	        r%| j        | j	        t          | j
                  z  z   }nt                      }d | j        D             }d | j
        D             }d | j        D             }d t          | j        | j        g          D             }t          | j        |          | j        t          | j                  z  z   }t          | j        |          | j        t          | j                  z  z   }	t          | j        |          }
t          | j        |          | j         z   }t!          t#          |
          d          }| j        }| j        }| j        r|dt#          | j                            }n|}| j        }| j        r|dt#          | j                            }n|}| j        }| j        }|                    t.          j                  }d t          ||g          D             }t3          t          || j        || j
        ||g                    t5          fd	| j        | j        | j        | j	        | j        | j        fD                       rt          d
          t9          t;          t          | j        |                              }|                    t@                     |D ]-}t-          |t.          j                  |v rt          d          .tC          ||	|
||||||||||||          S )a   Returns 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).NNeed to compute Fr, Fr* first.c                     i | ]}|d S rK   rL   rM   s     r/   rP   z-KanesMethod.to_linearizer.<locals>.<dictcomp>  s    '''1!Q'''r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z-KanesMethod.to_linearizer.<locals>.<dictcomp>      ,,,A1a,,,r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z-KanesMethod.to_linearizer.<locals>.<dictcomp>  r   r1   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z-KanesMethod.to_linearizer.<locals>.<dictcomp>  s    @@@aQ@@@r1   r   c                     i | ]}|d S rK   rL   rM   s     r/   rP   z-KanesMethod.to_linearizer.<locals>.<dictcomp>  s    ;;;aQ;;;r1   c              3   8   K   | ]}t          |          V  d S r4   )r   )rN   rO   sym_lists     r/   r   z,KanesMethod.to_linearizer.<locals>.<genexpr>  sF       M MA"1h// M M M M M Mr1   zWCannot have dynamicsymbols outside dynamic                              forcing vector.)keyzpCannot have derivatives of specified                                  quantities when linearizing forcing terms.)"r   r   rS   rT   rV   rX   r   rA   rY   rZ   rB   r>   r   rw   ry   rx   r   rR   r<   r:   r?   rC   r   r	   r=   setanyr   listr   r   sortr   r   )r"   f_cf_vf_arc   ud_zeroqd_zero	qd_u_zerof_0f_1f_2f_3f_4r<   rA   q_iq_du_iu_duauxr   r|   rrO   r   s                           @r/   to_linearizerzKanesMethod.to_linearizer  s    H 	?$, 	?=>>> i: 	$* 	*tz&..88CC((C; 	4; 	+F4:,>,> >>CC((C''''',,,,,,,,,,@@64:tv*>#?#?@@@	DIv&&vdj7I7I)IIDIw''$*VDF^^*CCDL),,DL'**TX5CHHa   FF: 	%c$*oo%%&CCCj: 	%c$*oo%%&CCCj z))N-..;;64/#:#:;;;	 vq$*aT7KLLMM M M M M$-
DIt{DK:L M M M M M 	/ . / / /
 $U49i%@%@(KKLL	#$$$  	N 	NAA~())Q. N  "M N N NN #sCc3S!QS#q" " 	"r1   )
new_methodc                Z    |                                  } |j        di |}||j        fz   S )a   Linearize the equations of motion about a symbolic operating point.

        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.rL   )r   	linearizer   )r"   r   kwargs
linearizerresults        r/   r   zKanesMethod.linearize  s<    6 ''))
%%////''r1   c           	         || j         }|| j        | j        }|g k    rd}| j        st          d          |                     |          }|                     |          }| j        r| j        s(t          | j	        | j
        | j        | j                  }nCt          | j	        | j
        | j        | j        | j        | j        | j        z  | j        z             }| j        |_        || _        |                    |          }|                    |          }||z   | _        |                    |          | _        |                    |          | _        | j        | j        fS )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().

        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.)r,   )r,   r)   r*   )r-   r   ry   AttributeErrorr   r   rC   r?   r   r   r<   rX   rA   rV   rU   _km_aux_eqcol_joinr   r   )r"   r-   loadsfrfrstarkmfraux	frstarauxs           r/   kanes_equationszKanesMethod.kanes_equations  sx   2  	![F 	$do 	$OEB; 	E} 	L  "K L L L]]5!!""6**: 	6: % )-5 5 5 !$(JDJ.2j46.A
/#% % % "-BNDHKK&&E//I 9,DL{{5))DH!??955DL$,''r1   c                 R    |                      | j        | j                  \  }}||z   S r4   )r  bodylistr.   )r"   r   r   s      r/   
_form_eomszKanesMethod._form_eomsR  s)    ))$-HH
FF{r1   c                    t          t          | j                  t          | j                  z   d          }|                                 }t          | j                  D ]"\  }}||                                         ||<   #|9| j                            | j	                  |t          | j                  ddf<   n=| j        
                    |d          | j	        z  |t          | j                  ddf<   |S )a  Returns 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.matrices.MatrixBase.inv`

        r   Nr   T)try_block_diag)r   rR   r<   rA   kindiffdictr   r   mass_matrixr[   forcinginv)r"   
inv_methodrhskdesrO   r   s         r/   r  zKanesMethod.rhsV  s    & CKK#df++-q11!!'' 	& 	&FAs#((**%CFF 	2#'#3#;#;DL#I#ICDFa  $($4$8$8HL %9 %N %N$(L%1CDFa  
r1   c                 <    | j         st          d          | j         S )z%Returns a dictionary mapping q' to u.r   )rU   r   r"   s    r/   r	  zKanesMethod.kindiffdictw  s2     	L  "K L L Lr1   c                 v    | j         r| j        st          d          | j        st          d          | j        S )z,A matrix containing the auxiliary equations.r   z'No auxiliary speeds have been declared.)r   r   rS   rC   r   r  s    r/   auxiliary_eqszKanesMethod.auxiliary_eqs~  sH     x 	?t| 	?=>>>z 	HFGGG|r1   c                 r    | j         r| j        st          d          t          | j        | j        g          S )zThe mass matrix of the system.r   )r   r   rS   r   r   rZ   r  s    r/   r
  zKanesMethod.mass_matrix  s=     x 	?t| 	?=>>>ty$+.///r1   c                 N   | j         r| j        st          d          t          | j                  }t          | j                  }| j                            t          ||                    	                    t          ||                              | j
                            S )zYThe mass matrix of the system, augmented by the kinematic
        differential equations.r   )r   r   rS   rR   rA   r<   ry   rs   r   r   r
  )r"   r`   ns      r/   mass_matrix_fullzKanesMethod.mass_matrix_full  s     x 	?t| 	?=>>>KKKK((q!55??qB BHT-..0 0 	0r1   c                 t    | j         r| j        st          d          t          | j        | j        g           S )z!The forcing vector of the system.r   )r   r   rS   r   r   rY   r  s    r/   r  zKanesMethod.forcing  s@     x 	?t| 	?=>>>	4;/0000r1   c                     | j         r| j        st          d          | j        t	          | j                  z  | j        z   }t	          || j        | j        g           S )z\The forcing vector of the system, augmented by the kinematic
        differential equations.r   )	r   r   rS   rx   r   rA   rw   r   rY   )r"   f1s     r/   forcing_fullzKanesMethod.forcing_full  s]     x 	?t| 	?=>>>Z&..(494DIt{34444r1   c                     | j         S r4   )r;   r  s    r/   r<   zKanesMethod.q  	    wr1   c                     | j         S r4   )r@   r  s    r/   rA   zKanesMethod.u  r  r1   c                     | j         S r4   r   r  s    r/   r  zKanesMethod.bodylist  
    ~r1   c                     | j         S r4   r   r  s    r/   r.   zKanesMethod.forcelist  
    r1   c                     | j         S r4   r   r  s    r/   r-   zKanesMethod.bodies  r!  r1   c                     | j         S r4   r#  r  s    r/   r   zKanesMethod.loads  r$  r1   )	NNNNNNNNN)NNr4   )__name__
__module____qualname____doc__r0   r   r!   r    r   r   r   r   r  r  r  r	  propertyr  r
  r  r  r  r<   rA   r  r.   r-   r   rL   r1   r/   r   r      sC       b bH FJ8<@D59@ @ @ @4) ) )8A! A! A!FB% B% B%H  @_ _ _BG" G" G"T '+ ( ( ( ( (>4( 4( 4( 4(l     B        X 0 0 X0 0 0 X0 1 1 X1 5 5 X5   X   X   X   X   X   X  r1   N)sympy.core.backendr   r   r   r   sympy.core.sortingr   sympy.physics.vectorr   r	   r
   sympy.physics.mechanics.methodr    sympy.physics.mechanics.particler   !sympy.physics.mechanics.rigidbodyr   !sympy.physics.mechanics.functionsr   r   r   !sympy.physics.mechanics.linearizer   sympy.utilities.iterablesr   __all__r   rL   r1   r/   <module>r6     s>   7 7 7 7 7 7 7 7 7 7 7 7 / / / / / /4 4 4 4 4 4 4 4 4 4 3 3 3 3 3 3 5 5 5 5 5 5 7 7 7 7 7 7? ? ? ? ? ? ? ? ? ? 8 8 8 8 8 8 . . . . . ./o
 o
 o
 o
 o
( o
 o
 o
 o
 o
r1   