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 <iostream>
00040 #include <sstream>
00041 #include <vector>
00042
00043 #include <ros/ros.h>
00044 #include <kdl_parser/kdl_parser.hpp>
00045 #include <kdl/chain.hpp>
00046 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
00047
00048 int main( int argc, char** argv )
00049 {
00050 ros::init(argc, argv, "kdl_dynamics");
00051
00052 KDL::Tree kdl_tree;
00053 ros::NodeHandle node;
00054 std::string robot_desc_string;
00055 node.param("robot_description", robot_desc_string, std::string());
00056 if (!kdl_parser::treeFromString(robot_desc_string, kdl_tree)){
00057 ROS_ERROR("Failed to construct KDL tree.");
00058 exit(-1);
00059 }
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069 KDL::Chain kdl_chain;
00070 if (!kdl_tree.getChain("torso_lift_link", "r_wrist_roll_link", kdl_chain)) {
00071 ROS_ERROR("Failed to get chain from KDL tree.");
00072 exit(-1);
00073 }
00074
00075 KDL::Vector grav;
00076 KDL::ChainIdSolver_RNE kdl_idsolver(kdl_chain, grav);
00077
00078 std::istream * in;
00079 in = &std::cin;
00080
00081 const size_t ndof(kdl_chain.getNrOfJoints());
00082 const size_t nstate(3 * ndof);
00083
00084 int status = 0;
00085
00086 for (size_t lineno(1); *in; ++lineno) {
00087 std::string line;
00088 getline(*in, line);
00089
00090 std::vector<double> state;
00091 std::istringstream ls(line);
00092 double value;
00093 while (ls >> value) {
00094 state.push_back(value);
00095 }
00096
00097
00098 if (0 == state.size()) {
00099 break;
00100 }
00101
00102
00103 if (nstate != state.size()) {
00104 ROS_ERROR("expected %zu DOF, but read line with %zu (!=%zu) doubles!", ndof, state.size(), nstate);
00105 break;
00106 }
00107
00108
00109 KDL::JntArray q = KDL::JntArray(ndof);
00110 KDL::JntArray q_dot = KDL::JntArray(ndof);
00111 KDL::JntArray q_dotdot = KDL::JntArray(ndof);
00112 KDL::JntArray torques = KDL::JntArray(ndof);
00113 KDL::Wrenches f_ext;
00114 f_ext.resize(ndof);
00115
00116 status = kdl_idsolver.CartToJnt(q, q_dot, q_dotdot, f_ext, torques);
00117 if (status < 0)
00118 ROS_ERROR("KDL inverse dynamics solver returned: %d", status);
00119
00120 for (uint i = 0; i < ndof; i++) {
00121 std::cout << torques(i) << " ";
00122 }
00123 std::cout << std::endl;
00124
00125 }
00126
00127 }