
    h2                     j    S /r SSKrSSKJr  SSKJr  SSKJr  SS0r	 " S S\\R                  5      rg)	zKallinteris-Andreas    N)utils)	MujocoEnv)Boxtrackbodyidc                   x    \ rS rSrSrS/ SQ0rSS\SS4S\S	\S
\	\\
\-  4   S\
S\
4
S jjrS rS rS rS rSrg)
ReacherEnv   a#  
## Description
"Reacher" is a two-jointed robot arm.
The goal is to move the robot's end effector (called *fingertip*) close to a target that is spawned at a random position.


## Action Space
```{figure} action_space_figures/reacher.png
:name: reacher
```

The action space is a `Box(-1, 1, (2,), float32)`. An action `(a, b)` represents the torques applied at the hinge joints.

| Num | Action                                                                          | Control Min | Control Max |Name (in corresponding XML file)| Joint | Type (Unit)  |
|-----|---------------------------------------------------------------------------------|-------------|-------------|--------------------------------|-------|--------------|
| 0   | Torque applied at the first hinge (connecting the link to the point of fixture) | -1          | 1           | joint0                         | hinge | torque (N m) |
| 1   | Torque applied at the second hinge (connecting the two links)                   | -1          | 1           | joint1                         | hinge | torque (N m) |


## Observation Space
The observation space consists of the following parts (in order):

- *cos(qpos) (2 elements):* The cosine of the angles of the two arms.
- *sin(qpos) (2 elements):* The sine of the angles of the two arms.
- *qpos (2 elements):* The coordinates of the target.
- *qvel (2 elements):* The angular velocities of the arms (their derivatives).
- *xpos (2 elements):* The vector between the target and the reacher's.

The observation space is a `Box(-Inf, Inf, (10,), float64)` where the elements are as follows:

| Num | Observation                                                                                    | Min  | Max | Name (in corresponding XML file) | Joint | Type (Unit)              |
| --- | ---------------------------------------------------------------------------------------------- | ---- | --- | -------------------------------- | ----- | ------------------------ |
| 0   | cosine of the angle of the first arm                                                           | -Inf | Inf | cos(joint0)                      | hinge | unitless                 |
| 1   | cosine of the angle of the second arm                                                          | -Inf | Inf | cos(joint1)                      | hinge | unitless                 |
| 2   | sine of the angle of the first arm                                                             | -Inf | Inf | sin(joint0)                      | hinge | unitless                 |
| 3   | sine of the angle of the second arm                                                            | -Inf | Inf | sin(joint1)                      | hinge | unitless                 |
| 4   | x-coordinate of the target                                                                     | -Inf | Inf | target_x                         | slide | position (m)             |
| 5   | y-coordinate of the target                                                                     | -Inf | Inf | target_y                         | slide | position (m)             |
| 6   | angular velocity of the first arm                                                              | -Inf | Inf | joint0                           | hinge | angular velocity (rad/s) |
| 7   | angular velocity of the second arm                                                             | -Inf | Inf | joint1                           | hinge | angular velocity (rad/s) |
| 8   | x-value of position_fingertip - position_target                                                | -Inf | Inf | NA                               | slide | position (m)             |
| 9   | y-value of position_fingertip - position_target                                                | -Inf | Inf | NA                               | slide | position (m)             |
| excluded | z-value of position_fingertip - position_target (constantly 0 since reacher is 2d)        | -Inf | Inf | NA                               | slide | position (m)             |


Most Gymnasium environments just return the positions and velocities of the joints in the `.xml` file as the state of the environment.
In reacher, however, the state is created by combining only certain elements of the position and velocity and performing some function transformations on them.
The `reacher.xml` contains these 4 joints:

| Num | Observation                 | Min      | Max      | Name (in corresponding XML file) | Joint | Unit               |
|-----|-----------------------------|----------|----------|----------------------------------|-------|--------------------|
| 0   | angle of the first arm      | -Inf     | Inf      | joint0                           | hinge | angle (rad)        |
| 1   | angle of the second arm     | -Inf     | Inf      | joint1                           | hinge | angle (rad)        |
| 2   | x-coordinate of the target  | -Inf     | Inf      | target_x                         | slide | position (m)       |
| 3   | y-coordinate of the target  | -Inf     | Inf      | target_y                         | slide | position (m)       |


## Rewards
The total reward is: ***reward*** *=* *reward_distance + reward_control*.

- *reward_distance*:
This reward is a measure of how far the *fingertip* of the reacher (the unattached end) is from the target,
with a more negative value assigned if the reacher's *fingertip* is further away from the target.
It is $-w_{near} \|(P_{fingertip} - P_{target})\|_2$.
where $w_{near}$ is the `reward_near_weight` (default is $1$).
- *reward_control*:
A negative reward to penalize the walker for taking actions that are too large.
It is measured as the negative squared Euclidean norm of the action, i.e. as $-w_{control} \|action\|_2^2$.
where $w_{control}$ is the `reward_control_weight`. (default is $0.1$)

`info` contains the individual reward terms.

## Starting State
The initial position state of the reacher arm is $\mathcal{U}_{[-0.1 \times I_{2}, 0.1 \times I_{2}]}$.
The position state of the goal is (permanently) $\mathcal{S}(0.2)$.
The initial velocity state of the Reacher arm is $\mathcal{U}_{[-0.005 \times 1_{2}, 0.005 \times 1_{2}]}$.
The velocity state of the object is (permanently) $0_2$.

where $\mathcal{U}$ is the multivariate uniform continuous distribution and $\mathcal{S}$ is the uniform continuous spherical distribution.

The default frame rate is $2$, with each frame lasting $0.01$, so *dt = 5 * 0.01 = 0.02*.


## Episode End
### Termination
The Reacher never terminates.

### Truncation
The default duration of an episode is 50 timesteps.


## Arguments
Reacher provides a range of parameters to modify the observation space, reward function, initial state, and termination condition.
These parameters can be applied during `gymnasium.make` in the following way:

```python
import gymnasium as gym
env = gym.make('Reacher-v5', xml_file=...)
```

| Parameter               | Type       | Default       | Description                                              |
|-------------------------|------------|---------------|----------------------------------------------------------|
| `xml_file`              | **str**    |`"reacher.xml"`| Path to a MuJoCo model                                   |
| `reward_dist_weight`    | **float**  | `1`           | Weight for _reward_dist_ term (see `Rewards` section)    |
| `reward_control_weight` | **float**  | `0.1`         | Weight for _reward_control_ term (see `Rewards` section) |

## Version History
* v5:
    - Minimum `mujoco` version is now 2.3.3.
    - Added `default_camera_config` argument, a dictionary for setting the `mj_camera` properties, mainly useful for custom environments.
    - Added `frame_skip` argument, used to configure the `dt` (duration of `step()`), default varies by environment check environment documentation pages.
    - Fixed bug: `reward_distance` was based on the state before the physics step, now it is based on the state after the physics step (related [GitHub issue](https://github.com/Farama-Foundation/Gymnasium/issues/821)).
    - Removed `"z - position_fingertip"` from the observation space since it is always 0 and therefore provides no useful information to the agent, this should result is slightly faster training (related [GitHub issue](https://github.com/Farama-Foundation/Gymnasium/issues/204)).
    - Added `xml_file` argument.
    - Added `reward_dist_weight`, `reward_control_weight` arguments to configure the reward function (defaults are effectively the same as in `v4`).
    - Fixed `info["reward_ctrl"]`  not being multiplied by the reward weight.
* v4: All MuJoCo environments now use the MuJoCo bindings in mujoco >= 2.1.3
* v3: This environment does not have a v3 release. Moved to the [gymnasium-robotics repo](https://github.com/Farama-Foundation/gymnasium-robotics).
* v2: All continuous control environments now use mujoco-py >= 1.50. Moved to the [gymnasium-robotics repo](https://github.com/Farama-Foundation/gymnasium-robotics).
* v1: max_time_steps raised to 1000 for robot based tasks (not including reacher, which has a max_time_steps of 50). Added reward_threshold to environments.
* v0: Initial versions release
render_modeshuman	rgb_arraydepth_array
rgbd_tuplezreacher.xml      xml_file
frame_skipdefault_camera_configreward_dist_weightreward_control_weightc                 z   [         R                  R                  " U UUUUU40 UD6  X@l        XPl        [        [        R                  * [        R                  S[        R                  S9n[        R                  " U UU4UUS.UD6  / SQ[        [        R                  " SU R                  -  5      5      S.U l        g )N)
   )lowhighshapedtype)observation_spacer   r   g      ?)r
   
render_fps)r   EzPickle__init___reward_dist_weight_reward_control_weightr   npinffloat64r   introunddtmetadata)selfr   r   r   r   r   kwargsr   s           Z/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/mujoco/reacher_v5.pyr    ReacherEnv.__init__   s     	!!	
 	
 $6 &;#RVVG"&&RZZX	
 0"7	
 	
 bhhsTWW}56
    c                     U R                  XR                  5        U R                  5       nU R                  U5      u  p4UnU R                  S:X  a  U R                  5         X#SSU4$ )Nr   F)do_simulationr   _get_obs_get_rewrender_moderender)r*   actionobservationrewardreward_infoinfos         r,   stepReacherEnv.step   s[    6??3mmo"mmF3w&KKME5$66r.   c                    U R                  S5      U R                  S5      -
  n[        R                  R                  U5      * U R                  -  n[        R
                  " U5      R                  5       * U R                  -  nX4-   nUUS.nXV4$ )N	fingertiptarget)reward_distreward_ctrl)get_body_comr#   linalgnormr!   squaresumr"   )r*   r5   vecr?   r@   r7   r8   s          r,   r2   ReacherEnv._get_rew   s    ,t/@/@/JJyy~~c**T-E-EEyy(,,..1L1LL* '&

 ""r.   c                    U R                   R                  SSU R                  R                  S9U R                  -   n U R                   R                  SSSS9U l        [        R                  R                  U R
                  5      S:  a  OMP  U R
                  USS & U R                  U R                   R                  SS	U R                  R                  S9-   nS
USS & U R                  X5        U R                  5       $ )Ngg?)r   r   sizegɿg?r   g{Gztg{Gzt?r   )	np_randomuniformmodelnq	init_qposgoalr#   rB   rC   	init_qvelnv	set_stater1   )r*   qposqvels      r,   reset_modelReacherEnv.reset_model   s    NN""t#DJJMM"Jnn 	 ..4c.JDIyy~~dii(3.  IIRS	~~ 6 6U !7 !
 
 RS	t"}}r.   c           
         U R                   R                  R                  5       S S n[        R                  " [        R
                  " U5      [        R                  " U5      U R                   R                  R                  5       SS  U R                   R                  R                  5       S S U R                  S5      U R                  S5      -
  S S /5      $ )Nr   r=   r>   )	datarT   flattenr#   concatenatecossinrU   rA   )r*   thetas     r,   r1   ReacherEnv._get_obs   s    		&&(!,~~uu		&&(,		&&(!,"";/$2C2CH2MMrPQR
 	
r.   )r"   r!   rP   r)   N)__name__
__module____qualname____firstlineno____doc__r)   DEFAULT_CAMERA_CONFIGstrr&   dictfloatr    r:   r2   rV   r1   __static_attributes__ r.   r,   r   r      s    yx 	 
H &8M$%'()
)
 )
  $C$45	)

 ")
  %)
V
7#"

r.   r   )__credits__numpyr#   	gymnasiumr   gymnasium.envs.mujocor   gymnasium.spacesr   re   r   r   rj   r.   r,   <module>rp      s:   $%   +   '* e
ENN e
r.   