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

Merge pull request #117 from ros/fix-com-import 

Fix com import
This commit is contained in:
Jackie Kay 2016-01-14 16:56:09 -08:00
parent 1e74e643e8 ed6237cf92
commit 2057b2b313
6 changed files with 192 additions and 2 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)
add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp )
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()
# How does CATKIN do this?

26
kdl_parser/src/kdl_parser.cpp
View File

@ -96,8 +96,30 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
Frame origin = toKdl(i->origin);
// kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
// the mass is frame indipendent
double kdl_mass = i->mass;
// kdl and urdf both specify the com position in the reference frame of the link
Vector kdl_com = origin.p;
// kdl specifies the inertia matrix in the reference frame of the link,
// while the urdf specifies the inertia matrix in the inertia reference frame
RotationalInertia urdf_inertia =
RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
// Rotation operators are not defined for rotational inertia,
// so we use the RigidBodyInertia operators (with com = 0) as a workaround
RigidBodyInertia kdl_inertia_wrt_com_workaround =
origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
// Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
// while the getRotationalInertia method returns the 3d inertia wrt the frame origin
// (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
RotationalInertia kdl_inertia_wrt_com =
kdl_inertia_wrt_com_workaround.getRotationalInertia();
return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
}

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>