gazebo: gazebo: urdf2gazebo.h Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #ifndef URDF2GAZEBO_HH
00035 #define URDF2GAZEBO_HH
00036 
00037 #include <cstdio>
00038 #include <cstdlib>
00039 #include <cmath>
00040 
00041 #include <vector>
00042 #include <string>
00043 
00044 #include <sstream>
00045 #include <map>
00046 #include <vector>
00047 
00048 #include <urdf/model.h>
00049 #include <urdf_interface/link.h>
00050 
00051 #include "LinearMath/btTransform.h"
00052 #include "LinearMath/btVector3.h"
00053 
00054 #include "ode/mass.h"
00055 #include "ode/rotation.h"
00056 
00057 namespace urdf2gazebo
00058 {
00059   double rad2deg(double v) { return v * 180.0 / M_PI; };
00060 
00061   class GazeboExtension
00062   {
00063     public:
00064       // HACK: add for now, since we are reducing fixed joints and removing links
00065       std::string original_reference;
00066       urdf::Pose reduction_transform;
00067 
00068       // visual
00069       std::string material;
00070 
00071       // body, default off
00072       bool setStaticFlag;
00073       bool turnGravityOff;
00074       bool is_dampingFactor;
00075       double dampingFactor;
00076       bool is_maxVel;
00077       double maxVel;
00078       bool is_minDepth;
00079       double minDepth;
00080       bool selfCollide;
00081 
00082       // geom, contact dynamics
00083       bool is_mu1, is_mu2, is_kp, is_kd, is_genTexCoord;
00084       double mu1, mu2, kp, kd;
00085       std::string fdir1;
00086       bool genTexCoord;
00087       bool is_laserRetro;
00088       double laserRetro;
00089 
00090       // joint, joint limit dynamics
00091       bool is_stopKp, is_stopKd, is_initial_joint_position, is_fudgeFactor;
00092       double stopKp, stopKd, initial_joint_position, fudgeFactor;
00093       bool provideFeedback;
00094 
00095       // blobs into body or robot
00096       std::vector<TiXmlElement*> blobs;
00097 
00098       GazeboExtension()
00099       {
00100         //initialize
00101         material.clear();
00102         setStaticFlag = false;
00103         turnGravityOff = false;
00104         is_dampingFactor = false;
00105         is_maxVel = false;
00106         is_minDepth = false;
00107         is_mu1 = false;
00108         fdir1.clear();
00109         is_mu2 = false;
00110         is_kp = false;
00111         is_kd = false;
00112         is_genTexCoord = false;
00113         selfCollide = false;
00114         is_laserRetro = false;
00115         is_stopKp = false;
00116         is_stopKd = false;
00117         is_initial_joint_position = false;
00118         is_fudgeFactor = false;
00119         provideFeedback = false;
00120         blobs.clear();
00121       };
00122 
00123       GazeboExtension(const GazeboExtension &ge)
00124       {
00125         //initialize
00126         material = ge.material;
00127         setStaticFlag = ge.setStaticFlag;
00128         turnGravityOff = ge.turnGravityOff;
00129         is_dampingFactor = ge.is_dampingFactor;
00130         is_maxVel = ge.is_maxVel;
00131         is_minDepth = ge.is_minDepth;
00132         is_mu1 = ge.is_mu1;
00133         is_mu2 = ge.is_mu2;
00134         is_kp = ge.is_kp;
00135         is_kd = ge.is_kd;
00136         is_genTexCoord = ge.is_genTexCoord;
00137         selfCollide = ge.selfCollide;
00138         is_laserRetro = ge.is_laserRetro;
00139         is_stopKp = ge.is_stopKp;
00140         is_stopKd = ge.is_stopKd;
00141         is_initial_joint_position = ge.is_initial_joint_position;
00142         is_fudgeFactor = ge.is_fudgeFactor;
00143         provideFeedback = ge.provideFeedback;
00144         original_reference = ge.original_reference;
00145         reduction_transform = ge.reduction_transform;
00146         blobs = ge.blobs;
00147       };
00148   };
00149 
00150 
00151   class URDF2Gazebo
00152   {
00153     public:
00154       URDF2Gazebo();
00155       ~URDF2Gazebo();
00156 
00157       std::string values2str(unsigned int count, const double *values, double (*conv)(double));
00158       std::string vector32str(const urdf::Vector3 vector, double (*conv)(double));
00159 
00160       void setupTransform(btTransform &transform, urdf::Pose pose);
00161       void setupTransform(urdf::Pose &pose, btTransform transform);
00162 
00163       void addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value);
00164 
00165       void addTransform(TiXmlElement *elem, const::btTransform& transform);
00166 
00167       void printMass(boost::shared_ptr<urdf::Link> link);
00168       void printMass(std::string link_name, dMass mass);
00169 
00170       std::string getGazeboValue(TiXmlElement* elem);
00171       void parseGazeboExtension(TiXmlDocument &urdf_in);
00172       void insertGazeboExtensionGeom(TiXmlElement *elem,std::string link_name);
00173       void insertGazeboExtensionVisual(TiXmlElement *elem,std::string link_name);
00174       void insertGazeboExtensionBody(TiXmlElement *elem,std::string link_name);
00175       void insertGazeboExtensionJoint(TiXmlElement *elem,std::string joint_name);
00176       void insertGazeboExtensionRobot(TiXmlElement *elem);
00177       void listGazeboExtensions();
00178       void listGazeboExtensions(std::string reference);
00179       void reduceGazeboExtensionToParent(boost::shared_ptr<urdf::Link> link);
00180       void updateGazeboExtensionFrameReplace(std::vector<TiXmlElement*> &blocks, std::string link_name, std::string new_link_name);
00181       void updateGazeboExtensionBlobsReductionTransform(GazeboExtension* ext);
00182 
00183       urdf::Pose  transformToParentFrame(urdf::Pose transform_in_link_frame, urdf::Pose parent_to_link_transform);
00184 
00185       std::string getGeometrySize(boost::shared_ptr<urdf::Geometry> geometry, int *sizeCount, double *sizeVals);
00186       
00187       std::string getGeometryBoundingBox(boost::shared_ptr<urdf::Geometry> geometry, double *sizeVals);
00188 
00189 #ifdef ROBOT_MODEL_MULTI_VISUAL_COLLISION
00190       void reduceFixedJoints(TiXmlElement *root, boost::shared_ptr<urdf::Link> link);
00191       void printCollisionGroups(boost::shared_ptr<urdf::Link> link);
00192 #endif
00193       void convertLink(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, const btTransform &transform,
00194                        bool enforce_limits,bool reduce_fixed_joints);
00195       void createBody(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, const btTransform &transform,
00196                       btTransform &currentTransform, bool enforce_limits, bool reduce_fixed_joints);
00197       void createGeom(TiXmlElement *root, TiXmlElement* elem, boost::shared_ptr<const urdf::Link> link,
00198                       const btTransform &transform, btTransform &currentTransform,std::string collision_type,
00199                       boost::shared_ptr<urdf::Collision> collision, boost::shared_ptr<urdf::Visual> visual,
00200                       int linkGeomSize, double linkSize[3], std::string original_reference = std::string(""));
00201       void createVisual(TiXmlElement *root, TiXmlElement *geom, boost::shared_ptr<const urdf::Link> link,
00202                       const btTransform &transform, btTransform &currentTransform,std::string collision_type,
00203                       boost::shared_ptr<urdf::Collision> collision, boost::shared_ptr<urdf::Visual> visual,
00204                       std::string original_reference);
00205       void createJoint(TiXmlElement *root, boost::shared_ptr<const urdf::Link> link, btTransform &currentTransform,
00206                        bool enforce_limits,bool reduce_fixed_joints);
00207 
00208       void convert( TiXmlDocument &urdf_in, TiXmlDocument &gazebo_xml_out, bool enforce_limits, 
00209                     urdf::Vector3 initial_xyz, urdf::Vector3 initial_rpy,std::string model_name=std::string(),
00210                     std::string robot_namespace=std::string(), bool xml_declaration = false);
00211 
00212       void walkChildAddNamespace(TiXmlNode* robot_xml,std::string robot_namespace);
00213 
00214       std::map<std::string, std::vector<GazeboExtension*> > gazebo_extensions_;
00215 
00216   };
00217 }
00218 
00219 #endif