jspace/jspace/Model.hpp

Go to the documentation of this file.

/*
 * Stanford Whole-Body Control Framework http://stanford-wbc.sourceforge.net/
 *
 * Copyright (C) 2010 The Board of Trustees of The Leland Stanford Junior 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;

namespace jspace {
  
  
  // declared in <jspace/tao_util.hpp>
  struct tao_tree_info_s;
  
  class Model
  {
  public:
    Model();
    
    ~Model();
    
    int init(
       tao_tree_info_s * kgm_tree,
       tao_tree_info_s * cc_tree,
       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(Vector & joint_limits_lower,
      Vector & joint_limits_upper) const;
    
    // kinematic facet
    
    void updateKinematics();
    
    bool getGlobalFrame(taoDNode const * node,
      Transform & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          Transform const & local_transform,
          Transform & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          double local_x, double local_y, double local_z,
          Transform & global_transform) const;
    
    bool computeGlobalFrame(taoDNode const * node,
          Vector const & local_translation,
          Transform & global_transform) const;
    
    bool computeGlobalCOMFrame(taoDNode const * node,
             Transform & global_com_transform) const;
    
    bool computeJacobian(taoDNode const * node,
       Matrix & jacobian) const;
    
    bool computeJacobian(taoDNode const * node,
       double gx, double gy, double gz,
       Matrix & jacobian) const;
    
    inline bool computeJacobian(taoDNode const * node,
        Vector const & global_point,
        Matrix & jacobian) const
    { return computeJacobian(node, global_point[0], global_point[1], global_point[2], jacobian); }
    
    // dynamics facet
    
    void updateDynamics();
    
    bool computeCOM(Vector & com, Matrix * opt_jcom) const;
    
    void computeGravity();
    
    bool disableGravityCompensation(size_t index, bool disable);
    
    bool getGravity(Vector & gravity) const;
    
    void computeCoriolisCentrifugal();
    
    bool getCoriolisCentrifugal(Vector & coriolis_centrifugal) const;
    
    void computeMassInertia();
    
    bool getMassInertia(Matrix & mass_inertia) const;
    
    void computeInverseMassInertia();
    
    bool getInverseMassInertia(Matrix & inverse_mass_inertia) const;
    
    
    tao_tree_info_s * _getKGMTree() { return kgm_tree_; }
    
    tao_tree_info_s * _getCCTree() { return cc_tree_; }
    
    
  private:
    typedef std::set<size_t> dof_set_t;
    dof_set_t gravity_disabled_;
    
    std::size_t ndof_;
    tao_tree_info_s * kgm_tree_;
    tao_tree_info_s * cc_tree_;
    
    State state_;
    Vector g_torque_;
    Vector cc_torque_;
    std::vector<double> a_upper_triangular_;
    std::vector<double> ainv_upper_triangular_;
    
    struct ancestry_entry_s {
      int id;
      taoJoint * joint;
    };
    typedef std::list<ancestry_entry_s> ancestry_list_t;
    typedef std::map<taoDNode *, ancestry_list_t> ancestry_table_t;
    ancestry_table_t ancestry_table_;
  };
  
}

#endif // JSPACE_MODEL_HPP

---