KinematicsOptimizer.hpp

Go to the documentation of this file.

#pragma once

#include <string>
#include <vector>
#include <memory>
#include <iostream>
#include <Eigen/Eigen>

#include <solver/interface/Solver.hpp>
//#include <solver/optimizer/LbfgsSolver.hpp>
#include <momentumopt/dynopt/DynamicsState.hpp>
#include <momentumopt/setting/PlannerSetting.hpp>
//#include <momentumopt/cntopt/ContactPlanFromFile.hpp>
#include <momentumopt/kinopt/KinematicsInterface.hpp>
#include <momentumopt/cntopt/ContactPlanInterface.hpp>

namespace momentumopt {

  class KinematicsOptimizer
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
      KinematicsOptimizer(){}
      ~KinematicsOptimizer();

      void initialize(PlannerSetting& planner_setting, KinematicsInterface* kin_interface);

      void optimize(const DynamicsState& ini_state, ContactPlanInterface* contact_plan, DynamicsSequence& dyn_sequence, bool is_first_kinopt);

      KinematicsSequence& kinematicsSequence() { return kin_sequence_; }
      const KinematicsSequence& kinematicsSequence() const { return kin_sequence_; }

//      /**
//       * helper function to replay a motion
//       * @param[in]  PlannerSetting                       setting of planner configuration variables
//       * @param[in]  kin_interface                        implementation of robot specific functions needed by the optimizer
//       * @param[in]  load_file                            file where the kinematic parameters have been stored
//       */
//      void displayMotion(PlannerSetting& planner_setting, KinematicsInterface* kin_interface, const std::string& load_file);
//
//      /**
//       * function to load a kinematic solution from a file and fill a sequence with it
//       * @param[in]  load_file                             name of configuration file from where to load the solution
//       */
//      void loadSolution(const std::string& load_file);

    private:
      inline PlannerSetting& getSetting() { return *planner_setting_; }
      inline const PlannerSetting& getSetting() const { return *planner_setting_; }

      void initializePostureKinematicVariables();
      void initializeTrajectoryKinematicVariables();

      void addVariableToModel(const solver::OptimizationVariable& opt_var, solver::Model& model, std::vector<solver::Var>& vars);

      void saveSolution(solver::OptimizationVariable& opt_var);
      void storeSolution(const DynamicsState& ini_state, DynamicsSequence& dyn_sequence);

      void optimizeTrajectory(const DynamicsState& ini_state, DynamicsSequence& dyn_sequence);
      void optimizePosture(KinematicsState& current_state, const DynamicsState& desired_state, bool is_initial_state = false);

      void displayMotion(const DynamicsSequence& dyn_sequence);

      void updateEndeffectorTrajectories(DynamicsSequence& dyn_sequence);

      // quaternion to rotation vector
      Eigen::Vector3d quaternionToRotationVector(const Eigen::Quaternion<double>& q) const;

    private:
      PlannerSetting* planner_setting_;

      solver::Model model_;

      solver::LinExpr lin_cons_;

      solver::ExitCode exitcode_;

      std::vector<solver::Var> vars_;

      KinematicsState *ini_state_, *current_state_;

      ContactPlanInterface* contact_plan_;

      KinematicsSequence kin_sequence_;

      KinematicsInterface* kin_interface_;

      char variable_type_;

      int size_, num_vars_;

      std::array<solver::OptimizationVariable, Problem::n_endeffs_> eef_pos_, eef_vel_, eef_ang_vel_;
      solver::OptimizationVariable jnt_q_, jnt_qd_, jnt_qdd_, total_qd_, total_qdd_, com_, comd_,
                                   lmom_, amom_, base_position_, base_ang_vel_, slack_vars_;

      solver::OptimizationVariable::OptVector solution_;
      solver::OptimizationVariable::OptMatrix mat_guess_, mat_lb_, mat_ub_;

      std::array<Eigen::Vector3d, Problem::n_endeffs_> desired_eff_velocity_;
      Eigen::Vector3d desired_com_velocity_, desired_lmom_velocity_, desired_amom_velocity_;
  };

}