Style fixes from ros2 (#11)
* Style fixes (these can go to upstream kdl_parser) Signed-off-by: Chris Lalancette <clalancette@openrobotics.org> * Windows compatibility. Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
This commit is contained in:
Chris Lalancette
Shane Loretz
parent
166680fd57
commit
07221fdc64
|
|
@ -30,6 +30,10 @@ target_link_libraries(${PROJECT_NAME}
|
|||
${TinyXML_LIBRARIES} ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
if(WIN32)
|
||||
target_compile_definitions(${PROJECT_NAME} PRIVATE "KDL_PARSER_BUILDING_DLL")
|
||||
endif()
|
||||
|
||||
add_executable(check_kdl_parser src/check_kdl_parser.cpp )
|
||||
target_link_libraries(check_kdl_parser ${PROJECT_NAME})
|
||||
|
||||
|
|
|
|||
|
|
@ -1,13 +1,13 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
|
|
@ -34,50 +34,58 @@
|
|||
|
||||
/* Author: Wim Meeussen */
|
||||
|
||||
#ifndef KDL_PARSER_H
|
||||
#define KDL_PARSER_H
|
||||
#ifndef KDL_PARSER__KDL_PARSER_HPP_
|
||||
#define KDL_PARSER__KDL_PARSER_HPP_
|
||||
|
||||
#include <kdl/tree.hpp>
|
||||
#include <string>
|
||||
#include <urdf_model/model.h>
|
||||
#include <tinyxml.h>
|
||||
#include <tinyxml.h> // NOLINT
|
||||
|
||||
namespace kdl_parser{
|
||||
#include "kdl_parser/visibility_control.hpp"
|
||||
|
||||
namespace kdl_parser
|
||||
{
|
||||
|
||||
/** Constructs a KDL tree from a file, given the file name
|
||||
* \param file The filename from where to read the xml
|
||||
* \param tree The resulting KDL Tree
|
||||
* returns true on success, false on failure
|
||||
*/
|
||||
bool treeFromFile(const std::string& file, KDL::Tree& tree);
|
||||
KDL_PARSER_PUBLIC
|
||||
bool treeFromFile(const std::string & file, KDL::Tree & tree);
|
||||
|
||||
/** Constructs a KDL tree from the parameter server, given the parameter name
|
||||
* \param param the name of the parameter on the parameter server
|
||||
* \param tree The resulting KDL Tree
|
||||
* returns true on success, false on failure
|
||||
*/
|
||||
bool treeFromParam(const std::string& param, KDL::Tree& tree);
|
||||
KDL_PARSER_PUBLIC
|
||||
bool treeFromParam(const std::string & param, KDL::Tree & tree);
|
||||
|
||||
/** Constructs a KDL tree from a string containing xml
|
||||
* \param xml A string containting the xml description of the robot
|
||||
* \param tree The resulting KDL Tree
|
||||
* returns true on success, false on failure
|
||||
*/
|
||||
bool treeFromString(const std::string& xml, KDL::Tree& tree);
|
||||
KDL_PARSER_PUBLIC
|
||||
bool treeFromString(const std::string & xml, KDL::Tree & tree);
|
||||
|
||||
/** Constructs a KDL tree from a TiXmlDocument
|
||||
* \param xml_doc The TiXmlDocument containting the xml description of the robot
|
||||
* \param tree The resulting KDL Tree
|
||||
* returns true on success, false on failure
|
||||
*/
|
||||
bool treeFromXml(TiXmlDocument *xml_doc, KDL::Tree& tree);
|
||||
KDL_PARSER_PUBLIC
|
||||
bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree);
|
||||
|
||||
/** Constructs a KDL tree from a URDF robot model
|
||||
* \param robot_model The URDF robot model
|
||||
* \param tree The resulting KDL Tree
|
||||
* returns true on success, false on failure
|
||||
*/
|
||||
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, KDL::Tree& tree);
|
||||
}
|
||||
KDL_PARSER_PUBLIC
|
||||
bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree);
|
||||
} // namespace kdl_parser
|
||||
|
||||
#endif
|
||||
#endif // KDL_PARSER__KDL_PARSER_HPP_
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Open Source Robotics Foundation, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* This header must be included by all kdl_parser headers which declare symbols
|
||||
* which are defined in the kdl_parser library. When not building the kdl_parser
|
||||
* library, i.e. when using the headers in other package's code, the contents
|
||||
* of this header change the visibility of certain symbols which the kdl_parser
|
||||
* library cannot have, but the consuming code must have inorder to link.
|
||||
*/
|
||||
|
||||
#ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_
|
||||
#define KDL_PARSER__VISIBILITY_CONTROL_HPP_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define KDL_PARSER_EXPORT __attribute__ ((dllexport))
|
||||
#define KDL_PARSER_IMPORT __attribute__ ((dllimport))
|
||||
#else
|
||||
#define KDL_PARSER_EXPORT __declspec(dllexport)
|
||||
#define KDL_PARSER_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef KDL_PARSER_BUILDING_DLL
|
||||
#define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT
|
||||
#else
|
||||
#define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT
|
||||
#endif
|
||||
#define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC
|
||||
#define KDL_PARSER_LOCAL
|
||||
#else
|
||||
#define KDL_PARSER_EXPORT __attribute__ ((visibility("default")))
|
||||
#define KDL_PARSER_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define KDL_PARSER_PUBLIC __attribute__ ((visibility("default")))
|
||||
#define KDL_PARSER_LOCAL __attribute__ ((visibility("hidden")))
|
||||
#else
|
||||
#define KDL_PARSER_PUBLIC
|
||||
#define KDL_PARSER_LOCAL
|
||||
#endif
|
||||
#define KDL_PARSER_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // KDL_PARSER__VISIBILITY_CONTROL_HPP_
|
||||
|
|
@ -1,13 +1,13 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
|
|
@ -34,45 +34,46 @@
|
|||
|
||||
/* Author: Wim Meeussen */
|
||||
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
|
||||
#include "kdl_parser/kdl_parser.hpp"
|
||||
#include <kdl/chainfksolverpos_recursive.hpp>
|
||||
#include <kdl/frames_io.hpp>
|
||||
#include <urdf/model.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace KDL;
|
||||
using namespace std;
|
||||
using namespace urdf;
|
||||
|
||||
void printLink(const SegmentMap::const_iterator& link, const std::string& prefix)
|
||||
void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix)
|
||||
{
|
||||
cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << " has "
|
||||
<< GetTreeElementChildren(link->second).size() << " children" << endl;
|
||||
for (unsigned int i=0; i < GetTreeElementChildren(link->second).size(); i++)
|
||||
printLink(GetTreeElementChildren(link->second)[i], prefix + " ");
|
||||
std::cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() <<
|
||||
" has " << GetTreeElementChildren(link->second).size() << " children" << std::endl;
|
||||
for (unsigned int i = 0; i < GetTreeElementChildren(link->second).size(); i++) {
|
||||
printLink(GetTreeElementChildren(link->second)[i], prefix + " ");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
if (argc < 2){
|
||||
if (argc < 2) {
|
||||
std::cerr << "Expect xml file to parse" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
Model robot_model;
|
||||
if (!robot_model.initFile(argv[1]))
|
||||
{cerr << "Could not generate robot model" << endl; return false;}
|
||||
urdf::Model robot_model;
|
||||
if (!robot_model.initFile(argv[1])) {
|
||||
std::cerr << "Could not generate robot model" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
Tree my_tree;
|
||||
if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree))
|
||||
{cerr << "Could not extract kdl tree" << endl; return false;}
|
||||
KDL::Tree my_tree;
|
||||
if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) {
|
||||
std::cerr << "Could not extract kdl tree" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// walk through tree
|
||||
cout << " ======================================" << endl;
|
||||
cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << endl;
|
||||
cout << " ======================================" << endl;
|
||||
SegmentMap::const_iterator root = my_tree.getRootSegment();
|
||||
std::cout << " ======================================" << std::endl;
|
||||
std::cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << std::endl;
|
||||
std::cout << " ======================================" << std::endl;
|
||||
KDL::SegmentMap::const_iterator root = my_tree.getRootSegment();
|
||||
printLink(root, "");
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,13 +1,13 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
|
|
@ -35,132 +35,137 @@
|
|||
/* Author: Wim Meeussen */
|
||||
|
||||
#include "kdl_parser/kdl_parser.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <urdf/model.h>
|
||||
#include <urdf/urdfdom_compatibility.h>
|
||||
#include <kdl/frames_io.hpp>
|
||||
#include <ros/console.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace KDL;
|
||||
|
||||
namespace kdl_parser{
|
||||
|
||||
namespace kdl_parser
|
||||
{
|
||||
|
||||
// construct vector
|
||||
Vector toKdl(urdf::Vector3 v)
|
||||
KDL::Vector toKdl(urdf::Vector3 v)
|
||||
{
|
||||
return Vector(v.x, v.y, v.z);
|
||||
return KDL::Vector(v.x, v.y, v.z);
|
||||
}
|
||||
|
||||
// construct rotation
|
||||
Rotation toKdl(urdf::Rotation r)
|
||||
KDL::Rotation toKdl(urdf::Rotation r)
|
||||
{
|
||||
return Rotation::Quaternion(r.x, r.y, r.z, r.w);
|
||||
return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
|
||||
}
|
||||
|
||||
// construct pose
|
||||
Frame toKdl(urdf::Pose p)
|
||||
KDL::Frame toKdl(urdf::Pose p)
|
||||
{
|
||||
return Frame(toKdl(p.rotation), toKdl(p.position));
|
||||
return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
|
||||
}
|
||||
|
||||
// construct joint
|
||||
Joint toKdl(urdf::JointSharedPtr jnt)
|
||||
KDL::Joint toKdl(urdf::JointSharedPtr jnt)
|
||||
{
|
||||
Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
|
||||
KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
|
||||
|
||||
switch (jnt->type){
|
||||
case urdf::Joint::FIXED:{
|
||||
return Joint(jnt->name, Joint::None);
|
||||
switch (jnt->type) {
|
||||
case urdf::Joint::FIXED: {
|
||||
return KDL::Joint(jnt->name, KDL::Joint::None);
|
||||
}
|
||||
case urdf::Joint::REVOLUTE: {
|
||||
KDL::Vector axis = toKdl(jnt->axis);
|
||||
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
|
||||
}
|
||||
case urdf::Joint::CONTINUOUS: {
|
||||
KDL::Vector axis = toKdl(jnt->axis);
|
||||
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
|
||||
}
|
||||
case urdf::Joint::PRISMATIC: {
|
||||
KDL::Vector axis = toKdl(jnt->axis);
|
||||
return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
|
||||
}
|
||||
default: {
|
||||
ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint",
|
||||
jnt->name.c_str());
|
||||
return KDL::Joint(jnt->name, KDL::Joint::None);
|
||||
}
|
||||
}
|
||||
case urdf::Joint::REVOLUTE:{
|
||||
Vector axis = toKdl(jnt->axis);
|
||||
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
|
||||
}
|
||||
case urdf::Joint::CONTINUOUS:{
|
||||
Vector axis = toKdl(jnt->axis);
|
||||
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::RotAxis);
|
||||
}
|
||||
case urdf::Joint::PRISMATIC:{
|
||||
Vector axis = toKdl(jnt->axis);
|
||||
return Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, Joint::TransAxis);
|
||||
}
|
||||
default:{
|
||||
ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
|
||||
return Joint(jnt->name, Joint::None);
|
||||
}
|
||||
}
|
||||
return Joint();
|
||||
return KDL::Joint();
|
||||
}
|
||||
|
||||
// construct inertia
|
||||
RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
|
||||
KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
|
||||
{
|
||||
Frame origin = toKdl(i->origin);
|
||||
|
||||
// the mass is frame indipendent
|
||||
KDL::Frame origin = toKdl(i->origin);
|
||||
|
||||
// the mass is frame independent
|
||||
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,
|
||||
KDL::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);
|
||||
|
||||
KDL::RotationalInertia urdf_inertia =
|
||||
KDL::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);
|
||||
|
||||
KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
|
||||
origin.M * KDL::RigidBodyInertia(0, KDL::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::RotationalInertia kdl_inertia_wrt_com =
|
||||
kdl_inertia_wrt_com_workaround.getRotationalInertia();
|
||||
|
||||
return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
|
||||
|
||||
return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);
|
||||
}
|
||||
|
||||
|
||||
// recursive function to walk through tree
|
||||
bool addChildrenToTree(urdf::LinkConstSharedPtr root, Tree& tree)
|
||||
bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
|
||||
{
|
||||
std::vector<urdf::LinkSharedPtr > children = root->child_links;
|
||||
ROS_DEBUG("Link %s had %i children", root->name.c_str(), (int)children.size());
|
||||
std::vector<urdf::LinkSharedPtr> children = root->child_links;
|
||||
ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
|
||||
|
||||
// constructs the optional inertia
|
||||
RigidBodyInertia inert(0);
|
||||
if (root->inertial)
|
||||
KDL::RigidBodyInertia inert(0);
|
||||
if (root->inertial) {
|
||||
inert = toKdl(root->inertial);
|
||||
}
|
||||
|
||||
// constructs the kdl joint
|
||||
Joint jnt = toKdl(root->parent_joint);
|
||||
KDL::Joint jnt = toKdl(root->parent_joint);
|
||||
|
||||
// construct the kdl segment
|
||||
Segment sgm(root->name, jnt, toKdl(root->parent_joint->parent_to_joint_origin_transform), inert);
|
||||
KDL::Segment sgm(root->name, jnt, toKdl(
|
||||
root->parent_joint->parent_to_joint_origin_transform), inert);
|
||||
|
||||
// add segment to tree
|
||||
tree.addSegment(sgm, root->parent_joint->parent_link_name);
|
||||
|
||||
// recurslively add all children
|
||||
for (size_t i=0; i<children.size(); i++){
|
||||
if (!addChildrenToTree(children[i], tree))
|
||||
for (size_t i = 0; i < children.size(); i++) {
|
||||
if (!addChildrenToTree(children[i], tree)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool treeFromFile(const string& file, Tree& tree)
|
||||
bool treeFromFile(const std::string & file, KDL::Tree & tree)
|
||||
{
|
||||
TiXmlDocument urdf_xml;
|
||||
urdf_xml.LoadFile(file);
|
||||
return treeFromXml(&urdf_xml, tree);
|
||||
}
|
||||
|
||||
bool treeFromParam(const string& param, Tree& tree)
|
||||
bool treeFromParam(const std::string & param, KDL::Tree & tree)
|
||||
{
|
||||
urdf::Model robot_model;
|
||||
if (!robot_model.initParam(param)){
|
||||
|
|
@ -170,17 +175,17 @@ bool treeFromParam(const string& param, Tree& tree)
|
|||
return treeFromUrdfModel(robot_model, tree);
|
||||
}
|
||||
|
||||
bool treeFromString(const string& xml, Tree& tree)
|
||||
bool treeFromString(const std::string & xml, KDL::Tree & tree)
|
||||
{
|
||||
TiXmlDocument urdf_xml;
|
||||
urdf_xml.Parse(xml.c_str());
|
||||
return treeFromXml(&urdf_xml, tree);
|
||||
}
|
||||
|
||||
bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
|
||||
bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
|
||||
{
|
||||
urdf::Model robot_model;
|
||||
if (!robot_model.initXml(xml_doc)){
|
||||
if (!robot_model.initXml(xml_doc)) {
|
||||
ROS_ERROR("Could not generate robot model");
|
||||
return false;
|
||||
}
|
||||
|
|
@ -188,24 +193,29 @@ bool treeFromXml(TiXmlDocument *xml_doc, Tree& tree)
|
|||
}
|
||||
|
||||
|
||||
bool treeFromUrdfModel(const urdf::ModelInterface& robot_model, Tree& tree)
|
||||
bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
|
||||
{
|
||||
if (!robot_model.getRoot())
|
||||
if (!robot_model.getRoot()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
tree = Tree(robot_model.getRoot()->name);
|
||||
tree = KDL::Tree(robot_model.getRoot()->name);
|
||||
|
||||
// warn if root link has inertia. KDL does not support this
|
||||
if (robot_model.getRoot()->inertial)
|
||||
ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.", robot_model.getRoot()->name.c_str());
|
||||
if (robot_model.getRoot()->inertial) {
|
||||
ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not "
|
||||
"support a root link with an inertia. As a workaround, you can add an extra "
|
||||
"dummy link to your URDF.", robot_model.getRoot()->name.c_str());
|
||||
}
|
||||
|
||||
// add all children
|
||||
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
|
||||
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
|
||||
for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
|
||||
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} // namespace kdl_parser
|
||||
|
|
|
|||
|
|
@ -1,5 +1,42 @@
|
|||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2015 Open Source Robotics Foundation, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Author: Jackie Kay */
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include <string>
|
||||
#include <gtest/gtest.h>
|
||||
#include <kdl/jntarray.hpp>
|
||||
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
|
||||
|
|
@ -8,22 +45,18 @@
|
|||
#include <ros/console.h>
|
||||
#include "kdl_parser/kdl_parser.hpp"
|
||||
|
||||
using namespace kdl_parser;
|
||||
|
||||
int g_argc;
|
||||
char** g_argv;
|
||||
char ** g_argv;
|
||||
|
||||
class TestInertiaRPY : public testing::Test
|
||||
{
|
||||
public:
|
||||
|
||||
protected:
|
||||
/// constructor
|
||||
TestInertiaRPY()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/// Destructor
|
||||
~TestInertiaRPY()
|
||||
{
|
||||
|
|
@ -31,10 +64,7 @@ protected:
|
|||
};
|
||||
|
||||
|
||||
TEST_F(TestInertiaRPY, test_torques)
|
||||
{
|
||||
//ASSERT_EQ(g_argc, 3);
|
||||
|
||||
TEST_F(TestInertiaRPY, test_torques) {
|
||||
// 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;
|
||||
|
|
@ -42,7 +72,7 @@ TEST_F(TestInertiaRPY, test_torques)
|
|||
KDL::JntArray torques_2;
|
||||
|
||||
{
|
||||
ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1));
|
||||
ASSERT_TRUE(kdl_parser::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;
|
||||
|
|
@ -50,11 +80,9 @@ TEST_F(TestInertiaRPY, test_torques)
|
|||
|
||||
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);
|
||||
|
||||
|
|
@ -63,17 +91,15 @@ TEST_F(TestInertiaRPY, test_torques)
|
|||
}
|
||||
|
||||
{
|
||||
ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2));
|
||||
ASSERT_TRUE(kdl_parser::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);
|
||||
|
||||
|
|
@ -86,9 +112,7 @@ TEST_F(TestInertiaRPY, test_torques)
|
|||
SUCCEED();
|
||||
}
|
||||
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "test_kdl_parser");
|
||||
|
|
|
|||
|
|
@ -34,15 +34,12 @@
|
|||
|
||||
/* Author: Wim Meeussen */
|
||||
|
||||
#include <string>
|
||||
#include <gtest/gtest.h>
|
||||
#include <ros/ros.h>
|
||||
#include "kdl_parser/kdl_parser.hpp"
|
||||
|
||||
using namespace kdl_parser;
|
||||
|
||||
int g_argc;
|
||||
char** g_argv;
|
||||
char ** g_argv;
|
||||
|
||||
class TestParser : public testing::Test
|
||||
{
|
||||
|
|
@ -55,7 +52,6 @@ protected:
|
|||
{
|
||||
}
|
||||
|
||||
|
||||
/// Destructor
|
||||
~TestParser()
|
||||
{
|
||||
|
|
@ -63,28 +59,25 @@ protected:
|
|||
};
|
||||
|
||||
|
||||
TEST_F(TestParser, test)
|
||||
{
|
||||
for (int i=1; i<g_argc-2; i++){
|
||||
TEST_F(TestParser, test) {
|
||||
for (int i = 1; i < g_argc - 2; i++) {
|
||||
ROS_ERROR("Testing file %s", g_argv[i]);
|
||||
ASSERT_FALSE(treeFromFile(g_argv[i], my_tree));
|
||||
ASSERT_FALSE(kdl_parser::treeFromFile(g_argv[i], my_tree));
|
||||
}
|
||||
|
||||
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
|
||||
ASSERT_EQ(my_tree.getNrOfJoints(), 8);
|
||||
ASSERT_EQ(my_tree.getNrOfSegments(), 16);
|
||||
ASSERT_TRUE(kdl_parser::treeFromFile(g_argv[g_argc - 1], my_tree));
|
||||
ASSERT_EQ(8, my_tree.getNrOfJoints());
|
||||
ASSERT_EQ(16, my_tree.getNrOfSegments());
|
||||
ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment());
|
||||
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
|
||||
ASSERT_EQ((unsigned int)1, my_tree.getRootSegment()->second.children.size());
|
||||
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
|
||||
ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 10.0);
|
||||
ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001);
|
||||
ASSERT_EQ(10.0, my_tree.getSegment("base_link")->second.segment.getInertia().getMass());
|
||||
ASSERT_NEAR(1.000, my_tree.getSegment(
|
||||
"base_link")->second.segment.getInertia().getRotationalInertia().data[0], 0.001);
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "test_kdl_parser");
|
||||
|
|
|
|||
Loading…
Reference in New Issue