Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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 }