mbsstate.h

Go to the documentation of this file.

#ifndef GCOP_MBSSTATE_H
#define GCOP_MBSSTATE_H

#include <Eigen/Dense>
#include "se3.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;
  
  // state dimension for nb-body system

  class MbsState {
  public:
  MbsState(int nb = 1, bool fixed = false) : 
    gs(nb), vs(nb), dgs(nb-1),
      r(nb-1), dr(nb-1), ub(nb-1, false), lb(nb-1, false), 
      zu(VectorXd::Zero(nb-1)), zl(VectorXd::Zero(nb-1)), fixed(fixed) {
    }
    
  MbsState(const MbsState &x) : 
    gs(x.gs), vs(x.vs), dgs(x.dgs),
      r(x.r), dr(x.dr), ub(x.ub), lb(x.lb), 
      zl(x.zl), zu(x.zu), fixed(x.fixed) {
    }
    
    vector<Matrix4d> gs;        
    vector<Vector6d> vs;        
    vector<Matrix4d> dgs;       
    
    VectorXd r;  
    VectorXd dr; 
    vector<bool> ub; 
    vector<bool> lb; 

    VectorXd zl;  
    VectorXd zu;  

    bool fixed;  
  };
}

#endif