collada exporter using soft limits if they exist (thanks to Kei Okada)

collada_urdf/src/collada_urdf.cpp

@@ -77,10 +77,12 @@ public:
     ResourceIOStream(const resource_retriever::MemoryResource& res)
         : res_(res)
         , pos_(res.data.get())
-    {}
+    {
+    }
 
     ~ResourceIOStream()
-    {}
+    {
+    }
 
     size_t Read(void* buffer, size_t size, size_t count)
     {
@@ -96,7 +98,9 @@ public:
         return to_read;
     }
 
-    size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
+    size_t Write( const void* buffer, size_t size, size_t count) {
+        ROS_BREAK(); return 0;
+    }
 
     aiReturn Seek( size_t offset, aiOrigin origin)
     {
@@ -135,7 +139,8 @@ public:
         return res_.size;
     }
 
-    void Flush() {}
+    void Flush() {
+    }
 
 private:
     resource_retriever::MemoryResource res_;
@@ -195,7 +200,9 @@ public:
         return new ResourceIOStream(res);
     }
 
-    void Close(Assimp::IOStream* stream) { delete stream; }
+    void Close(Assimp::IOStream* stream) {
+        delete stream;
+    }
 
 private:
     mutable resource_retriever::Retriever retriever_;
@@ -229,7 +236,8 @@ private:
         struct axis_output
         {
             //axis_output(const string& sid, KinBody::JointConstPtr pjoint, int iaxis) : sid(sid), pjoint(pjoint), iaxis(iaxis) {}
-        axis_output() : iaxis(0) {}
+            axis_output() : iaxis(0) {
+            }
             string sid, nodesid;
             boost::shared_ptr<const urdf::Joint> pjoint;
             int iaxis;
@@ -242,7 +250,8 @@ private:
 
     struct axis_sids
     {
-    axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {}
+        axis_sids(const string& axissid, const string& valuesid, const string& jointnodesid) : axissid(axissid), valuesid(valuesid), jointnodesid(jointnodesid) {
+        }
         string axissid, valuesid, jointnodesid;
     };
 
@@ -270,7 +279,8 @@ public:
         _collada->setDatabase(NULL);
         _importer.SetIOHandler(new ResourceIOSystem());
     }
-    virtual ~ColladaWriter() {}
+    virtual ~ColladaWriter() {
+    }
 
     boost::shared_ptr<DAE> convert()
     {
@@ -350,8 +360,12 @@ public:
     }
 
 protected:
-    virtual void handleError(daeString msg) { throw ColladaUrdfException(msg); }
+    virtual void handleError(daeString msg) {
-    virtual void handleWarning(daeString msg) { std::cerr << "COLLADA DOM warning: " << msg << std::endl; }
+        throw ColladaUrdfException(msg);
+    }
+    virtual void handleWarning(daeString msg) {
+        std::cerr << "COLLADA DOM warning: " << msg << std::endl;
+    }
 
     void _CreateScene()
     {
@@ -440,6 +454,10 @@ protected:
                     flower = pjoint->limits->lower;
                     fupper = pjoint->limits->upper;
                 }
+                if( !!pjoint->safety ) {
+                    flower = pjoint->safety->soft_lower_limit;
+                    fupper = pjoint->safety->soft_upper_limit;
+                }
                 if( flower == fupper ) {
                     bactive = false;
                 }
@@ -544,11 +562,14 @@ protected:
             if( !!it->pjoint->limits ) {
                 flower = it->pjoint->limits->lower;
                 fupper = it->pjoint->limits->upper;
+            }
+            if( !!it->pjoint->safety ) {
+                flower = it->pjoint->safety->soft_lower_limit;
+                fupper = it->pjoint->safety->soft_upper_limit;
+            }
             if( flower > 0 || fupper < 0 ) {
                 value = 0.5*(flower+fupper);
             }
-            }
-
             domKinematics_newparamRef pvalueparam = daeSafeCast<domKinematics_newparam>(_ikmout->ikm->add(COLLADA_ELEMENT_NEWPARAM));
             pvalueparam->setSid((sid+string("_value")).c_str());
             daeSafeCast<domKinematics_newparam::domFloat>(pvalueparam->add(COLLADA_ELEMENT_FLOAT))->setValue(value);
@@ -605,7 +626,11 @@ protected:
             pdomjoint->setSid(jointid.c_str() );
             pdomjoint->setName(pjoint->name.c_str());
             domAxis_constraintRef axis;
-            if( !!pjoint->limits ) {
+            if( !!pjoint->safety ) {
+                lmin=pjoint->safety->soft_lower_limit;
+                lmax=pjoint->safety->soft_upper_limit;
+            }
+            else if( !!pjoint->limits ) {
                 lmin=pjoint->limits->lower;
                 lmax=pjoint->limits->upper;
             }