bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add COM import test

This commit is contained in:
Jackie Kay 2015-10-13 13:24:02 -07:00
parent add47a792b
commit ed6237cf92
5 changed files with 168 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
kdl_parser/CMakeLists.txt
View File

@ -38,6 +38,9 @@ if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest) find_package(catkin REQUIRED COMPONENTS rostest)
add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp ) add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp )
target_link_libraries(test_kdl_parser ${PROJECT_NAME}) target_link_libraries(test_kdl_parser ${PROJECT_NAME})
add_rostest_gtest(test_inertia_rpy test/test_inertia_rpy.launch test/test_inertia_rpy.cpp )
target_link_libraries(test_inertia_rpy ${PROJECT_NAME})
endif() endif()
# How does CATKIN do this? # How does CATKIN do this?

29
kdl_parser/test/testInertiaRPYmodel1.urdf Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<robot name="testInertialRPYmodel1">
<link name="base_link" />
<joint name="base_fixed_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 -0 0" />
<axis xyz="0 0 0" />
<parent link="base_link" />
<child link="link1" />
</joint>
<link name="link1">
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<joint name="joint_1_2" type="continuous">
<origin xyz="0 0 0" rpy="0 -0 0" />
<axis xyz="0 0 1" />
<parent link="link1" />
<child link="link2" />
</joint>
<link name="link2">
<inertial>
<mass value="100" />
<origin xyz="1 2 3" rpy="0 -0 0" />
<inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
</inertial>
</link>
</robot>

29
kdl_parser/test/testInertiaRPYmodel2.urdf Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<robot name="testInertialRPYmodel2">
<link name="base_link" />
<joint name="base_fixed_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 -0 0" />
<axis xyz="0 0 0" />
<parent link="base_link" />
<child link="link1" />
</joint>
<link name="link1">
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<joint name="joint_1_2" type="continuous">
<origin xyz="0 0 0" rpy="0 -0 0" />
<axis xyz="0 0 1" />
<parent link="link1" />
<child link="link2" />
</joint>
<link name="link2">
<inertial>
<mass value="100" />
<origin xyz="1 2 3" rpy="3.141592653589793 0 0" />
<inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
</inertial>
</link>
</robot>

101
kdl_parser/test/test_inertia_rpy.cpp Normal file
View File

@ -0,0 +1,101 @@
#include <string>
#include <gtest/gtest.h>
#include <kdl/jntarray.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <ros/ros.h>
#include <ros/console.h>
#include "kdl_parser/kdl_parser.hpp"
using namespace kdl_parser;
int g_argc;
char** g_argv;
class TestInertiaRPY : public testing::Test
{
public:
protected:
/// constructor
TestInertiaRPY()
{
}
/// Destructor
~TestInertiaRPY()
{
}
};
TEST_F(TestInertiaRPY, test_torques)
{
//ASSERT_EQ(g_argc, 3);
// workaround for segfault issue with parsing 2 trees instantiated on the stack
KDL::Tree * tree_1 = new KDL::Tree;
KDL::Tree * tree_2 = new KDL::Tree;
KDL::JntArray torques_1;
KDL::JntArray torques_2;
{
ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1));
KDL::Vector gravity(0, 0, -9.81);
KDL::Chain chain;
std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl;
std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl;
ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain));
KDL::ChainIdSolver_RNE solver(chain, gravity);
//JntArrays get initialized with all 0 values
KDL::JntArray q(chain.getNrOfJoints());
KDL::JntArray qdot(chain.getNrOfJoints());
KDL::JntArray qdotdot(chain.getNrOfJoints());
//KDL::JntArray torques(chain.getNrOfJoints());
std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1);
delete tree_1;
tree_1 = NULL;
}
{
ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2));
KDL::Vector gravity(0, 0, -9.81);
KDL::Chain chain;
ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain));
KDL::ChainIdSolver_RNE solver(chain, gravity);
//JntArrays get initialized with all 0 values
KDL::JntArray q(chain.getNrOfJoints());
KDL::JntArray qdot(chain.getNrOfJoints());
KDL::JntArray qdotdot(chain.getNrOfJoints());
//KDL::JntArray torques(chain.getNrOfJoints());
std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2);
delete tree_2;
tree_2 = NULL;
}
ASSERT_TRUE(torques_1 == torques_2);
SUCCEED();
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_kdl_parser");
for (size_t i = 0; i < argc; ++i) {
std::cout << argv[i] << std::endl;
}
g_argc = argc;
g_argv = argv;
return RUN_ALL_TESTS();
}

6
kdl_parser/test/test_inertia_rpy.launch Normal file
View File

@ -0,0 +1,6 @@
<launch>
<test test-name="test_inertia_rpy" pkg="kdl_parser" type="test_inertia_rpy"
args="$(find kdl_parser)/test/testInertiaRPYmodel1.urdf
$(find kdl_parser)/test/testInertiaRPYmodel2.urdf"
/>
</launch>