Model.hpp

Go to the documentation of this file.

/*
 * Stanford Whole-Body Control Framework http://stanford-scl.sourceforge.net/
 *
 * Copyright (c) 2010 Stanford University. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#ifndef JSPACE_MODEL_HPP
#define JSPACE_MODEL_HPP

#include <jspace/State.hpp>
#include <jspace/wrap_eigen.hpp>
#include <string>
#include <vector>
#include <list>
#include <map>
#include <set>

// Clients of Model never really need to worry about what exactly lies
// behind TAO, they can treat this as an opaque pointer type.
class taoDNode;
class taoJoint;
class deVector3;

namespace jspace {
  
  
  // declared in <jspace/tao_util.hpp>
  struct STaoTreeInfo;
  
  class Model
  {
  public:
    Model();
    
    ~Model();
    
    int init(
       STaoTreeInfo * kgm_tree,
       STaoTreeInfo * cc_tree,
       double grav_x_,
       double grav_y_,
       double grav_z_,
       std::ostream * msg);
    
    // fire-and-forget facet
    
    void update(State const & state);
    
    void setState(State const & state);
    
    inline State const & getState() const { return state_; }
    
    // Bare tree accessors.
    
    size_t getNNodes() const;
    
    size_t getNJoints() const;
    
    size_t getNDOF() const;
    
    std::string getNodeName(size_t id) const;
    
    std::string getJointName(size_t id) const;
    
    taoDNode * getNode(size_t id) const;
    
    taoDNode * getNodeByName(std::string const & name) const;
    
    taoDNode * getNodeByJointName(std::string const & name) const;
    
    void getJointLimits(Eigen::VectorXd & joint_limits_lower,
      Eigen::VectorXd & joint_limits_upper) const;
    
    // kinematic facet
    
    void updateKinematics();
    
    bool getGlobalFrame(taoDNode const * node,
      Eigen::Affine3d & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          Eigen::Affine3d const & local_transform,
          Eigen::Affine3d & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          double local_x, double local_y, double local_z,
          Eigen::Affine3d & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          Eigen::VectorXd const & local_translation,
          Eigen::Affine3d & global_transform) const;
    
    bool computeGlobalCOMFrame(taoDNode const * node,
             Eigen::Affine3d & global_com_transform) const;
    
    bool computeJacobian(taoDNode const * node,
       Eigen::MatrixXd & jacobian) const;
    
    bool computeJacobian(taoDNode const * node,
       double gx, double gy, double gz,
       Eigen::MatrixXd & jacobian) const;
    
    inline bool computeJacobian(taoDNode const * node,
        Eigen::VectorXd const & global_point,
        Eigen::MatrixXd & jacobian) const
    { return computeJacobian(node, global_point[0], global_point[1], global_point[2], jacobian); }
    
    // dynamics facet
    
    void updateDynamics();
    
    bool computeCOM(Eigen::VectorXd & com, Eigen::MatrixXd * opt_jcom) const;
    
    void computeGravity();

    bool disableGravityCompensation(size_t index, bool disable);
    
    bool getGravity(Eigen::VectorXd & gravity) const;
    
    void computeCoriolisCentrifugal();
    
    bool getCoriolisCentrifugal(Eigen::VectorXd & coriolis_centrifugal) const;
    
    void computeMassInertia();
    
    bool getMassInertia(Eigen::MatrixXd & mass_inertia) const;
    
    void computeInverseMassInertia();
    
    bool getInverseMassInertia(Eigen::MatrixXd & inverse_mass_inertia) const;
    
    
    STaoTreeInfo * _getKGMTree() { return kgm_tree_; }
    
    STaoTreeInfo * _getCCTree() { return cc_tree_; }
    
    
  private:
    typedef std::set<size_t> dof_set_t;
    
    std::size_t ndof_;
    STaoTreeInfo * kgm_tree_;
    STaoTreeInfo * cc_tree_;

    dof_set_t gravity_disabled_;
    deVector3* gravity_;
    
    State state_;
    Eigen::VectorXd g_torque_;
    Eigen::VectorXd cc_torque_;
    std::vector<double> a_upper_triangular_;
    std::vector<double> ainv_upper_triangular_;
    
    struct SAncestryEntry {
      int id;
      taoJoint * joint;
    };

    std::map<taoDNode *, std::list<SAncestryEntry> > ancestry_table_;
  };
  
}

#endif // JSPACE_MODEL_HPP