parser_urdf.hh

Go to the documentation of this file.

/*
 * Copyright 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifndef URDF2GAZEBO_HH
#define URDF2GAZEBO_HH

#include <urdf_model/model.h>
#include <urdf_model/link.h>
#include <tinyxml.h>

#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <vector>
#include <string>
#include <sstream>
#include <map>

#include "ode/mass.h"
#include "ode/rotation.h"

#include "gazebo/math/Pose.hh"
#include "gazebo/common/Console.hh"

namespace urdf2gazebo
{

  typedef boost::shared_ptr<urdf::Collision> UrdfCollisionPtr;
  typedef boost::shared_ptr<urdf::Visual> UrdfVisualPtr;
  typedef boost::shared_ptr<urdf::Link> UrdfLinkPtr;
  typedef boost::shared_ptr<const urdf::Link> ConstUrdfLinkPtr;

  class GazeboExtension
  {
    private: GazeboExtension()
    {
      material.clear();
      setStaticFlag = false;
      gravity = true;
      isDampingFactor = false;
      isMaxVel = false;
      isMinDepth = false;
      fdir1.clear();
      isMu1 = false;
      isMu2 = false;
      isKp = false;
      isKd = false;
      selfCollide = false;
      isLaserRetro = false;
      isStopCfm = false;
      isStopErp = false;
      isInitialJointPosition = false;
      isFudgeFactor = false;
      provideFeedback = false;
      blobs.clear();

      dampingFactor = 0;
      maxVel = 0;
      minDepth = 0;
      mu1 = 0;
      mu2 = 0;
      kp = 100000000;
      kd = 1;
      laserRetro = 101;
      stopCfm = 0;
      stopErp = 0.1;
      initialJointPosition = 0;
      fudgeFactor = 1;
    };

    private: GazeboExtension(const GazeboExtension &ge)
    {
      material = ge.material;
      setStaticFlag = ge.setStaticFlag;
      gravity = ge.gravity;
      isDampingFactor = ge.isDampingFactor;
      isMaxVel = ge.isMaxVel;
      isMinDepth = ge.isMinDepth;
      fdir1 = ge.fdir1;
      isMu1 = ge.isMu1;
      isMu2 = ge.isMu2;
      isKp = ge.isKp;
      isKd = ge.isKd;
      selfCollide = ge.selfCollide;
      isLaserRetro = ge.isLaserRetro;
      isStopCfm = ge.isStopCfm;
      isStopErp = ge.isStopErp;
      isInitialJointPosition = ge.isInitialJointPosition;
      isFudgeFactor = ge.isFudgeFactor;
      provideFeedback = ge.provideFeedback;
      oldLinkName = ge.oldLinkName;
      reductionTransform = ge.reductionTransform;
      blobs = ge.blobs;

      dampingFactor = ge.dampingFactor;
      maxVel = ge.maxVel;
      minDepth = ge.minDepth;
      mu1 = ge.mu1;
      mu2 = ge.mu2;
      kp = ge.kp;
      kd = ge.kd;
      laserRetro = ge.laserRetro;
      stopCfm = ge.stopCfm;
      stopErp = ge.stopErp;
      initialJointPosition = ge.initialJointPosition;
      fudgeFactor = ge.fudgeFactor;
    };

    // for reducing fixed joints and removing links
    private: std::string oldLinkName;
    private: gazebo::math::Pose reductionTransform;

    // visual
    private: std::string material;

    // body, default off
    private: bool setStaticFlag;
    private: bool gravity;
    private: bool isDampingFactor;
    private: double dampingFactor;
    private: bool isMaxVel;
    private: double maxVel;
    private: bool isMinDepth;
    private: double minDepth;
    private: bool selfCollide;

    // geom, contact dynamics
    private: bool isMu1, isMu2, isKp, isKd;
    private: double mu1, mu2, kp, kd;
    private: std::string fdir1;
    private: bool isLaserRetro;
    private: double laserRetro;

    // joint, joint limit dynamics
    private: bool isStopCfm, isStopErp, isInitialJointPosition, isFudgeFactor;
    private: double stopCfm, stopErp, initialJointPosition, fudgeFactor;
    private: bool provideFeedback;

    // blobs into body or robot
    private: std::vector<TiXmlElement*> blobs;

    friend class URDF2Gazebo;
  };

  class URDF2Gazebo
  {
    public: URDF2Gazebo();

    public: ~URDF2Gazebo();

    public: TiXmlDocument InitModelDoc(TiXmlDocument* _xmlDoc);

    public: TiXmlDocument InitModelFile(const std::string &_filename);

    public: TiXmlDocument InitModelString(const std::string &_urdfStr,
                                          bool _enforceLimits = true);

    private: urdf::Vector3 ParseVector3(TiXmlNode* _key, double _scale = 1.0);

    private: std::string Values2str(unsigned int _count, const double *_values);

    private: std::string Vector32Str(const urdf::Vector3 _vector);

    private: void AddKeyValue(TiXmlElement *_elem, const std::string &_key,
                     const std::string &_value);

    private: void AddTransform(TiXmlElement *_elem,
                      const::gazebo::math::Pose& _transform);

    private: void PrintMass(UrdfLinkPtr _link);

    private: void PrintMass(const std::string &_linkName, dMass _mass);

    private: std::string GetKeyValueAsString(TiXmlElement* _elem);

    private: void ParseGazeboExtension(TiXmlDocument &_urdfXml);

    private: void InsertGazeboExtensionCollision(TiXmlElement *_elem,
                                        const std::string &_linkName);

    private: void InsertGazeboExtensionVisual(TiXmlElement *_elem,
                                     const std::string &_linkName);

    private: void InsertGazeboExtensionLink(TiXmlElement *_elem,
                                   const std::string &_linkName);

    private: void InsertGazeboExtensionJoint(TiXmlElement *_elem,
                                    const std::string &_jointName);

    private: void InsertGazeboExtensionRobot(TiXmlElement *_elem);

    private: void ListGazeboExtensions();

    private: void ListGazeboExtensions(const std::string &_reference);

    // collision elements of the child link into the parent link
    private: void ReduceFixedJoints(TiXmlElement *_root, UrdfLinkPtr _link);

    private: void ReduceInertialToParent(UrdfLinkPtr _link);

    private: void ReduceVisualsToParent(UrdfLinkPtr _link);

    private: void ReduceCollisionsToParent(UrdfLinkPtr _link);

    private: void ReduceJointsToParent(UrdfLinkPtr _link);

    private: void ReduceVisualToParent(UrdfLinkPtr _link,
                              const std::string &_groupName,
                              UrdfVisualPtr _visual);

    private: void ReduceCollisionToParent(UrdfLinkPtr _link,
                         const std::string &_groupName,
                         UrdfCollisionPtr _collision);

    private: void ReduceGazeboExtensionToParent(UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionFrameReplace(GazeboExtension* _ge,
                                           UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionPluginFrameReplace(
      std::vector<TiXmlElement*>::iterator _blobIt,
      UrdfLinkPtr _link, const std::string &_pluginName,
      const std::string &_elementName, gazebo::math::Pose _reductionTransform);

    private: void ReduceGazeboExtensionProjectorFrameReplace(
      std::vector<TiXmlElement*>::iterator _blobIt,
      UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionGripperFrameReplace(
      std::vector<TiXmlElement*>::iterator _blobIt,
       UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionJointFrameReplace(
      std::vector<TiXmlElement*>::iterator _blobIt,
       UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionContactSensorFrameReplace(
      std::vector<TiXmlElement*>::iterator _blobIt,
       UrdfLinkPtr _link);

    private: void ReduceGazeboExtensionsTransform(GazeboExtension* _ge);

    private: void ReduceGazeboExtensionSensorTransformReduction(
      std::vector<TiXmlElement*>::iterator _blobIt,
      gazebo::math::Pose _reductionTransform);

    private: void ReduceGazeboExtensionProjectorTransformReduction(
      std::vector<TiXmlElement*>::iterator _blobIt,
      gazebo::math::Pose _reductionTransform);

    private: urdf::Pose  TransformToParentFrame(
      urdf::Pose _transformInLinkFrame,
      urdf::Pose _parentToLinkTransform);
    private: gazebo::math::Pose  TransformToParentFrame(
      gazebo::math::Pose _transformInLinkFrame,
      urdf::Pose _parentToLinkTransform);
    private: gazebo::math::Pose  TransformToParentFrame(
      gazebo::math::Pose _transformInLinkFrame,
      gazebo::math::Pose _parentToLinkTransform);
    private: gazebo::math::Pose  inverseTransformToParentFrame(
      gazebo::math::Pose _transformInLinkFrame,
      urdf::Pose _parentToLinkTransform);
    private: gazebo::math::Pose  CopyPose(urdf::Pose _pose);
    private: urdf::Pose  CopyPose(gazebo::math::Pose _pose);

    private: std::string GetGeometryBoundingBox(
      boost::shared_ptr<urdf::Geometry> _geometry, double *_sizeVals);


    private: void PrintCollisionGroups(UrdfLinkPtr _link);

    private: void CreateSDF(TiXmlElement *_root,
      ConstUrdfLinkPtr _link,
      const gazebo::math::Pose &_transform);

    private: void CreateLink(TiXmlElement *_root,
      ConstUrdfLinkPtr _link,
      gazebo::math::Pose &_currentTransform);

    private: void CreateCollisions(TiXmlElement* _elem,
      ConstUrdfLinkPtr _link);

    private: void CreateVisuals(TiXmlElement* _elem,
      ConstUrdfLinkPtr _link);

    private: void CreateInertial(TiXmlElement *_elem,
      ConstUrdfLinkPtr _link);

    private: void CreateCollision(TiXmlElement* _elem,
      ConstUrdfLinkPtr _link,
      UrdfCollisionPtr _collision,
      const std::string &_oldLinkName = std::string(""));

    private: void CreateVisual(TiXmlElement *_elem,
      ConstUrdfLinkPtr _link,
      UrdfVisualPtr _visual,
      const std::string &_oldLinkName = std::string(""));

    private: void CreateJoint(TiXmlElement *_root,
      ConstUrdfLinkPtr _link,
      gazebo::math::Pose &_currentTransform);

    private: void CreateGeometry(TiXmlElement* _elem,
      boost::shared_ptr<urdf::Geometry> _geometry);

    private: std::map<std::string, std::vector<GazeboExtension*> > extensions;

    private: bool enforceLimits;
    private: bool reduceFixedJoints;
  };
}

#endif