RL-Project/Fetch-Reinforcement_learning_Project
0
1�
2 ��hY � �T � d dl Zd dlZd dlZd dlZ G d� dej � � Zdd�ZdS )� Nc �* � � e Zd Zd� fd� Z� fd�Z� xZS )�CustomFetchWrapperNTc � �� t � � � |� � |j | _ |�t j |t �� � nd | _ |�t j |t �� � nd | _ || _ d S )N��dtype)3�super�__init__� unwrapped�u�np�array�float�default_block_xy�default_goal_xyz�object)�self�env�block_xy�goal_xyzr � __class__s ��{C:\Users\gkemp\OneDrive\Documents\College\ELE392\RL Final Project\RL-App\Fetch-Reinforcement_learning_Project\custom_env.pyr zCustomFetchWrapper.__init__ s� �� �
������ � � �� ��� %-�$8� "$��(�%�!@�!@�!@�!@�>B�
� � %-�$8� "$��(�%�!@�!@�!@�!@�>B�
� � �� � � � c �� �� t � � j |i |��\ }}| j }|j }|j }|j }|j } t g d�g d�� � D ] \ }4} |� |||5| � � � |d d d� � � � } | j6 dk r�| j ��|j d d� }
|j
}|j } | � | |d�� � }t j � |� � |k r�>|
|z }t j � || z7 � � |k r�j |}n| j }|� ||d� � }||dd�< d |d< |� ||d|� � | j �=| j }||_ t- j |t, j j d8� � }||j |< |j � ||� � |� � � }||fS ) N)z
robot0:slide0z
robot0:slide1z
robot0:slide2)g�Q� ���?g� ��Q��?g � desired_goal� T)�sizez
object0:jointr ��z�G��?�target0) r �resetr9 �model�data�_utils� np_random�zip�set_joint_qpos�copyr r �initial_gripper_xpos� obj_range�distance_threshold�uniformr �linalg�norm�get_joint_qposr �goal�mujoco�10mj_name2id�mjtObj�11mjOBJ_SITE� site_xpos�_mujoco�12mj_forward�_get_obs)r �args�kwargs�obs�infor r r! �utils�rng�name�val�goal_pos�home_xyr( �min_dist�offset� candidate_xyr �blk_qpos�new_goal�sidr s �r r zCustomFetchWrapper.reset s' �� �!�E�G�G�M�4�2�6�2�2� ��T�������������� �� � =� =� =� +� +� +�13� 14� 9� 9�I�D�#�
� � ���d�C� 8� 8� 8� 8� �~�&�r��r�*�/�/�1�1�� �K�� � ��$�,� �1�"�1�"�5�� �K� � � 0��15� �[�[�)��Y�Q�[� G� G�F��y�~�~�f�-�-��8�8� �#*�V�#3�L��y�~�~�l�X�&=�>�>��I�I� ��'��� �0�� �+�+�E�4� �I�I�H� $�H�Q�q�S�M� !�H�Q�K� � � ���o�x� H� H� H� � � ,��,�H� �A�F��#�E�$*�M�$<�$-�/� /�C� #+�D�N�3� � 16� � � �U�D�)�)�)��j�j�l�l���D�y�r )NNT)�__name__�17__module__� __qualname__r r �
__classcell__)r s @r r r 18 s_ �� � � � � � � � � � � �@� @� @� @� @� @� @� @� @r r �FetchPickAndPlace-v3c � � t j t � � |dk rd}nd}t j || �� � }|j }|j d d� }d}t
j g |�|�� � } |�!t
j |t �� � }19||20z } nd } |�!t
j |t �� � }21| |22z } nd } t || | |�� � }
|
S ) Nz
FetchReach-v3FT)� render_moder r r )r r r )23�gym�
register_envs�gymnasium_robotics�maker24 r' r r
r r )rM r r � environmentr �base_envr � center_xy�table_z� table_center�rel� abs_block_xy� abs_goal_xyzr s r �25create_envrZ X s� � ���(�)�)�)��o�%�%�������x� ��=�=�=�H���A�26 �&�r��r�*�I��G��8� 1�i� 1�� 1�2�2�L� ���h�x�u�-�-�-�� �3�� � �� � ���h�x�u�-�-�-��#�c�)� � �� � �� � �� � � �C� �Jr )NNNrK ) �numpyr � gymnasiumrN rP r/ �Wrapperr rZ � r r �<module>r_ s� ��27 � � � � � � � � � � � �
�
�
�
�K� K� K� K� K��� K� K� K�\'� '� '� '� '� 'r