mbs.h

Go to the documentation of this file.

// This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP)
//
// Copyright (C) 2004-2014 Marin Kobilarov <marin(at)jhu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef GCOP_MBS_H
#define GCOP_MBS_H

#include "body3d.h"
#include "joint.h"
#include "mbsmanifold.h"
#include "function.h"
#include "mbscspace.h"
#include "mbstspace.h"
#include <exception>      // std::exception
#include <stdexcept>

namespace gcop {
  
  using namespace std;
  using namespace Eigen;
  
  class Mbs : public System<MbsState> {
    
  public:

    Mbs(int nb, int c, bool fixed = false);

    virtual ~Mbs();

    void Init();
    
    double Step(MbsState& xb, double t, const MbsState& xa,
                const VectorXd &u, double h, const VectorXd *p = 0,
                MatrixXd *A = 0, MatrixXd *B = 0, MatrixXd *C = 0);


    double EulerStep(MbsState& xb, double t, const MbsState& xa,
                     const VectorXd &u, double h,
                     MatrixXd *A = 0, MatrixXd *B = 0);


    double HeunStep(MbsState& xb, double t, const MbsState& xa,
                    const VectorXd &u, double h,
                    MatrixXd *A = 0, MatrixXd *B = 0);

    double TrapStep(MbsState& xb, double t, const MbsState& xa,
                    const VectorXd &u, double h,
                    MatrixXd *A, MatrixXd *B);      


    void NE(VectorXd &e, const VectorXd &vdr, 
            MbsState &xb,               
            double t, const MbsState &xa, 
            const VectorXd &u, double h);


    void ID(VectorXd &f,
            double t, const MbsState &x,
            const VectorXd &u);

    void Bias(VectorXd &b,
              double t, const MbsState &x) const;

    void DBias(VectorXd &b,
                double t,
                const MbsState &xb, 
                const MbsState &xa, double h);    
    
    void FK(MbsState &x);

    void KStep(MbsState &xb, const MbsState &xa, double h, bool impl = true);

    void NewtonEulerJacobian(MatrixXd &De, const MbsState &xb, const MbsState &xa, double h);  

    void Mass(MatrixXd &M, const MbsState &x) const;

    void Acc(VectorXd &a, double t, const MbsState& x, const VectorXd &u, double h);

    virtual void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 
                       MatrixXd *A = 0, MatrixXd *B = 0);
    
    void Rec(MbsState &x, double h);

    void ClampPose(MbsState &x, int i) const;

    void CheckLimits(MbsState &x, int i, double h) const;

    void GetImpulse(double f, const MbsState &x, int i, double h) const;

    void ClampVelocity(MbsState &x) const;

    void print(const MbsState &x) const; 

    int nb;                   

    bool fixed;               

    vector<Body3d<>> links;   

    vector<Joint> joints;     

    vector<Matrix6d> Ips;     
    
    vector<int> pis;          
    vector<vector<int> > cs;  

    Vector3d ag;             

    VectorXd damping;        
    
    VectorXd lbK;            
    VectorXd lbD;            
    VectorXd ubK;            
    VectorXd ubD;            
    VectorXd fsl;             
    VectorXd fsu;             
    
    int basetype;            
    static const int FIXEDBASE = 0;   
    static const int FLOATBASE = 1;   
    static const int AIRBASE = 2;     

    SE3 &se3;                 

    int method;                   
    static const int EULER = 1;   
    static const int HEUN = 2;    
    static const int TRAP = 3;    
    int iters;                    

    bool debug;
  };
}

#endif