Chassis.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "Leg.hpp"
#include "spatial.hpp"
#include <array>

class Chassis {
public:
  Chassis(Eigen::Matrix6d &G, Eigen::Isometry3d &Ms,
          std::array<Eigen::Isometry3d, 4> &M01)
      : G{G}, Ms{Ms}, M01{M01} {}

  bool ik(const Eigen::Isometry3d &Ts0,
          const std::array<Eigen::Isometry3d, 4> &T4,
          std::array<Eigen::Vector3d, 4> &pf,
          Eigen::Matrix<double, 6, 12> *Jinv);
  bool ivk(const Eigen::Vector6d &Vb, const Eigen::Matrix<double, 6, 12> &Jinv,
           std::array<Eigen::Vector3d, 4> &vf);

  bool id(std::array<Eigen::Vector3d, 4> &ff);

  bool nvp(const Eigen::Vector6d &F,
           const std::array<std::shared_ptr<Leg>, 4> &legs, double &K);
 
  void run();
 
  /* Chassis state */
  Eigen::Isometry3d Ts0; 
  Eigen::Vector6d Vb;    
  Eigen::Vector6d dVb;   
  Eigen::Vector6d Fb;    
 
  /* Rollover */
  double K;         
  int iRolloverLeg; 
 
  /* Legs */
  std::array<Eigen::Isometry3d, 4> T4; 
  std::array<std::shared_ptr<Leg>, 4> legs; 
 
  Eigen::Hyperplane<double, 3> footPlane; 
 
private:
  const Eigen::Matrix6d G; 
 
  // Body Frame in Home Configuration
  const Eigen::Isometry3d Ms; 
  const std::array<Eigen::Isometry3d, 4>
      M01; 
 
  Eigen::Matrix<double, 6, 12> Jinv; 
 
  const Eigen::Vector3d g{0, 0, 9.81}; 
};