hrl_kinematics: hrl_kinematics: Kinematics.cpp Source File

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/hrl_kinematics/src/Kinematics.cpp $
00002 // SVN $Id: Kinematics.cpp 3069 2012-08-21 14:26:59Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * hrl_kinematics - a kinematics library for humanoid robots based on KDL
00006  *
00007  * Copyright 2011-2012 Armin Hornung, University of Freiburg
00008  * License: BSD
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions are met:
00012  *
00013  *     * Redistributions of source code must retain the above copyright
00014  *       notice, this list of conditions and the following disclaimer.
00015  *     * Redistributions in binary form must reproduce the above copyright
00016  *       notice, this list of conditions and the following disclaimer in the
00017  *       documentation and/or other materials provided with the distribution.
00018  *     * Neither the name of the University of Freiburg nor the names of its
00019  *       contributors may be used to endorse or promote products derived from
00020  *       this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <hrl_kinematics/Kinematics.h>
00036 
00037 using robot_state_publisher::SegmentPair;
00038 
00039 namespace hrl_kinematics {
00040 
00041 Kinematics::Kinematics()
00042 : nh_(), nh_private_ ("~"),
00043   root_link_name_("base_link"), rfoot_link_name_("r_sole"),  lfoot_link_name_("l_sole")
00044 {
00045   // Get URDF XML
00046   std::string urdf_xml, full_urdf_xml;
00047   nh_private_.param("robot_description_name",urdf_xml,std::string("robot_description"));
00048   nh_.searchParam(urdf_xml,full_urdf_xml);
00049 
00050   ROS_DEBUG("Reading xml file from parameter server");
00051   std::string result;
00052 
00053 
00054   if (!nh_.getParam(full_urdf_xml, result))
00055     throw Kinematics::InitFailed("Could not load the xml from parameter server: " + urdf_xml);
00056 
00057 
00058   // Load and Read Models
00059   if (!loadModel(result))
00060     throw Kinematics::InitFailed("Could not load models!");
00061 
00062   ROS_INFO("Kinematics initialized");
00063 
00064 }
00065 
00066 Kinematics::~Kinematics() {
00067 
00068 }
00069 
00070 bool Kinematics::loadModel(const std::string xml) {
00071 
00072   if (!urdf_model_.initString(xml)) {
00073     ROS_FATAL("Could not initialize robot model");
00074     return -1;
00075   }
00076 
00077 
00078   if (!kdl_parser::treeFromUrdfModel(urdf_model_, kdl_tree_)) {
00079     ROS_ERROR("Could not initialize tree object");
00080     return false;
00081   }
00082 
00083 
00084   // walk the tree and add segments to segments_
00085   addChildren(kdl_tree_.getRootSegment());
00086 
00087 
00088   if (!(kdl_tree_.getChain(root_link_name_, rfoot_link_name_, kdl_chain_right_)
00089       && kdl_tree_.getChain(root_link_name_, lfoot_link_name_, kdl_chain_left_))) {
00090     ROS_ERROR("Could not initialize leg chain objects");
00091     return false;
00092   }
00093 
00094   return true;
00095 }
00096 
00097 
00098 
00099 // add children to correct maps
00100 void Kinematics::addChildren(const KDL::SegmentMap::const_iterator segment)
00101 {
00102   const std::string& root = segment->second.segment.getName();
00103 
00104   const std::vector<KDL::SegmentMap::const_iterator>& children = segment->second.children;
00105   for (unsigned int i=0; i<children.size(); i++){
00106     const KDL::Segment& child = children[i]->second.segment;
00107     SegmentPair s(children[i]->second.segment, root, child.getName());
00108     if (child.getJoint().getType() == KDL::Joint::None){
00109       // skip over fixed:
00110       //      segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
00111       ROS_DEBUG("Tree initialization: Skipping fixed segment from %s to %s", root.c_str(), child.getName().c_str());
00112     }
00113     else{
00114       segments_.insert(make_pair(child.getJoint().getName(), s));
00115       ROS_DEBUG("Tree initialization: Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
00116     }
00117     addChildren(children[i]);
00118   }
00119 }
00120 
00121 
00122 void Kinematics::createCoGMarker(const std::string& ns, const std::string& frame_id, double radius, const KDL::Vector& cog, visualization_msgs::Marker& marker) const{
00123   marker.header.frame_id = frame_id;
00124   marker.ns = ns;
00125   marker.type = visualization_msgs::Marker::SPHERE;
00126   marker.action = visualization_msgs::Marker::ADD;
00127   marker.pose.position.x = cog.x();
00128   marker.pose.position.y = cog.y();
00129   marker.pose.position.z = cog.z();
00130   marker.scale.x = radius;
00131   marker.scale.y = radius;
00132   marker.scale.z = radius;
00133   marker.color.r = 1.0;
00134   marker.color.a = 0.8;
00135 }
00136 
00137 void Kinematics::computeCOMRecurs(const KDL::SegmentMap::const_iterator& current_seg, const std::map<std::string, double>& joint_positions,
00138                                   const KDL::Frame& tf, KDL::Frame& tf_right_foot, KDL::Frame& tf_left_foot, double& m, KDL::Vector& com) {
00139 
00140   double jnt_p = 0.0;
00141 
00142   if (current_seg->second.segment.getJoint().getType() != KDL::Joint::None){
00143     std::map<std::string, double>::const_iterator jnt = joint_positions.find(current_seg->second.segment.getJoint().getName());
00144 
00145     if (jnt == joint_positions.end()){
00146       ROS_WARN("Could not find joint %s of %s in joint positions. Aborting tree branch.", current_seg->second.segment.getJoint().getName().c_str(), current_seg->first.c_str());
00147       return;
00148     }
00149     jnt_p = jnt->second;
00150   }
00151 
00152   KDL::Frame current_frame = tf * current_seg->second.segment.pose(jnt_p);
00153   if (current_seg->first == lfoot_link_name_){
00154     tf_left_foot = current_frame;
00155     ROS_DEBUG("Right foot tip transform found");
00156   } else if (current_seg->first == rfoot_link_name_){
00157     tf_right_foot = current_frame;
00158     ROS_DEBUG("Left foot tip transform found");
00159   }
00160 
00161 
00162   KDL::Vector current_cog = current_seg->second.segment.getInertia().getCOG();
00163   double current_m = current_seg->second.segment.getInertia().getMass();
00164 
00165 
00166   com = com + current_m * (current_frame*current_cog);
00167 
00168   m += current_m;
00169   ROS_DEBUG("At link %s. local: %f / [%f %f %f]; global: %f / [%f %f %f]",current_seg->first.c_str(), current_m, current_cog.x(), current_cog.y(), current_cog.z(),
00170             m, com.x(), com.y(), com.z());
00171 
00172   // TODO: separate recursive fct to create markers, callable on demand
00173 //  if (current_m > 0.0){
00174 //    visualization_msgs::Marker marker;
00175 //    createCoGMarker(current_seg->first, "torso", 0.02, (current_frame*current_cog), marker);
00176 //    com_vis_markers_.markers.push_back(marker);
00177 //  }
00178 
00179   std::vector<KDL::SegmentMap::const_iterator >::const_iterator child_it;
00180   for (child_it = current_seg->second.children.begin(); child_it !=current_seg->second.children.end(); ++child_it){
00181     computeCOMRecurs(*child_it, joint_positions, current_frame, tf_right_foot, tf_left_foot, m, com);
00182   }
00183 
00184 }
00185 
00186 void Kinematics::computeCOM(const std::map<std::string, double>& joint_positions, tf::Point& COM, double& mass,
00187                             tf::Transform& tf_right_foot, tf::Transform& tf_left_foot){
00188   mass = 0.0;
00189   KDL::Vector com;
00190   KDL::Frame ident = KDL::Frame::Identity();
00191   KDL::Frame transform = ident;
00192   KDL::Frame right_foot_tf = ident;
00193   KDL::Frame left_foot_tf = ident;
00194 
00195   computeCOMRecurs(kdl_tree_.getRootSegment(), joint_positions, transform, right_foot_tf, left_foot_tf, mass, com);
00196   if (left_foot_tf == ident || right_foot_tf == ident){
00197     ROS_ERROR("Could not obtain left or right foot transforms");
00198     return;
00199   }
00200 
00201   if (mass <= 0.0){
00202     ROS_WARN("Total mass is 0, no CoM possible.");
00203     COM.setValue(0,0,0);
00204     return;
00205   }
00206 
00207   com = 1.0/mass * com;
00208   ROS_DEBUG("Total mass: %f CoG: (%f %f %f)", mass, com.x(), com.y(), com.z());
00209 
00210   COM.setValue(com.x(), com.y(), com.z());
00211   tf::TransformKDLToTF(right_foot_tf, tf_right_foot);
00212   tf::TransformKDLToTF(left_foot_tf, tf_left_foot);
00213 }
00214 
00215 
00216 
00217 } /* namespace hrl_kinematics */