*******************************
Task 1: Move Cube on Trajectory
*******************************

.. image:: images/trifingerpro_with_cube.jpg
   :alt: TriFinger robot holding a cube

.. automodule:: trifinger_simulation.tasks.move_cube_on_trajectory

   .. autodata:: EPISODE_LENGTH
   .. autofunction:: sample_goal

   .. function:: evaluate_state(trajectory, time_index, actual_position)

      Compute cost of a given cube pose at a given time step.  Less is better.

      As cost the position error between actual_position and the trajectory goal
      that is active at the given time_index is used.
      It is computed as a weighted sum of the Euclidean
      distance on the x/y-plane and the absolute distance on the z-axis.
      Both components are scaled based on their expected range.  Since the
      z-range is smaller, this means that the height has a higher weight.
      The sum is again rescaled so that the total error is in the interval
      ``[0, 1]``.

      ::

          Input: active goal position and actual cube position

          err_xy = ||goal[xy] - actual[xy]||
          err_z  = |goal[z] - actual[z]|

          cost = (err_xy / arena_diameter + err_z / height_range) / 2


      :param trajectory: The trajectory based on which the cost is computed.
      :type trajectory: Sequence[Tuple[int, Sequence[float]]]
      :param int time_index: Index of the time step that is evaluated.
      :param Sequence[float] actual_position: Cube position at the specified
                                              time step.
      :return: Cost of the actual position w.r.t. to the goal position of the
               active step in the trajectory.  Lower value means that the actual
               pose is closer to the goal.  Zero if actual == goal.

   .. autofunction:: get_active_goal
   .. autofunction:: validate_goal
   .. autofunction:: json_goal_from_config
   .. autofunction:: seed