From 7c50adc588a3ebeccffb70735a16f46ce7996058 Mon Sep 17 00:00:00 2001
From: YoheiKakiuchi <youhei@jsk.imi.i.u-tokyo.ac.jp>
Date: Sat, 5 Jul 2014 12:39:23 +0900
Subject: [PATCH] collada_parser: add extract actuators, fix for using nominal
 torque

---
 collada_parser/src/collada_parser.cpp | 46 ++++++++++++++++++++++++++-
 1 file changed, 45 insertions(+), 1 deletion(-)

diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp
index 6560abe..95d4a75 100644
--- a/collada_parser/src/collada_parser.cpp
+++ b/collada_parser/src/collada_parser.cpp
@@ -599,6 +599,7 @@ protected:
             }
         }
 
+        _ExtractRobotAttachedActuators(articulated_system);
         _ExtractRobotManipulators(articulated_system);
         _ExtractRobotAttachedSensors(articulated_system);
         return true;
@@ -1997,6 +1998,43 @@ protected:
         return false;
     }
 
+    /// \brief extract the robot actuators
+    void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
+    {
+        // over write joint parameters by elements in instance_actuator
+        for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
+            domExtraRef pextra = as->getExtra_array()[ie];
+            if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
+                //std::string aname = pextra->getAttribute("name");
+                domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
+                if( !!tec ) {
+                    boost::shared_ptr<Joint> pjoint;
+                    daeElementRef domactuator;
+                    {
+                        daeElementRef bact = tec->getChild("bind_actuator");
+                        pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
+                        if (!pjoint) continue;
+                    }
+                    {
+                        daeElementRef iact = tec->getChild("instance_actuator");
+                        if(!iact) continue;
+                        std::string instance_url = iact->getAttribute("url");
+                        domactuator = daeURI(*iact, instance_url).getElement();
+                        if(!domactuator) continue;
+                    }
+                    daeElement *nom_torque = domactuator->getChild("nominal_torque");
+                    if ( !! nom_torque ) {
+                        if( !! pjoint->limits ) {
+                            pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
+                            ROS_DEBUG("effort limit at joint (%s) is over written by %f",
+                                      pjoint->name.c_str(), pjoint->limits->effort);
+                        }
+                    }
+                }
+            }
+        }
+    }
+
     /// \brief extract the robot manipulators
     void _ExtractRobotManipulators(const domArticulated_systemRef as)
     {
@@ -2379,7 +2417,13 @@ protected:
             return boost::shared_ptr<Joint>();
         }
 
-        boost::shared_ptr<Joint> pjoint = _model->joints_[std::string(pdomjoint->getName())];
+        boost::shared_ptr<Joint> pjoint;
+        std::string name(pdomjoint->getName());
+        if (_model->joints_.find(name) == _model->joints_.end()) {
+            pjoint.reset();
+        } else {
+            pjoint = _model->joints_.find(name)->second;
+        }
         if(!pjoint) {
             ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
         }