Go to the documentation of this file.00001 #include <robodyn_controllers/KdlTreeUtilities.h>
00002
00003 KdlTreeUtilities::KdlTreeUtilities()
00004 {
00005 }
00006
00007 KdlTreeUtilities::~KdlTreeUtilities()
00008 {
00009 }
00010
00011 void KdlTreeUtilities::getChainJointNames(const KDL::Chain& chain,
00012 std::vector<std::string>& jointNames) const
00013 {
00014 jointNames.resize(chain.getNrOfJoints());
00015 const std::vector<KDL::Segment>& segments = chain.segments;
00016 std::vector<std::string>::iterator nameIt = jointNames.begin();
00017
00018 for (unsigned int i = 0; i < segments.size(); ++i)
00019 {
00020 const KDL::Joint& joint = segments[i].getJoint();
00021
00022 if (joint.getType() != KDL::Joint::None)
00023 {
00024
00025 *nameIt = joint.getName();
00026 ++nameIt;
00027 }
00028 }
00029 }
00030
00031 void KdlTreeUtilities::getChainJointNames(const std::string& toolFrame,
00032 std::vector<std::string>& jointNames) const
00033 {
00034 getChainJointNames(getBaseName(), toolFrame, jointNames);
00035 }
00036
00037 void KdlTreeUtilities::getChainJointNames(const std::string& baseFrame, const std::string& toolFrame,
00038 std::vector<std::string>& jointNames) const
00039 {
00040 KDL::Chain chain;
00041
00042 if (!getChain(baseFrame, toolFrame, chain))
00043 {
00044 jointNames.clear();
00045 return;
00046 }
00047
00048 getChainJointNames(chain, jointNames);
00049 }
00050
00051 void KdlTreeUtilities::getJointNames(std::vector<std::string>& jointNames) const
00052 {
00053 jointNames.resize(tree.getNrOfJoints());
00054 const KDL::SegmentMap& segments = tree.getSegments();
00055
00056 for (KDL::SegmentMap::const_iterator segIt = segments.begin(); segIt != segments.end(); ++segIt)
00057 {
00058 const KDL::TreeElement& element = segIt->second;
00059 const KDL::Joint& joint = element.segment.getJoint();
00060
00061 if (joint.getType() != KDL::Joint::None)
00062 {
00063
00064 jointNames[element.q_nr] = joint.getName();
00065 }
00066 }
00067 }
00068
00069
00070 bool KdlTreeUtilities::getChain(const std::string& baseFrame, const std::string& toolFrame, KDL::Chain& chain) const
00071 {
00072 if (baseFrame == toolFrame)
00073 {
00074 chain = KDL::Chain();
00075 return false;
00076 }
00077
00078 return (tree.getChain(baseFrame, toolFrame, chain));
00079 }
00086 void KdlTreeUtilities::addSegment(const KDL::Segment& segment, const std::string& hookName )
00087 {
00088 if (hasSegment(segment.getName()))
00089 {
00090 std::stringstream err;
00091 err << "KdlTreeUtilities::addSegment could not add " << segment.getName() << ". A segment with that name already exists in the tree.";
00092 NasaCommonLogging::Logger::log("gov.nasa.controllers.KdlTreeUtilities", log4cpp::Priority::ERROR, err.str());
00093 throw std::runtime_error(err.str());
00094 }
00095
00096 if (!hasSegment(hookName))
00097 {
00098 std::stringstream err;
00099 err << "KdlTreeUtilities::addSegment could not add " << segment.getName() << " to " << hookName << "--" << hookName << " does not exist in the tree.";
00100 NasaCommonLogging::Logger::log("gov.nasa.controllers.KdlTreeUtilities", log4cpp::Priority::ERROR, err.str());
00101 throw std::runtime_error(err.str());
00102 }
00103
00104 tree.addSegment(segment, hookName);
00105 }
00106
00111 bool KdlTreeUtilities::removeSegment(const std::string& segName)
00112 {
00113 KDL::Tree t(tree.getRootSegment()->first);
00114 bool success = this->removeSegRecursive(t, tree.getRootSegment(), tree.getRootSegment()->first, segName);
00115 tree = t;
00116 return success;
00117 }
00118
00119 bool KdlTreeUtilities::removeSegRecursive(KDL::Tree& tree, KDL::SegmentMap::const_iterator root, const std::string& hook_name, const std::string& seg_remove_name)
00120 {
00121
00122 KDL::SegmentMap::const_iterator child;
00123
00124
00125 for (unsigned int i = 0; i < root->second.children.size(); i++)
00126 {
00127 child = root->second.children[i];
00128
00129 if (child->second.segment.getName() != seg_remove_name)
00130 {
00131
00132 if (tree.addSegment(child->second.segment, hook_name))
00133 {
00134
00135 if (!(this->removeSegRecursive(tree, child, child->first, seg_remove_name)))
00136
00137 {
00138 return false;
00139 }
00140 }
00141 else
00142 {
00143 std::stringstream err;
00144 err << "KdlTreeUtilities::removeSegRecursive could not add child" << child->second.segment.getName();
00145 NasaCommonLogging::Logger::log("gov.nasa.controllers.KdlTreeUtilities", log4cpp::Priority::ERROR, err.str());
00146 throw std::runtime_error(err.str());
00147 }
00148 }
00149 }
00150
00151 return true;
00152 }