ó
ÍÄiQc           @   s   d  d l  Z  d  d l  m Z d  d l Z d  d l Z d  d l Z d e j f d „  ƒ  YZ d d	 d „  ƒ  YZ e	 d k rŒ d  d l
 Te ƒ  n  d S(
   iÿÿÿÿN(   t   pit   CrawlingRobotEnvironmentc           B   s5   e  Z d  „  Z d „  Z d „  Z d „  Z d „  Z RS(   c   	      C   sà   | |  _  d  |  _ d |  _ d |  _ |  j  j ƒ  \ } } |  j  j ƒ  \ } } | | |  j d } | | |  j d } g  t |  j ƒ D] } | | | ^ qˆ |  _ g  t |  j ƒ D] } | | | ^ qµ |  _	 |  j
 ƒ  d  S(   Ni	   i   i   (   t   crawlingRobott   Nonet   statet
   nArmStatest   nHandStatest   getMinAndMaxArmAnglest   getMinAndMaxHandAnglest   ranget
   armBucketst   handBucketst   reset(	   t   selfR   t   minArmAnglet   maxArmAnglet   minHandAnglet   maxHandAnglet   armIncrementt   handIncrementt   i(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   __init__   s    				**c         C   s   |  j  S(   sL   
          Return the current state
          of the crawling robot
        (   R   (   R   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   getCurrentState.   s    c         C   s—   t  ƒ  } | \ } } | d k r1 | j d ƒ n  | |  j d k  rT | j d ƒ n  | d k rp | j d ƒ n  | |  j d k  r“ | j d ƒ n  | S(   sd   
          Returns possible actions
          for the states in the
          current state
        i    s   arm-downi   s   arm-ups	   hand-downs   hand-up(   t   listt   appendR   R   (   R   R   t   actionst   currArmBuckett   currHandBucket(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   getPossibleActions5   s    	    c         C   sw  d \ } } |  j j ƒ  \ } } |  j \ } } |  j j ƒ  \ } }	 | d k r… |  j | d }
 |  j j |
 ƒ | d | f } n  | d k rÅ |  j | d }
 |  j j |
 ƒ | d | f } n  | d k r|  j | d } |  j j | ƒ | | d f } n  | d k rE|  j | d } |  j j | ƒ | | d f } n  |  j j ƒ  \ } } | | } | |  _ | | f S(   s  
          Perform the action and update
          the current state of the Environment
          and return the reward for the
          current state, the next state
          and the taken action.

          Returns:
            nextState, reward
        s   arm-upi   s   arm-downs   hand-ups	   hand-downN(   NN(	   R   R   t   getRobotPositionR   t	   getAnglesR
   t   moveArmR   t   moveHand(   R   t   actiont	   nextStatet   rewardt   oldXt   oldYt	   armBuckett
   handBuckett   armAnglet	   handAnglet   newArmAnglet   newHandAnglet   newXt   newY(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   doActionF   s0    
	c         C   sm   |  j  d } |  j d } | | f |  _ |  j j |  j | |  j | ƒ d |  j j ƒ  d g |  j _ d S(   s>   
         Resets the Environment to the initial state
        i   i   i    N(	   R   R   R   R   t	   setAnglesR
   R   R   t	   positions(   R   t   armStatet	   handState(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   q   s
    !(   t   __name__t
   __module__R   R   R   R.   R   (    (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR      s
   				+t   CrawlingRobotc           B   st   e  Z d  „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z d „  Z	 d „  Z
 d	 „  Z d
 „  Z d „  Z RS(   c         C   s   | |  _  | |  _ d S(   s]   
            set the robot's arm and hand angles
            to the passed in values
        N(   R(   R)   (   R   R(   R)   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR/   †   s    	c         C   s   |  j  |  j f S(   s?   
            returns the pair of (armAngle, handAngle)
        (   R(   R)   (   R   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   Ž   s    c         C   s   |  j  S(   sp   
            returns the (x,y) coordinates
            of the lower-left point of the
            robot
        (   t   robotPos(   R   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   ”   s    c         C   sÐ   |  j  } | |  j k r! d ‚ n  | |  j k  r9 d ‚ n  |  j |  j  |  j | |  j ƒ } |  j d } | | |  j d f |  _ | |  _  |  j j |  j ƒ  d ƒ t	 |  j ƒ d k rÌ |  j j
 d ƒ n  d S(   s9   
            move the robot arm to 'newArmAngle'
        s-   Crawling Robot: Arm Raised too high. Careful!s,   Crawling Robot: Arm Raised too low. Careful!i    i   id   N(   R(   R   R   t   displacementR)   R6   R0   R   R   t   lent   pop(   R   R*   t   oldArmAnglet   dispt   curXPos(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   œ   s    				c         C   sÐ   |  j  } | |  j k r! d ‚ n  | |  j k  r9 d ‚ n  |  j |  j |  j  |  j | ƒ } |  j d } | | |  j d f |  _ | |  _  |  j j |  j ƒ  d ƒ t	 |  j ƒ d k rÌ |  j j
 d ƒ n  d S(   s:   
            move the robot hand to 'newArmAngle'
        s.   Crawling Robot: Hand Raised too high. Careful!s-   Crawling Robot: Hand Raised too low. Careful!i    i   id   N(   R)   R   R   R7   R(   R6   R0   R   R   R8   R9   (   R   R+   t   oldHandAngleR;   R<   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR    ²   s    			!	c         C   s   |  j  |  j f S(   sk   
            get the lower- and upper- bound
            for the arm angles returns (min,max) pair
        (   R   R   (   R   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   È   s    c         C   s   |  j  |  j f S(   sl   
            get the lower- and upper- bound
            for the hand angles returns (min,max) pair
        (   R   R   (   R   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   Ï   s    c         C   s   |  j  |  j ƒ \ } } |  j  |  j ƒ \ } } |  j | |  j | |  j } |  j | |  j | |  j } | d k  rŒ t j | | ƒ Sd S(   s`   
            get the current angle the
            robot body is rotated off the ground
        i    g        (	   t   _CrawlingRobot__getCosAndSinR(   R)   t	   armLengtht
   handLengtht
   robotWidtht   robotHeightt   matht   atan(   R   t   armCost   armSint   handCost   handSint   xt   y(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   getRotationAngleÖ   s    c         C   s   t  j | ƒ t  j | ƒ f S(   N(   RC   t   cost   sin(   R   t   angle(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   __getCosAndSinç   s    c         C   s—  |  j  | ƒ \ } } |  j  | ƒ \ } } |  j  | ƒ \ }	 }
 |  j  | ƒ \ } } |  j | |  j |	 |  j } |  j | |  j |
 |  j } |  j | |  j | |  j } |  j | |  j | |  j } | d k  rK| d k rt j | | | | ƒ t j | | | | ƒ S| | | | | | t j | | | | ƒ S| d k r[d S| | | | | | t j | | | | ƒ Sd ‚ d  S(   Ni    g        s   Never Should See This!(   R>   R?   R@   RA   RB   RC   t   sqrt(   R   t   oldArmDegreet   oldHandDegreet	   armDegreet
   handDegreet	   oldArmCost	   oldArmSinRE   RF   t
   oldHandCost
   oldHandSinRG   RH   t   xOldt   yOldRI   RJ   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR7   ê   s     212c      
   C   s%  |  j  ƒ  \ } } | |  j } | |  j k r7 d ‚ n  |  j ƒ  } |  j | ƒ \ } } | |  j | } | |  j | }	 | |  j | }
 | |  j | } |
 | |  j } | | |  j } |  j j |  j	 | | | |	 | | |
 | ƒ	 |  j | |  j
 ƒ \ } } | |  j | } | |  j | } |  j j |  j | | | | ƒ |  j |  j | ƒ \ } } | |  j | } | |  j | } |  j j |  j | | | | ƒ | |  j } | d k rÀd  S|  j d } | |  j d } | |  j d t |  j ƒ } d |  j d | |  _ d |  j } d | } d	 | } d
 | } d t |  ƒ k r |  j j |  j ƒ |  j j |  j ƒ |  j j |  j ƒ |  j j |  j ƒ n  |  j j d d d | ƒ|  _ |  j j d d d | ƒ|  _ |  j j d d d | ƒ|  _ |  j j d d d | ƒ|  _ | |  _ d  S(   Ns   Flying Robot!!i    iÿÿÿÿiþÿÿÿgÍÌÌÌÌÌì?gš™™™™™¹?s   100-step Avg Velocity: %.2fs   Velocity: %.2fs   Position: %2.fs   Step: %dt   vel_msgiŠ  i¾   t   textiÂ  iú   i2   (   R   t   totWidtht   groundYRK   R>   RA   RB   t   canvast   coordst	   robotBodyR(   R?   t   robotArmR)   R@   t	   robotHandt   lastStepR0   R8   t   velAvgt   dirt   deleteR[   t   pos_msgt   step_msgt
   velavg_msgt   create_text(   R   t	   stepCountt	   stepDelayt   x1t   y1t   rotationAnglet   cosRott   sinRott   x2t   y2t   x3t   y3t   x4t   y4RE   RF   t   xArmt   yArmRG   RH   t   xHandt   yHandt   stepst   post   velocityt   vel2t   velMsgt   velocityMsgt   positionMsgt   stepMsg(    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   draw  sT    	+ 


c         C   sž  | |  _  d |  _ d |  _ d |  _ |  _ t d |  _ |  _ t d |  _ t d |  _	 d |  _
 d d t |  _ | j ƒ  |  _ | j ƒ  |  _ d |  _ |  j |  j |  _ | j d |  j |  j |  j d d ƒ|  _ d	 |  _ d |  _ d
 |  j f |  _ | j d d d d d d d d d d ƒ|  _ d |  _ | j d d d d d d d d ƒ|  _ d |  _ | j d d d d d d d d ƒ|  _ d d g |  _ d  S(   Ni    g        i   g      @g      @i(   t   fillt   blueiP   i   t   greeni<   t   oranget   widthi   t   redi   (   R_   Re   Rd   R(   RQ   t   PIR)   RR   R   R   R   R   t   winfo_reqwidthR]   t   winfo_reqheightt	   totHeightt   groundHeightR^   t   create_rectanglet   groundRA   RB   R6   t   create_polygonRa   R?   t   create_lineRb   R@   Rc   R0   (   R   R_   (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR   J  s0    						!		-	'	'(   R3   R4   R/   R   R   R   R    R   R   RK   R>   R7   R…   R   (    (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyR5   „   s   											Ht   __main__(   t   *(    (   RC   R    RŒ   t   timet   environmentt   randomt   EnvironmentR   R5   R3   t   graphicsCrawlerDisplayt   run(    (    (    sQ   C:\Users\Nicholas\Desktop\CS188X Berkeley\05 Projects\03 Reinforcement\crawler.pyt   <module>   s   rö
