DynamicsOptimizer.hpp

Go to the documentation of this file.

#pragma once

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

#include <solver/interface/Solver.hpp>
#include <momentumopt/utilities/Clock.hpp>
#include <momentumopt/setting/PlannerSetting.hpp>
#include <momentumopt/kinopt/KinematicsState.hpp>
#include <momentumopt/cntopt/ContactPlanInterface.hpp>

namespace momentumopt {

  struct FrictionCone
  {
    typedef Eigen::Matrix< double, 4, 3> ConeMat;
    void getCone(double fcoeff, FrictionCone::ConeMat& cone_mat)
    {
      if (fcoeff <= 0.001) { fcoeff = 0.001; }
      Eigen::Vector3d fconevec = Eigen::Vector3d(0.5*sqrt(2.0), 0.5*sqrt(2.0), -fcoeff); fconevec.normalize();
      Eigen::Matrix3d fconemat = Eigen::Matrix3d::Identity();
      double angle = 2*M_PI/4;
      fconemat(0,0) = cos(angle);    fconemat(0,1) = -sin(angle);
      fconemat(1,0) = sin(angle);    fconemat(1,1) =  cos(angle);
      for (int i=0; i<4; i++) {
        cone_mat.row(i) = fconevec;
        fconevec = fconemat * fconevec;
      }
    }
  };

  class DynamicsOptimizer
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
      DynamicsOptimizer(){}
      ~DynamicsOptimizer(){}

      void initialize(PlannerSetting& planner_setting);

      solver::ExitCode optimize(const DynamicsState& ini_state, ContactPlanInterface* contact_plan,
                                const KinematicsSequence& kin_sequence, bool update_tracking_objective = false);

      DynamicsSequence& dynamicsSequence() { return dyn_sequence_; }
      const DynamicsSequence& dynamicsSequence() const { return dyn_sequence_; }

      const double& solveTime() const { return solve_time_; }

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

      const ContactType contactType(int time_id, int eff_id) const;
      const Eigen::Matrix3d contactRotation(int time_id, int eff_id) const;
      const double contactLocation(int time_id, int eff_id, int axis_id) const;

      void initializeOptimizationVariables();

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

      void internalOptimize(const KinematicsSequence& kin_sequence, bool is_first_time = false);
      void updateTrackingObjective();

      void saveSolution(solver::OptimizationVariable& opt_var);
      void storeSolution();
      void saveToFile(const KinematicsSequence& kin_sequence);

    private:
      PlannerSetting* planner_setting_;

      solver::Model model_;

      solver::LinExpr lin_cons_;
      solver::DCPQuadExpr quad_objective_, quad_cons_;

      solver::ExitCode exitcode_;

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

      DynamicsState ini_state_;

      FrictionCone friction_cone_;
      FrictionCone::ConeMat cone_matrix_;

      DynamicsSequence dyn_sequence_;

      ContactPlanInterface* contact_plan_;

      Clock timer_;

      char variable_type_;

      bool has_converged_;

      int size_, num_vars_;

      double solve_time_, convergence_err_, last_convergence_err_;

      Eigen::Vector3d com_pos_goal_, weight_desired_com_tracking_;

      solver::OptimizationVariable dt_, com_, lmom_, amom_, lmomd_, amomd_;
      std::array<solver::OptimizationVariable, Problem::n_endeffs_> frc_world_, trq_local_, cop_local_, ub_var_, lb_var_;

      solver::OptimizationVariable::OptVector solution_;
      solver::OptimizationVariable::OptMatrix mat_lb_, mat_ub_, mat_guess_, com_guess_, lmom_guess_, amom_guess_;
  };

}