tao_util.hpp

Go to the documentation of this file.

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

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


class taoNodeRoot;
class taoDNode;
class taoJoint;


namespace jspace
{
  // =================================================================
  //                      Tao data structure wrappers
  // =================================================================

  struct STaoNodeInfo {
    STaoNodeInfo();
    STaoNodeInfo(taoDNode * node, std::string const & link_name,
        std::string joint_name, double limit_lower, double limit_upper);
    STaoNodeInfo(STaoNodeInfo const & orig);

    int id;
    taoDNode * node;
    taoJoint * joint;
    std::string link_name;
    std::string joint_name;
    double limit_lower;
    double limit_upper;
  };


  struct STaoTreeInfo {
    STaoTreeInfo();

    virtual ~STaoTreeInfo();

    bool sort();

    taoNodeRoot * root;
    std::vector<STaoNodeInfo> info;
  };

  // =================================================================
  //                     Tao helper functions
  // =================================================================

  STaoTreeInfo * create_bare_tao_tree_info(taoNodeRoot * root);


  int tao_consistency_check(
      taoNodeRoot * root,
      std::ostream * msg);

  void mapNodesToIDs(std::map<int, taoDNode*> & 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