
     f:R                         d dl Z d dlZd dlZd dlZd dlZd dlT d dlT d dlZd dl	T d dl
mZ  G d d          Zedk    rdS dS )    N)*)COMMANDc                       e Zd Zd Zd Zd Zd"dZd"dZd Zd	 Z	d
 Z
d Zd Zd Zdddddd ej         ej        d                    fdZd Zd Zd Zd Zd Zd Zd Zd#dZd Zd Zd Zd Zd$d Zd!S )%Controlc                 D   t                      | _        t                      | _        t	          ddd          | _        d| _        d| _        d| _        d| _	        d| _
        d| _        g d| _        g dg dg d	g d	g| _        |                     d
          | _        g dg dg dg dg| _        g dg dg dg dg| _        d| _        d| _        d| _        t+          j        | j                  | _        |                                  |                     d           d S )Ng      ?        g{Gzd?   c   r    r   r   r   r   )r   r
   
   )r   r
   point)Z   r   r   )r   r   r   TFtarget)IMUimuServoservoIncremental_PIDpidspeedheighttimeout	move_flag
move_countmove_timeoutorderr   readFromTxtcalibration_pointanglecalibration_angle
relax_flagbalance_flagattitude_flag	threadingThread	conditionThread_conditionacalibrationrelax)selfs    2/home/digit/Desktop/robocat/Code/Server/Control.py__init__zControl.__init__   s3   77
"3s622
%%%
!kk;;;lllK
!%!1!1'!:!:hhxxx:
 '@ (/t~FFF

4    c                    t          |dz   d          }|                                }g }t          t          |                    D ]D}||                                                             d          }|                    |           Et          t          |                    D ]K}t          t          ||                             D ]&}t          ||         |                   ||         |<   'L|                                 |S )N.txtr	)	open	readlinesrangelenstripsplitappendintclose)r-   filenamefile1list_rowlist_sourceicolumn_listjs           r.   r    zControl.readFromTxt#   s    X&,,??$$s8}}%% 	, 	,A"1+++--33D99K{++++s;''(( 	; 	;A3{1~..// ; ;$'Aq(9$:$:Aq!!;r0   c                    t          |dz   d          }t          t          |                    D ]}t          t          ||                             D ]E}|                    t	          ||         |                              |                    d           F|                    d           |                                 d S )Nr2   wr4   
)r5   r7   r8   writestrr=   )r-   listr>   file2rB   rD   s         r.   	saveToTxtzControl.saveToTxt0   s    X&,,s4yy!! 	 	A3tAw<<(( " "CQ
OO,,,D!!!!KKr0      7   c                 0   t           j        dz  t          j        ||          z
  }d}|t          j        |          z  }	|t          j        |          z  }
t          j        ||
z
  dz  ||	z
  dz  z   ||z
  dz  z             }||z
  |z  }||z  ||z  z   ||z  z
  d|z  |z  z  }t          j        t          |d                    t          j        t          |d                    z
  }t           j        t          j        t          |dz  |dz  z   |dz  z
  d|z  |z  z  d                    z
  }t          t          j	        |                    }t          t          j	        |                    }t          t          j	        |                    }|||fS )N   r   )
mathpiatan2sincossqrtasinroundacosdegrees)r-   xyzl1l2l3ax_3x_4x_5l23rF   vbcs                   r.   coordinateToAnglezControl.coordinateToAngle9   sb   
'!)DJqOO
#tx{{Ntx{{NIquqj!C%!+QsUQJ677S5#+b5S=B2c
*
)E!AJJ

	%!** 5 5
5
'$)E2q5Q;sAv#5"R"@CCDD
D
Q
 
 
Q
 
 
Q
 
 1ur0   c                    t           j        dz  |z  }t           j        dz  |z  }t           j        dz  |z  }|t          j        ||z             z  |t          j        |          z  z   }|t          j        |          z  t          j        ||z             z  |t          j        |          z  t          j        |          z  z   |t          j        |          z  z   }|t          j        |          z  t          j        ||z             z  |t          j        |          z  t          j        |          z  z   |t          j        |          z  z   }	|||	fS )N   )rQ   rR   rT   rU   )
r-   ra   rg   rh   r^   r_   r`   r[   r\   r]   s
             r.   angleToCoordinatezControl.angleToCoordinateH   s   
'#+a-
'#+a-
'#+a-
TXac]]
2dhqkk>
)
TXa[[.!A#
&r$(1++~dhqkk'A
A"TXa[[.
P
TXa[[.!A#
&r$(1++~dhqkk'A
A"TXa[[.
P1ur0   c                    t          d          D ]z}|                     | j        |         d         | j        |         d         | j        |         d                   \  | j        |         d<   | j        |         d<   | j        |         d<   {t          d          D ]z}|                     | j        |         d         | j        |         d         | j        |         d                   \  | j        |         d<   | j        |         d<   | j        |         d<   {t          d          D ]}| j        |         d         | j        |         d         z
  | j        |         d<   | j        |         d         | j        |         d         z
  | j        |         d<   | j        |         d         | j        |         d         z
  | j        |         d<   d S )N   r      rP   )r7   ri   r!   r#   r   r"   )r-   rB   s     r.   r+   zControl.calibrationQ   s%   q 	X 	XAcgcycyz~  {Q  RS  {T  UV  {Wz~  {Q  RS  {T  UV  {Wz~  {Q  RS  {T  UV  {WdX dXbD"1%a()?)B1)EdF\]^F_`aFbFb q 	h 	hA?C?U?UVZV`abVcdeVfVZV`abVcdeVfVZV`abVcdeVf@h @h>DJqM!TZ]1-djmA.>.> q 	W 	WA)-)?)B1)EdjQRmTUFV)VD"1%a()-)?)B1)EdjQRmTUFV)VD"1%a()-)?)B1)EdjQRmTUFV)VD"1%a((	W 	Wr0   c                    |                                  r	 t          d          D ]z}|                     | j        |         d         | j        |         d         | j        |         d                   \  | j        |         d<   | j        |         d<   | j        |         d<   {t          d          D ]}|                     | j        |         d         | j        |         d         z   dd          | j        |         d<   |                     d| j        |         d         | j        |         d         z   z
  dd          | j        |         d<   |                     | j        |         d         | j        |         d         z   dd          | j        |         d<   |                     | j        |dz            d         | j        |dz            d         z   dd          | j        |dz            d<   |                     d| j        |dz            d         z   | j        |dz            d         z   dd          | j        |dz            d<   |                     d| j        |dz            d         | j        |dz            d         z   z
  dd          | j        |dz            d<   t          d          D ]8}| j                            d|dz  z   | j        |         d                    | j                            d|dz  z   | j        |         d                    | j                            d|dz  z   | j        |         d                    | j                            d|dz  z   | j        |dz            d                    | j                            d	|dz  z   | j        |dz            d                    | j                            d
|dz  z   | j        |dz            d                    :d S # t          $ r}Y d }~d S d }~ww xY wt          d           d S )Nrn   r   ro   rP   rk   r      r	   	   r   z0This coordinate point is out of the active range)
checkPointr7   ri   r   r"   restrictionr#   r   setServoAngle	Exceptionprint)r-   rB   es      r.   runzControl.run^   s   ?? 	Fq p pAGKG]G]^b^hij^klm^n^b^hij^klm^n^b^hij^klm^nHp HpFDJqM!$TZ]1%5djmA6F6F q w wA%)%5%5djmA6FtG]^_G`abGc6cdefi%j%jDJqM!$%)%5%5b$*Q-:J4KabcKdefKg:g6hijkn%o%oDJqM!$%)%5%5djmA6FtG]^_G`abGc6cdefi%j%jDJqM!$'+'7'7
1Q38J4KabcdebeKfghKi8ijklo'p'pDJqsOA&'+'7'74:ac?1;M8MdNdefghehNijkNl8lmnor's'sDJqsOA&'+'7'7TZ!_Q=OPTPfghijgjPklmPn=n8opqru'v'vDJqsOA&&q H HAJ,,QqsU4:a=3CDDDJ,,QqsU4:a=3CDDDJ,,QqsU4:a=3CDDDJ,,QqsU4:ac?13EFFFJ,,QqsU4:ac?13EFFFJ,,R!VDJqsOA4FGGGGH H     DEEEEEs   OO   
O50O5c                 @   d}g d}t          d          D ][}t          j        | j        |         d         dz  | j        |         d         dz  z   | j        |         d         dz  z             ||<   \t          d          D ]}||         dk    s||         dk     rd	}|S )
NT)r   r   r   r   r   r   rn   r   rP   ro         F)r7   rQ   rV   r   )r-   flag
leg_lenghtrB   s       r.   rs   zControl.checkPointw   s     ==
q 	_ 	_A	$*Q-"2A"5djmA6F6I"I$*UV-XYJZ\]J]"]^^*Q--q"" 	 	A]S  JqMB$6$6Dr0   c                    	 	 t          j                     | j        z
  dk    r5| j        dk    r*| j        dk    rd| _        t          j                     | _        | j        dk     rt          j                     | j        z
  dk    rd| j        dk    rY| j        dk    rN| j        d         dk    r=t          j                     | _        d| _        |                     d           g d| _        | j        dk    rH| j        d         dk    r7| j        d         t          j        k    r|                     d           d| _        | j	        dk    rG| j        d         t          j
        k    r,| j        d         dk    r|                                  d| _	        | j        dk    rA| xj        t          j                     | j        z
  z  c_        t          j                     | _        | j        d         t          j        k    rg d| _        |                                  n=| j        d         t          j        k    r5t          | j        d	                   | _        |                                  n| j        d         t          j        k    r5t          | j        d	                   | _        |                                  n| j        d         t          j        k    r5t          | j        d	                   | _        |                                  nM| j        d         t          j        k    r5t          | j        d	                   | _        |                                  n| j        d         t          j        k    r5t          | j        d	                   | _        |                                  n| j        d         t          j        k    r5t          | j        d	                   | _        |                                  n]| j        d         t          j        k    rK| j        rd| _        |                     d           nd| _        |                     d           g d| _        n| j        d         t          j        k    r8|                     t          | j        d	                              g d| _        n| j        d         t          j        k    r8|                     t          | j        d	                              g d| _        nQ| j        d         t          j
        k    rAd| _	        |                     | j        d	         | j        d
         | j        d                    n| j        d         t          j         k    rd| _        | j        d	         dk    rt          | j        d
                   | j!        d         d<   t          | j        d                   | j!        d         d	<   t          | j        d                   | j!        d         d
<   | "                                 | #                                 n| j        d	         dk    rt          | j        d
                   | j!        d	         d<   t          | j        d                   | j!        d	         d	<   t          | j        d                   | j!        d	         d
<   | "                                 | #                                 nl| j        d	         dk    rt          | j        d
                   | j!        d
         d<   t          | j        d                   | j!        d
         d	<   t          | j        d                   | j!        d
         d
<   | "                                 | #                                 n| j        d	         dk    rt          | j        d
                   | j!        d         d<   t          | j        d                   | j!        d         d	<   t          | j        d                   | j!        d         d
<   | "                                 | #                                 n| j        d	         dk    r/| $                    | j!        d           |                                  n| j        d         t          j%        k    rN| j        d	         dk    r=tM          j'        | (                                          }|)                                 d S n[| j        dk    rPd| _        |                     d           | j*        d	k    rd	| _*        | j        dk    rd| _        d| _*        g d| _        n&# tV          $ r}tY          |           Y d }~nd }~ww xY w	u)NT<   r   rk   r   Fr   r   ro   rP   rq   onern   twothreefoursaver   1r      )-timer   r$   r   r   r   r,   cmd	CMD_RELAXr&   CMD_ATTITUDEstopCMD_MOVE_STOPCMD_MOVE_FORWARDr<   r   forWardCMD_MOVE_BACKWARDbackWardCMD_MOVE_LEFTsetpLeftCMD_MOVE_RIGHT	setpRightCMD_TURN_LEFTturnLeftCMD_TURN_RIGHT	turnRight
CMD_HEIGHT	upAndDownCMD_HORIZONbeforeAndAfterattitudeCMD_CALIBRATIONr!   r+   ry   rL   CMD_BALANCEr'   r(   IMU6050startr   rv   rw   )r-   
Thread_IMUrx   s      r.   r)   zControl.condition   s   e	d9;;t00255$:KQ:N:NSWSbdhShSh$%DO&*ikkD%?S((	DL0"44qT_^cMcMchlhrsthuy{h{h{%)Y[[(,

4(((#3#3#3
,,A"1D1D$*UV-Z]ZgJgJg

5)))(-)4//DJqMCDT4T4TY]YcdeYfjlYlYl		+0*--T5F)FF*.)++)z!}c&777#3#3#3
		A(<<<#&tz!}#5#5
A(===#&tz!}#5#5
A(999#&tz!}#5#5
A(:::#&tz!}#5#5
((((A(999#&tz!}#5#5
A(:::#&tz!}#5#5
((((A55? -,1DO JJu----,0DO JJt,,,#3#3#3

A66s4:a='9'9:::#3#3#3

A77++C
1,>,>???#3#3#3

A(888+/*djmDJqM$*Q-PPPPA(;;;():a=%//9<TZ]9K9KD215a89<TZ]9K9KD215a89<TZ]9K9KD215a8 ,,... HHJJJJ!Z]E119<TZ]9K9KD215a89<TZ]9K9KD215a89<TZ]9K9KD215a8 ,,... HHJJJJ!Z]G339<TZ]9K9KD215a89<TZ]9K9KD215a89<TZ]9K9KD215a8 ,,... HHJJJJ!Z]F229<TZ]9K9KD215a89<TZ]9K9KD215a89<TZ]9K9KD215a8 ,,... HHJJJJ!Z]F22 NN4+A'JJJ IIKKKA77DJqM3<N<N#,#34<<>>#J#J#J
"((***_s**$(DOJJt$$$~q(('(#--()'(///DJ   aIe	s   c-e 2Ae 
e2e--e2c                 &    ||k     r|S ||k    r|S |S N )r-   varv_minv_maxs       r.   rt   zControl.restriction   s#    ;;L5[[LJr0   c                 *    ||z
  ||z
  z  ||z
  z  |z   S r   r   )r-   valuefromLowfromHightoLowtoHighs         r.   mapzControl.map   s$    uuW}-'1ABUJJr0   r   `   rq   rn   c	                 .   |dk    rt          d          D ]}	dd|	z   z  |z  dz   | j        d|	z           d<   || j        d|	z           d<   d|	z  |z  d|	z  dz  z   | j        d|	z           d<   dd|	z   z  |z  dz   | j        dd|	z  z            d<   || j        dd|	z  z            d<   dd|	z   z  |z  d|	z  dz  z   | j        dd|	z  z            d<   n|dk    rt          d          D ]}	d|	z  |z  dz   | j        d|	z           d<   || j        d|	z           d<   dd|	z   z  |z  d|	z  dz  z   | j        d|	z           d<   d|	z  |z  dz   | j        dd|	z  z            d<   || j        dd|	z  z            d<   d|	z  |z  d|	z  dz  z   | j        dd|	z  z            d<   n|dk    s|d	k    rft          d          D ]T}	|dz   | j        d
|	z           d<   || j        d
|	z           d<   |dz   | j        d|	z            d<   || j        d|	z            d<   Unx|dk    rt          d          D ]}	|ddd|	z  z   f         dz   | j        d
|	z
           d<   |ddd|	z  z   f         | j        d
|	z
           d<   |ddd|	z  z   f         | j        d
|	z
           d<   |dd|	z  f         dz   | j        |	         d<   |dd|	z  f         | j        |	         d<   |dd|	z  f         | j        |	         d<   nt          d          D ]}	|dz   | j        |	dz           d<   || j        |	dz           d<   |dz   | j        |	dz  dz            d<   || j        |	dz  dz            d<   |d|	z  dz  z   | j        |	dz           d<   |d|	z  dz  z   | j        |	dz  dz            d<   |                                  d S )Nr   rP   ro   r   r   r   r   horizonrq   Attitude Angle)r7   r   ry   )
r-   
move_orderX1Y1Z1X2Y2Z2posrB   s
             r.   changeCoordinateszControl.changeCoordinates   sU   ##1XX C C%'1Q3K#3B#6
1Q3"#%
1Q3"%'1Ir>B7B,#>
1Q3"')QqSk2%5b%8
1QqS5!!$%'
1QqS5!!$')QqSk2%5Qwl%B
1QqS5!!$$C ;&&1XX A A%'1Ir>"#4
1Q3"#%
1Q3"%'1Q3K#3b1WbL#@
1Q3"')Qi^B%6
1QqS5!!$%'
1QqS5!!$')Qi^b1WbL%@
1QqS5!!$$A H$$*	*A*A1XX & &#%b5
1Q3"#%
1Q3"#%b5
1Q3"#%
1Q3""	&
 +++1XX , ,#&q1Q3w<?
1Q3"#&q1Q3w<
1Q3"#&q1Q3w<
1Q3"!$QqsUB
1a !$QqsU
1a !$QqsU
1a  , 1XX 5 5#%b5
1Q3"#%
1Q3"%'U
1Q3q5!!$%'
1Q3q5!!$#%Qwl?
1Q3"%'"q"_
1Q3q5!!$$




r0   c           
         t          dd| j                   D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                     d||d||d           d S )N  Y      rk      r   r   r7   r   rQ   rU   rR   rT   r   r   r-   rB   r   r   r   r   s         r.   r   zControl.backWard  s   s2tzk** 		? 		?A$(1TW9S=)))B!DG)C-(((4B$(AcE47?3.///B1S5$'/#-...t{:BDK;DK;"":bAbA>>>>		? 		?r0   c           
         t          dd| j                  D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                     d||d||d           d S )Nr     r   rk   r   r   r   r   r   s         r.   r   zControl.forWard%  s   r#dj)) 		> 		>A$(1TW9S=)))B!DG)C-(((4B$(AcE47?3.///B1S5$'/#-...t{:BDK;DK;""9R1R1====		> 		>r0   c           
      "   t          dd| j                  D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|}|}|                     d||||||           d S )Nr   i  rq   rk   r	   r   r   r-   rB   r   r   r   r   r   r   s           r.   r   zControl.turnLeft1  s   qTZ(( 	A 	AA!DG)C-(((B!DG)C-(((4B1S5$'/#-...B1S5$'/#-...t{:BDK;DK;BB"":bBr"R@@@@	A 	Ar0   c           
      "   t          dd| j                  D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|}|}|                     d||||||           d S )Nr   r   rq   rk   r	   r   r   r   s           r.   r   zControl.turnRight@  s   #dj)) 
B 
BQ!DG)C-(((B!DG)C-(((4B1S5$'/#-...B1S5$'/#-...t{:BDK;DK;BB"";r"R2bAAAA
B 
Br0   c                    d| j         dgd| j         dgd| j         dgd| j         dgg}t          d          D ]}||         d         | j        |         d         z
  dz  ||         d<   ||         d         | j        |         d         z
  dz  ||         d<   ||         d         | j        |         d         z
  dz  ||         d<   t          d          D ]}t          d          D ]w}| j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   x|                                  d S )Nr   r   rn   r   2   ro   rP   )r   r7   r   ry   )r-   prB   rD   s       r.   r   zControl.stopN  s   R 2t{B"7"dk39ORTVZVacfQg
hq 	2 	2AqT!WTZ]1--r1AaDGqT!WTZ]1--r1AaDGqT!WTZ]1--r1AaDGGr 	 	A1XX * *
1a   !A$q')   
1a   !A$q')   
1a   !A$q')    HHJJJJ	 	r0   c           
         t          dd| j                  D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                     dd||d||           d S )Nr   r   r   rk      r   r   r   r-   rB   r   r   r   r   s         r.   r   zControl.setpLeftZ  s   r#dj)) 		? 		?A$(1TW9S=)))B!DG)C-(((4B$(AcE47?3.///B1S5$'/#-...t{:BDK;DK;"":a2a2>>>>		? 		?r0   c           
         t          dd| j                   D ]}dt          j        |t          j        z  dz            z  }dt          j        |t          j        z  dz            z  | j        z   }dt          j        |dz   t          j        z  dz            z  }dt          j        |dz   t          j        z  dz            z  | j        z   }|| j        k    r| j        }|| j        k    r| j        }|                     dd||d||           d S )Nr   r   r   rk   r   r   r   r   r   s         r.   r   zControl.setpRightf  s
   s2tzk** 		@ 		@A$(1TW9S=)))B!DG)C-(((4B$(AcE47?3.///B1S5$'/#-...t{:BDK;DK;"";qBqB????		@ 		@r0   Fc                    |dk    rg dg dg dg dg}t          d          D ]}| j        |         d         ||         d         z
  dz  ||         d<   | j        |         d         ||         d         z
  dz  ||         d<   | j        |         d         ||         d         z
  dz  ||         d<   t          dd          D ]}t          d          D ]w}| j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   | j        |         dxx         ||         d         z  cc<   x|                                  | j        dk    rC| xj        t          j                    | j        z
  z  c_        t          j                    | _        d S d S |                                  t          j                    | _        d S )	NT)rN   N   r   rn   r   r   ro   rP   3   )r7   r   ry   r   r   r   r   )r-   r}   r   rB   rD   s        r.   r,   zControl.relaxr  s   ::{{KKKkkkBA1XX 6 6Aq)!A$q'125!QAq)!A$q'125!QAq)!A$q'125!Q1R[[  q . .AJqM!$$$ad1g-$$$JqM!$$$ad1g-$$$JqM!$$$ad1g-$$$$



 !##T->!>>"&)++!!! $# IIKKK"ikkDr0   c           	      d    |dz   | _         |                     dd| j         dd| j         d           d S )Nr
   r   r   )r   r   r-   r   s     r.   r   zControl.upAndDown  s7    Fx$+a$+aHHHHHr0   c           	      P    |                      d|| j        d|| j        d           d S )Nr   r   )r   r   r   s     r.   r   zControl.beforeAndAfter  s,    yT[3t{1MMMMMr0   c                 H   |                      t          |          dddd          }|                      t          |          dddd          }|                      t          |          dddd          }|                     |||d          }|                     d|           d S )Ni   r   r   r   r   r   )r   r<   postureBalancer   )r-   r3   r   r\   r   s        r.   r   zControl.attitude  s    
((3q66#bR
(
(
((3q66#bR
(
(
((3q66#bR
(
(!Aa((/C88888r0   c                    d| _         g d| _        |                     ddd          }|                     d|           t	          j        d           | j                                        \  | j        _        | j        _	        t	          j        d           	 | xj
        t	          j                    | j        z
  z  c_
        t	          j                    | _        | j                                        \  }}}| j                            |          }| j                            |          }|                     ||d          }|                     d|           | j        d         t          j        k    r| j        d         dk    s'| j         dk    r| j        d         d	k    s| j
        d
k    r;t#          j        | j                  }|                                 | j         dk     d S P)NTr   r   r   r   rP   ro   0r   rk   r   F)r%   r   r   r   r   sleepr   average_filterError_value_accel_dataError_value_gyro_datar   r   	imuUpdater   PID_computer   r   r'   r(   r)   r   )r-   r   r3   r   r\   r*   s         r.   r   zControl.IMU6050  s   ###
!A&&/C888
1GKxG^G^G`G`F'(F
1	OOTY[[):::OO"ikkD($$&&EAah""1%%Ah""1%%A##Aa**C""#3"<<<A//DJqM34F4F$J[]aJaJafjfpqrfsuwfwfw{  |K  LO  |O  |O"+"2$."I"I"I!!'')))!5((	r0   ro   c           	         d}d}d}|dk    r| j         }t          j        dd|g          j        }t          j        |||g          t
          j        z  dz  }	|	d         |	d         |	d         }}}
t          j        g ddt          j        |
          t          j        |
           gdt          j        |
          t          j        |
          gg          }t          j        t          j        |          dt          j        |           gg d	t          j        |          dt          j        |          gg          }t          j        t          j        |          t          j        |           dgt          j        |          t          j        |          dgg d
g          }||z  |z  }t          j        |dz  |dz  dg|dz  | dz  dg| dz  |dz  dg| dz  | dz  dgg          j        }t          j        |dz  |dz  dz   | j         |z
  g|dz  | dz  dz
  | j         |z
  g| dz  |dz  dz   | j         |z
  g| dz  | dz  dz
  | j         |z
  gg          j        }t          j        t          j	        d                    }t          d          D ](}|||d d |f         z  z   |d d |f         z
  |d d |f<   )|S )NL      r   r   rk   ro   rP   )ro   r   r   )r   ro   r   )r   r   ro   r   r   rn   )r   npmatTarrayrQ   rR   rU   rT   zerosr7   )r-   r3   r   r\   hrg   rF   lr   rpyRPYrotxrotyrotzrot_mat
body_strucfootpoint_strucABrB   s                        r.   r   zControl.postureBalance  s   a44kAfcC!%&&(hA{##dg-3a&#a&#a&a1v<<<dhqkk\:dhqkk:< = = v!q|<:::x{{Qdhqkk:< = = v!tx{{lQ<x{{TXa[[1::::< = = +$Vq1uq1uq1UQBFq1BFQUq1BFQBFq13 4 4 56 	 &AEa!eRZ$+a-"H#$q5QBFB;4;q= I#$"q&QUBJ4;q= I#$"q&QBFB;4;q= I"K L L MN 	 VBHV$$%%q 	P 	PAWqqq!t'<<<z!!!Q$?OOBqqq!tHHr0   N)rM   rN   rN   )F)ro   )__name__
__module____qualname__r/   r    rL   ri   rl   r+   ry   rs   r)   rt   r   r   r   r   r   r   r   r   r   r   r   r   r,   r   r   r   r   r   r   r0   r.   r   r      s         *          W W WF F F2  f f fN  K K K-."Q"fbfU]UWU]^dUeUeNfNf ' ' ' 'P
? 
? 
?
> 
> 
>A A AB B B  
? 
? 
?
@ 
@ 
@* * * *&I I IN N N9 9 9  *     r0   r   __main__)r   rQ   smbuscopyr'   r   PIDnumpyr   r   Commandr   r   r   r   r   r0   r.   <module>r     s                            " " " " " "v v v v v v v vp ZD r0   