jspace/jspace/tao_util.hpp

Go to the documentation of this file.

/*
 * Stanford Whole-Body Control Framework http://stanford-wbc.sourceforge.net/
 *
 * Copyright (C) 2009 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_TAO_UTIL_H
#define JSPACE_TAO_UTIL_H

#include <stdexcept>
#include <string>
#include <vector>
#include <map>


class taoNodeRoot;
class taoDNode;
class taoJoint;


namespace jspace {

  struct tao_node_info_s {
    tao_node_info_s();
    tao_node_info_s(taoDNode * node, std::string const & link_name,
        std::string joint_name, double limit_lower, double limit_upper);
    tao_node_info_s(tao_node_info_s const & orig);
    
    int id;
    taoDNode * node;
    taoJoint * joint;
    std::string link_name;
    std::string joint_name;
    double limit_lower;
    double limit_upper;
  };
  
  
  struct tao_tree_info_s {
    tao_tree_info_s();
    
    virtual ~tao_tree_info_s();
    
    bool sort();
    
    taoNodeRoot * root;
    typedef std::vector<tao_node_info_s> node_info_t;
    node_info_t info;
  };
  
  
  tao_tree_info_s * create_bare_tao_tree_info(taoNodeRoot * root);
  
  
  int tao_consistency_check(
          taoNodeRoot * root,
          std::ostream * msg);
  
  
  typedef std::map<int, taoDNode *> idToNodeMap_t;
  
  void mapNodesToIDs(idToNodeMap_t & idToNodeMap,
         taoDNode * node)
    throw(std::runtime_error);
  
  
  int countNumberOfLinks(taoDNode * root);
  
  
  int countNumberOfJoints(taoDNode * node);
  
  
  int countDegreesOfFreedom(taoDNode * node);
  
  
  double computeTotalMass(taoDNode * node);
  
}

#endif // JSPACE_TAO_UTIL_H

---