unconstr_dynamics.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_DYNAMICS_HPP_
#define ROBOTOC_UNCONSTR_DYNAMICS_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
 
 
namespace robotoc {
 
class UnconstrDynamics {
public:
  UnconstrDynamics(const Robot& robot);
 
  UnconstrDynamics();
 
  ~UnconstrDynamics() = default;
 
  UnconstrDynamics(const UnconstrDynamics&) = default;
 
  UnconstrDynamics& operator=(const UnconstrDynamics&) = default;
 
  UnconstrDynamics(UnconstrDynamics&&) noexcept = default;
 
  UnconstrDynamics& operator=(UnconstrDynamics&&) noexcept = default;
 
  void evalUnconstrDynamics(Robot& robot, const SplitSolution& s);
 
  void linearizeUnconstrDynamics(Robot& robot, const double dt, 
                                 const SplitSolution& s, 
                                 SplitKKTResidual& kkt_residual);
 
  void condenseUnconstrDynamics(SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual);
 
  void expandPrimal(SplitDirection& d) const;
 
  static void expandDual(const double dt, const SplitKKTMatrix& kkt_matrix, 
                         const SplitKKTResidual& kkt_residual, 
                         SplitDirection& d);
 
  template <int p=1>
  double primalFeasibility() const {
    return ID_.template lpNorm<p>();
  }
 
  double KKTError() const {
    return ID_.squaredNorm();
  }
 
  template <typename MatrixType1, typename MatrixType2>
  void getStateFeedbackGain(const Eigen::MatrixBase<MatrixType1>& Ka,
                            const Eigen::MatrixBase<MatrixType2>& Ku) const {
    assert(Ka.rows() == dimv_);
    assert(Ka.cols() == 2*dimv_);
    assert(Ku.rows() == dimv_);
    assert(Ku.cols() == 2*dimv_);
    getStateFeedbackGain(
        Ka.leftCols(dimv_), Ka.rightCols(dimv_),
        const_cast<Eigen::MatrixBase<MatrixType2>&>(Ku).leftCols(dimv_),
        const_cast<Eigen::MatrixBase<MatrixType2>&>(Ku).rightCols(dimv_));
  }
 
  template <typename MatrixType1, typename MatrixType2, typename MatrixType3, 
            typename MatrixType4>
  void getStateFeedbackGain(const Eigen::MatrixBase<MatrixType1>& Kaq,
                            const Eigen::MatrixBase<MatrixType2>& Kav,
                            const Eigen::MatrixBase<MatrixType3>& Kuq,
                            const Eigen::MatrixBase<MatrixType4>& Kuv) const {
    assert(Kaq.rows() == dimv_);
    assert(Kaq.cols() == dimv_);
    assert(Kav.rows() == dimv_);
    assert(Kav.cols() == dimv_);
    assert(Kuq.rows() == dimv_);
    assert(Kuq.cols() == dimv_);
    assert(Kuv.rows() == dimv_);
    assert(Kuv.cols() == dimv_);
    const_cast<Eigen::MatrixBase<MatrixType3>&>(Kuq) = dID_dq_;
    const_cast<Eigen::MatrixBase<MatrixType3>&>(Kuq).noalias() += dID_da_ * Kaq;
    const_cast<Eigen::MatrixBase<MatrixType4>&>(Kuv) = dID_dv_;
    const_cast<Eigen::MatrixBase<MatrixType4>&>(Kuv).noalias() += dID_da_ * Kav;
  }
 
private:
  Eigen::VectorXd ID_, lu_condensed_;
  Eigen::MatrixXd dID_dq_, dID_dv_, dID_da_, Quu_, Quu_dID_dq_, Quu_dID_dv_, 
                  Quu_dID_da_;
  int dimv_;
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_UNCONSTR_DYNAMICS_HPP_