diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 76c8c7f..232de10 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -27,8 +27,11 @@ rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp) rosbuild_add_boost_directories() #rosbuild_link_boost(${PROJECT_NAME} thread) -rosbuild_add_executable(urdf_check src/urdf_check.cpp) -target_link_libraries(urdf_check ${PROJECT_NAME}) +rosbuild_add_executable(check_urdf src/check_urdf.cpp) +target_link_libraries(check_urdf ${PROJECT_NAME}) + +rosbuild_add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) +target_link_libraries(urdf_to_graphiz ${PROJECT_NAME}) rosbuild_add_executable(mem_test test/memtest.cpp) target_link_libraries(mem_test ${PROJECT_NAME}) diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index c54ba52..cf7160b 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -99,11 +99,6 @@ private: /// it's time to build a tree bool initTree(std::map &parent_link_tree); - /// in initXml(), onece tree is built, - /// it's time to find the root Link - bool initRoot(std::map &parent_link_tree); - - /// Model is restricted to a tree for now, which means there exists one root link /// typically, root link is the world(inertial). Where world is a special link /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint? diff --git a/urdf/src/urdf_check.cpp b/urdf/src/check_urdf.cpp similarity index 100% rename from urdf/src/urdf_check.cpp rename to urdf/src/check_urdf.cpp diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index af4b30e..d22305b 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -238,126 +238,94 @@ bool Model::initXml(TiXmlElement *robot_xml) return false; } - // find the root link - if (!this->initRoot(parent_link_tree)) - { - ROS_ERROR("failed to find root link"); - return false; - } - return true; } bool Model::initTree(std::map &parent_link_tree) { + this->root_link_.reset(); + // loop through all joints, for every link, assign children links and children joints for (std::map >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) { std::string parent_link_name = joint->second->parent_link_name; std::string child_link_name = joint->second->child_link_name; - ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); + ROS_DEBUG(" Build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); - /// deal with deprecated case - if (parent_link_name == "world" && !this->getLink(parent_link_name)){ - ROS_WARN("Joint %s specifies the world link as its parent, but there in no link called world. This used to be allowed, but this behavior has been deprecated. You need to explicitly specify each link in your tree.", joint->first.c_str()); - boost::shared_ptr link; - link.reset(new Link); - link->name = "world"; - this->links_.insert(make_pair(link->name,link)); - ROS_DEBUG(" successfully added new link '%s'", link->name.c_str()); + // case where joint is not connected to any link + if (parent_link_name.empty() && !child_link_name.empty()) + { + ROS_ERROR(" Joint %s specifies only parent link but not child link",(joint->second)->name.c_str()); + return false; + } + else if (!parent_link_name.empty() && child_link_name.empty()) + { + ROS_ERROR(" Joint %s specifies only child link but not parent link",(joint->second)->name.c_str()); + return false; + } + else if (parent_link_name.empty() && child_link_name.empty()) + { + ROS_DEBUG(" Joint %s has not parent or child link, it is an abstract joint.",(joint->second)->name.c_str()); } - if (parent_link_name.empty()) - { - ROS_DEBUG(" Joint %s: does not have parent link name specified. Joint is an abstract joint.",(joint->second)->name.c_str()); - } - else if (child_link_name.empty()) - { - ROS_DEBUG(" Joint %s: does not have child link name specified. Joint is an abstract joint.",(joint->second)->name.c_str()); - } + // normal joint case else { - // find parent link - boost::shared_ptr parent_link; + // find child and parent link of joint + boost::shared_ptr child_link, parent_link; + this->getLink(child_link_name, child_link); this->getLink(parent_link_name, parent_link); - - if (!parent_link) + if (!child_link) { - ROS_ERROR(" parent link '%s' of joint '%s' not found", parent_link_name.c_str(), joint->first.c_str() ); + ROS_ERROR(" Child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str()); return false; } - else + if (!parent_link) { - // find child link - boost::shared_ptr child_link; - this->getLink(child_link_name, child_link); - - if (!child_link) + if (root_link_) { - ROS_ERROR(" child link '%s' of joint: %s not found",child_link_name.c_str(),joint->first.c_str()); - return false; - } - else - { - //set parent link for child link - child_link->setParent(parent_link); - - //set parent joint for child link - child_link->setParentJoint(joint->second); - - //set child joint for parent link - parent_link->addChildJoint(joint->second); - - //set child link for parent link - parent_link->addChild(child_link); - - // fill in child/parent string map - parent_link_tree[child_link->name] = parent_link_name; - - ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); + ROS_ERROR("This tree contains two root links"); + return false; } + ROS_DEBUG(" Parent link '%s' of joint '%s' not found. This should be the root link.", parent_link_name.c_str(), joint->first.c_str() ); + parent_link.reset(new Link); + parent_link->name = parent_link_name; + root_link_ = parent_link; + this->links_.insert(make_pair(parent_link_name, parent_link)); } + + //set parent link for child link + child_link->setParent(parent_link); + + //set parent joint for child link + child_link->setParentJoint(joint->second); + + //set child joint for parent link + parent_link->addChildJoint(joint->second); + + //set child link for parent link + parent_link->addChild(child_link); + + // fill in child/parent string map + parent_link_tree[child_link->name] = parent_link_name; + + ROS_DEBUG(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); } } - return true; -} - - - -bool Model::initRoot(std::map &parent_link_tree) -{ - - this->root_link_.reset(); - - for (std::map::iterator p=parent_link_tree.begin(); p!=parent_link_tree.end(); p++) - { - if (parent_link_tree.find(p->second) == parent_link_tree.end()) - { - if (this->root_link_) - { - ROS_DEBUG("child '%s', parent '%s', root '%s'", p->first.c_str(), p->second.c_str(), this->root_link_->name.c_str()); - if (this->root_link_->name != p->second) - { - ROS_ERROR("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), p->second.c_str()); - return false; - } - } - else - getLink(p->second,this->root_link_); - } - } - if (!this->root_link_) - { - ROS_ERROR("No root link found. The robot xml is not a valid tree."); + // we should have found root + if (!root_link_){ + ROS_ERROR("The tree does not contain a root link"); return false; } - ROS_DEBUG("Link '%s' is the root link", this->root_link_->name.c_str()); return true; } + + + boost::shared_ptr Model::getMaterial(const std::string& name) const { boost::shared_ptr ptr; diff --git a/urdf/src/urdf_to_graphiz.cpp b/urdf/src/urdf_to_graphiz.cpp new file mode 100644 index 0000000..3541128 --- /dev/null +++ b/urdf/src/urdf_to_graphiz.cpp @@ -0,0 +1,117 @@ +/********************************************************************* +* 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: +* +* * Redstributions 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 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 +* 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: Wim Meeussen */ + +#include "urdf/model.h" +#include +#include + +using namespace urdf; +using namespace std; + +void addChildLinkNames(boost::shared_ptr link, ofstream& os) +{ + os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) + addChildLinkNames(*child, os); +} + +void addChildJointNames(boost::shared_ptr link, ofstream& os) +{ + double r, p, y; + for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ + (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); + os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name + << "\" [label=\"xyz: " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " + << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " + << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl; + os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl; + addChildJointNames(*child, os); + } +} + + +void printTree(boost::shared_ptr link, string file) +{ + std::ofstream os; + os.open(file.c_str()); + os << "digraph G {" << endl; + + os << "node [shape=box];" << endl; + addChildLinkNames(link, os); + + os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl; + addChildJointNames(link, os); + + os << "}" << endl; + os.close(); +} + + + +int main(int argc, char** argv) +{ + if (argc != 2){ + std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl; + return -1; + } + + TiXmlDocument robot_model_xml; + robot_model_xml.LoadFile(argv[1]); + TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot"); + if (!robot_xml){ + std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl; + return -1; + } + + Model robot; + if (!robot.initXml(robot_xml)){ + std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + return -1; + } + string output = robot.getName(); + + // print entire tree to file + printTree(robot.getRoot(), output+".gv"); + cout << "Created file " << output << ".gv" << endl; + + string command = "dot -Tpdf "+output+".gv -o "+output+".pdf"; + system(command.c_str()); + cout << "Created file " << output << ".pdf" << endl; + return 0; +} + diff --git a/urdf/test/pr2_desc_explicit_world.xml b/urdf/test/pr2_desc_explicit_world.xml new file mode 100644 index 0000000..ff3db28 --- /dev/null +++ b/urdf/test/pr2_desc_explicit_world.xml @@ -0,0 +1,3448 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + + true + 1.0 + 5 + + power_state + 10.0 + 87.78 + -474 + 525 + 15.52 + 16.41 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + + 0.0 0.0 0.0 + false + + -80 + 80 + + 0.05 + 10.0 + 0.01 + 20.0 + + 0.005 + true + 20.0 + base_scan + base_laser_link + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 79.2380952381 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -79.2380952381 + + + + true + + base_link_geom + 100.0 + + true + 100.0 + base_bumper + + + + + + + true + 100.0 + base_link + base_pose_ground_truth + 0.01 + map + 25.7 25.7 0 + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + torso_lift_link_geom + 100.0 + + true + 100.0 + torso_lift_bumper + + + + + + + + -52143.33 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + R8G8B8 + 2448 2050 + 45 + 0.1 + 100 + 20.0 + + true + 20.0 + /prosilica/cam_info + /prosilica/image + /prosilica/image_rect + /prosilica/cam_info_service + /prosilica/poll + hight_def_optical_frame + 1224.5 + 1224.5 + 1025.5 + 2955 + 0 + 0 + 0 + 0 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + B8G8R8 + 90.0 + 0.1 + 100 + 25.0 + + true + 25.0 + wide_stereo/left/image_raw + wide_stereo/left/camera_info + wide_stereo_optical_frame + 0.0 + 320.5 + 320.5 + 240.5 + + + 320.0 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + B8G8R8 + 90.0 + 0.1 + 100 + 25.0 + + true + 25.0 + wide_stereo/right/image_raw + wide_stereo/right/camera_info + wide_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 320.0 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + + + + + true + 20.0 + wide_stereo_l_stereo_camera_sensor + wide_stereo_r_stereo_camera_sensor + wide_stereo/raw_stereo + wide_stereo_optical_frame + 320.5 + 320.5 + 240.5 + + + 320.0 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45.0 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/left/image_raw + narrow_stereo/left/camera_info + narrow_stereo_optical_frame + 0.0 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 45.0 + 0.1 + 100 + 25.0 + + true + 25.0 + narrow_stereo/right/image_raw + narrow_stereo/right/camera_info + narrow_stereo_optical_frame + 0.09 + 320.5 + 320.5 + 240.5 + + + 772.55 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + + + + + true + 20.0 + narrow_stereo_l_stereo_camera_sensor + narrow_stereo_r_stereo_camera_sensor + narrow_stereo/raw_stereo + narrow_stereo_optical_frame + 320.5 + 320.5 + 240.5 + + + 772.55 + 0 + 0 + 0 + 0 + 0 + -0.09 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 + 640 + 1 + + 0.0 0.0 0.0 + false + + -80 + 80 + + 0.05 + 10.0 + 0.01 + 40.0 + + 0.005 + true + 40.0 + tilt_scan + laser_tilt_link + + + + + + + + + -6.05 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_shoulder_pan_link_geom + 100.0 + + true + 100.0 + r_shoulder_pan_bumper + + + + + true + + + + + + r_shoulder_lift_link_geom + 100.0 + + true + 100.0 + r_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + r_upper_arm_link_geom + 100.0 + + true + 100.0 + r_upper_arm_bumper + + + + true + + + -1.0 + + + r_elbow_flex_link_geom + 100.0 + + true + 100.0 + r_elbow_flex_bumper + + + + + true + + + + + + true + + + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_forearm_link_geom + 100.0 + + true + 100.0 + r_forearm_bumper + + + + + + true + + r_wrist_flex_link_geom + 100.0 + + true + 100.0 + r_wrist_flex_bumper + + + + + + + + + true + + r_wrist_roll_link_geom + 100.0 + + true + 100.0 + r_wrist_roll_bumper + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + r_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + r_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + false + + r_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + false + + r_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + r_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + r_gripper_l_finger_link + r_gripper_l_finger_force_ground_truth + r_gripper_l_finger_link + + + + + + + + + + + + + true + + r_gripper_palm_link_geom + 100.0 + + true + 100.0 + r_gripper_palm_bumper + + + + + + true + + + + + true + 0.01 + 0.0001 + 0 + 0 + 0.0001 + 0 + 0.0001 + 0 + 0 + 0 + 0.82025 0.188 0.790675 + 0 -0 0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + unit_box + PR2/White + + + true + false + + + r_gripper_float_link + r_gripper_palm_link + r_gripper_float_link + 1 0 0 + -0.05 + 0.001 + + + r_gripper_l_finger_tip_link + r_gripper_float_link + r_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + r_gripper_r_finger_tip_link + r_gripper_float_link + r_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + r_gripper_palm_link + r_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + r_gripper_tool_frame + r_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_shoulder_pan_link_geom + 100.0 + + true + 100.0 + l_shoulder_pan_bumper + + + + + true + + + + + + l_shoulder_lift_link_geom + 100.0 + + true + 100.0 + l_r_shoulder_lift_bumper + + + + true + + + + + + true + + + + + + + + 63.16 + + + + 61.89 + + + + 32.65 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + l_upper_arm_link_geom + 100.0 + + true + 100.0 + l_upper_arm_bumper + + + + true + + + -1.0 + + + l_elbow_flex_link_geom + 100.0 + + true + 100.0 + l_elbow_flex_bumper + + + + + true + + + + + + true + + + + + + -36.17 + + + + 90.5142857143 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_forearm_link_geom + 100.0 + + true + 100.0 + l_forearm_bumper + + + + + + true + + l_wrist_flex_link_geom + 100.0 + + true + 100.0 + l_wrist_flex_bumper + + + + + + + + + true + + l_wrist_roll_link_geom + 100.0 + + true + 100.0 + l_wrist_roll_bumper + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + l_gripper_l_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_bumper + + + + + + + + + + + + + + + + true + + l_gripper_r_finger_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_bumper + + + + + + + + + + + + + + + true + false + + l_gripper_l_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_l_finger_tip_bumper + + + + + + + + + + + + + + + true + false + + l_gripper_r_finger_tip_link_geom + 100.0 + + true + 100.0 + l_gripper_r_finger_tip_bumper + + + + + + + + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_pose_ground_truth + 0.0 + map + + + + true + 100.0 + l_gripper_l_finger_link + l_gripper_l_finger_force_ground_truth + l_gripper_l_finger_link + + + + + + + + + + + + + true + + l_gripper_palm_link_geom + 100.0 + + true + 100.0 + l_gripper_palm_bumper + + + + + + true + + + + + true + 0.01 + 0.0001 + 0 + 0 + 0.0001 + 0 + 0.0001 + 0 + 0 + 0 + 0.82025 0.188 0.790675 + 0 -0 0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + + 0 0 0 + 0 -0 0 + 0.0 0.0 0.0 + unit_box + PR2/White + + + true + false + + + l_gripper_float_link + l_gripper_palm_link + l_gripper_float_link + 1 0 0 + -0.05 + 0.001 + + + l_gripper_l_finger_tip_link + l_gripper_float_link + l_gripper_l_finger_tip_link + 0 1 0 + 0 0 0 + + + l_gripper_r_finger_tip_link + l_gripper_float_link + l_gripper_r_finger_tip_link + 0 1 0 + 0 0 0 + + + + + true + 100.0 + l_gripper_palm_link + l_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + true + 100.0 + l_gripper_tool_frame + l_gripper_tool_frame_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + /map + + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90.0 + 0.1 + 100 + 25.0 + + true + 25.0 + l_forearm_cam/image_raw + l_forearm_cam/camera_info + l_forearm_cam_frame + 0.0 + 320.5 + 320.5 + 240.5 + + + 320.0 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 640 480 + L8 + 90.0 + 0.1 + 100 + 25.0 + + true + 25.0 + r_forearm_cam/image_raw + r_forearm_cam/camera_info + r_forearm_cam_frame + 0.0 + 320.5 + 320.5 + 240.5 + + + 320.0 + 0 + 0 + 0 + 0 + 0 + + + + true + PR2/Blue + + true + + + diff --git a/urdf/test/test_robot_model_parser.launch b/urdf/test/test_robot_model_parser.launch index 66d8df1..582529a 100644 --- a/urdf/test/test_robot_model_parser.launch +++ b/urdf/test/test_robot_model_parser.launch @@ -1,3 +1,3 @@ - +