Go to the documentation of this file.00001
00002 #include <stdio.h>
00003 #include <ros/ros.h>
00004 #include <kdl_parser/kdl_parser.hpp>
00005 #include <kdl/chaindynparam.hpp>
00006
00007 using namespace std;
00008
00009 int main(int argc, char **argv) {
00010 ros::init(argc, argv, "test_inertia");
00011 KDL::Tree my_tree;
00012 ros::NodeHandle node;
00013 string robot_desc_string;
00014 node.param("robot_description", robot_desc_string, string());
00015 if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
00016 ROS_ERROR("Failed to construct kdl tree");
00017 return false;
00018 }
00019 KDL::Chain r_arm_chain;
00020 my_tree.getChain("torso_lift_link", "r_gripper_palm_link", r_arm_chain);
00021 ROS_INFO("%d\n", r_arm_chain.getNrOfJoints());
00022 KDL::Vector g(0, 0, 1);
00023 KDL::ChainDynParam dyn_params(r_arm_chain, g);
00024 KDL::JntArray q(7);
00025 ROS_INFO("%f\n", q.data[0]);
00026 KDL::JntSpaceInertiaMatrix H(7);
00027 dyn_params.JntToMass(q, H);
00028 ROS_INFO("%d %d\n", H.columns(), H.rows());
00029 for(int i=0;i<7;i++) {
00030 for(int j=0;j<7;j++)
00031 std::printf("%f, ", H(i,j));
00032 std::printf("\n");
00033 }
00034 }