batch_wbc.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036   Author: Daniel Hennes
00037  */
00038 
00039 #include <vector>
00040 #include <iostream>
00041 #include <fstream>
00042 #include <sstream>
00043 #include "inverse_dynamics/InverseDynamicsSolverWBC.h"
00044 
00045 int main( int argc, char** argv )
00046 {
00047   if (argc != 2) {
00048     printf("define input filename!\n");
00049     exit(-1);
00050   }
00051 
00052   std::string infname(argv[1]);
00053   std::ifstream infile;
00054   infile.open(infname.c_str());
00055   if (!infile) {
00056     printf("couldn't load %s!\n", infname.c_str());
00057     exit(-1);
00058   }
00059   std::istream* is = &infile;
00060 
00061   std::string sai_xml_fname("./config/pr2_left_arm_wbc.sai.xml");
00062 
00063   std::vector<std::string> joint_names;
00064   joint_names.push_back("r_shoulder_pan_joint");
00065   joint_names.push_back("r_shoulder_lift_joint");
00066   joint_names.push_back("r_upper_arm_roll_joint");
00067   joint_names.push_back("r_elbow_flex_joint");
00068   joint_names.push_back("r_forearm_roll_joint");
00069   joint_names.push_back("r_wrist_flex_joint");
00070   joint_names.push_back("r_wrist_roll_joint");
00071 
00072   InverseDynamicsSolverWBC solver = 
00073     InverseDynamicsSolverWBC(sai_xml_fname);
00074 
00075   const size_t ndof = solver.getNumDOF();
00076   const size_t ndof2(2 * ndof);
00077   const size_t ndof3(3 * ndof);
00078   for (size_t lineno(1); *is; ++lineno) {
00079     std::vector<double> array;
00080     std::string line;
00081     getline(*is, line);
00082     std::istringstream ls(line);
00083     double value;
00084     while (ls >> value) {
00085       array.push_back(value);
00086     }
00087 
00088     if (array.size() == 0)
00089       break;
00090 
00091     if (ndof3 != array.size()) {
00092       printf("expected %zu values, got %zu!\n", ndof3, array.size());
00093       exit(-1);
00094     }
00095 
00096     std::vector<double> pos(ndof);
00097     std::vector<double> vel(ndof);
00098     std::vector<double> acc(ndof);
00099 
00100     std::copy(array.begin(), array.begin() + ndof, pos.begin());
00101     std::copy(array.begin() + ndof + 1, array.begin() + ndof2, vel.begin());
00102     std::copy(array.begin() + ndof2 + 1, array.end(), acc.begin());
00103 
00104     std::vector<double> tau(ndof);
00105     solver.updateState(pos, vel, acc);
00106     solver.getTorques(tau);
00107 
00108     for (std::vector<double>::iterator it = tau.begin(); it < tau.end(); it++) {
00109       std::cout << *it << " ";
00110     }
00111     std::cout << "\n";
00112 
00113   }
00114 }


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03