matrix-inertia.h

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#ifndef __SOT_SOTMATRIXINERTIA_H__
#define __SOT_SOTMATRIXINERTIA_H__
 
#include <ostream>
#include <sot/core/matrix-geometry.hh>
 
#include "dynamic-graph/linear-algebra.h"
 
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(matrix_inertia_EXPORTS)
#define SOTMATRIXINERTIA_EXPORT __declspec(dllexport)
#else
#define SOTMATRIXINERTIA_EXPORT __declspec(dllimport)
#endif
#else
#define SOTMATRIXINERTIA_EXPORT
#endif
 
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
 
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
class SOTMATRIXINERTIA_EXPORT MatrixInertia {
 public:
 private:
  MatrixInertia(void) {}
 
  void initParents(void);
  void initDofTable(void);
 
 public:
  MatrixInertia(CjrlHumanoidDynamicRobot* aHDR);
  ~MatrixInertia(void);
  void init(CjrlHumanoidDynamicRobot* aHDR);
 
 public:
  void update(void);
  void computeInertiaMatrix();
  void getInertiaMatrix(double* A);
  const maal::boost::Matrix& getInertiaMatrix(void);
  size_t getDoF() { return joints_.size(); }
 
 private:
  CjrlHumanoidDynamicRobot* aHDR_;
  dynamicsJRLJapan::HumanoidDynamicMultiBody* aHDMB_;
  std::vector<CjrlJoint*> joints_;
  std::vector<int> parentIndex_;
 
  std::vector<dynamicgraph::Matrix> Ic;
  std::vector<dynamicgraph::Vector> phi;
  std::vector<MatrixTwist> iVpi;
  std::vector<MatrixForce> iVpiT;
  dynamicgraph::Matrix inertia_;
};
 
} /* namespace sot */
} /* namespace dynamicgraph */
 
#endif  // __SOT_SOTMATRIXINERTIA_H__