text: /* Copyright (C) 2010-2019, The Regents of The University of Michigan.
   All rights reserved.
  
   This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
   under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
   MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
  */
  
  
  /**
   * \file     mpepc_pose_follower.h
   * \author   Jong Jin Park
   *
   * Declaration of MPEPCPoseFollower.
   */
  
  #ifndef MPEPC_POSE_FOLLOWER_H
  #define MPEPC_POSE_FOLLOWER_H
  
  #include "core/pose.h"
  #include "mpepc/control/control_law_coordinates.h"
  #include "mpepc/trajectory/params.h"
  #include "mpepc/trajectory/robot_trajectory_info.h"
  #include <memory>
  #include <nlopt.h>
  #include <vector>
  
  namespace vulcan
  {
  
  namespace mpepc
  {
  
  class RobotSimulator;
  class TrajectoryEvaluator;
  class TaskManifold;
  struct trajectory_planner_debug_info_t;
  
  /**
   * MPEPCPoseFollower optimizes velocity gain given a pose to converge to.
   *
   * The optimizer handles the simulate/evaluate loop internally and finds the optimal
   * target along with the intermediate trajectories generated by the optimization
   * algorithm. NLopt package is used for the numerical optimzation, and a number of
   * different algorithms can be implemented.
   */
  class MPEPCPoseFollower
  {
  
  public:
      /**
       * Constructor for MPEPCPoseFollower.
       *
       * \param    params          Parameters for the optimization
       */
      MPEPCPoseFollower(const mpepc_optimizer_params_t& params);
  
      /**
       * Destructor for MPEPCPoseFollower.
       */
      ~MPEPCPoseFollower(void);
  
      /**
       * Setup sets up the optmizer with the robot simulator and trajectory evaluator to be used.
       *
       * \param    simulator       RobotSimulator to use for the trajectory generation.
       * \param    evaluator       TrajectoryEvaluator for calculating the cost of a trajectory.
       */
      void setup(RobotSimulator& simulator,
                 TrajectoryEvaluator& evaluator);   // NOTE: This feels very unsafe. A better way?
  
      /**
       * setInitialGuesses adds a goal pose (if navigation task) and the previous optimum
       * to the initial guesses for local optimizer.
       *
       * \param    task                 TaskManifold that contains the goal.
       * \param    previousMotionTarget Previous output of the metric planner, i.e. the previous optimum.
       */
      void setInitialGuesses(const TaskManifold& task, const motion_target_t& previousMotionTarget);
  
      /**
       * run runs the optimizer and finds the best motion target by evaluating the expected
       * costs of an estimated trajectory generated from a candidate motion target.
       *
       * It returns the optimal target found, while the intermediate information including
       * all trajectories considered are stored in the provided debug info instance.
       *
       * \param    debugInfo        Place to store relevant information
       * \return   Optimal target found
       */
      motion_target_t runOptimizer(trajectory_planner_debug_info_t& debugInfo);
  
      /**
       * evaluateCost calculates the cost of a fixed-length trajectory represented by the given optimizer_target_t.
       * The length of the trajectory is specified in the given instance of the planner_data_t.
       *
       * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a
       * velocity gain for the kinematic control law. \return   Cost of the trajectory.
       */
      double evaluateCost(const double x[]);
  
      /**
       * evaluateGradient calculates the gradient around a particular point, by a simple forward differencing.
       *
       * \param    coords          Optimization variable, currently encoding a target pose in the robot frame and a
       * velocity gain for the kinematic control law. \param    cost            Evaluated cost at the given coordinate.
       * \param    gradient        Calculated gradient for each dimension (r, theta, delta, velocity) [output]
       */
      void evaluateGradient(const double x[], double cost, double gradient[]);
  
      bool haveFoundSolution(void) const { return haveFoundSolution_; };
  
      //     /**
      //     * evaluateSingleMotionTarget is a method which resets the data and info struct and evaluate a single
      //     optimizer target.
      //     * The optimization_info_t will hold a single trajectory considered. TODO: Do i need this?
      //     *
      //     * \param candidateMotionTarget
      //     * \param data
      //     * \param info
      //     */
      //     double evaluateSingleMotionTarget(const motion_target_t& candidateMotionTarget, const planner_data_t& data,
      //     optimization_info_t& info);
  
  private:
      // setting up the optimizer
      void setupNLOpt(void);
  
      // optimizers
      nlopt_opt globalOptimizer;
      nlopt_opt localOptimizer;
  
      // simulator and evaluator
      RobotSimulator* simulator_;
      TrajectoryEvaluator* evaluator_;
  
      robot_trajectory_info_t trajectory;
  
      // debug info
      trajectory_planner_debug_info_t* optimizerInfo_;
  
      // inital robot state
      pose_t robotPose_;
  
      // target pose to converge to
      pose_t targetPose_;
      motion_target_t candidateMotionTarget_;
  
      // indicator for finding a right solution
      bool haveFoundSolution_;
  
      // parameters
      mpepc_optimizer_params_t params_;
  };
  
  
  }   // namespace mpepc
  }   // namespace vulcan
  
  #endif   // MPEPC_POSE_FOLLOWER_H
  