Merge pull request #415 from snozawa/add_load_dump_offset_file

Add load dump offset file
fkanehiro · Dec 5, 2014 · 4a09c5d · 4a09c5d
2 parents 969d433 + 267e13b
commit 4a09c5d
Show file tree

Hide file tree

Showing 7 changed files with 103 additions and 5 deletions.
diff --git a/idl/RemoveForceSensorLinkOffsetService.idl b/idl/RemoveForceSensorLinkOffsetService.idl
@@ -35,5 +35,19 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean getForceMomentOffsetParam(in string name, out forcemomentOffsetParam i_param);
+
+    /**
+     * @brief loadForceMomentOffsetParams from file
+     * @param filename is full-path input file name.
+     * @return true if load successfully, false otherwise
+     */
+    boolean loadForceMomentOffsetParams(in string filename);
+
+    /**
+     * @brief dumpForceMomentOffsetParams from file
+     * @param filename is full-path output file name.
+     * @return true if dump successfully, false otherwise
+     */
+    boolean dumpForceMomentOffsetParams(in string filename);
   };
 };
diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp
@@ -224,6 +224,14 @@ void RemoveForceSensorLinkOffset::updateRootLinkPosRot (const hrp::Vector3& rpy)
   }
 }
 
+void RemoveForceSensorLinkOffset::printForceMomentOffsetParam(const std::string& i_name_)
+{
+  std::cerr << "[" << m_profile.instance_name << "]   force_offset = " << m_forcemoment_offset_param[i_name_].force_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   moment_offset = " << m_forcemoment_offset_param[i_name_].moment_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   link_offset_centroid = " << m_forcemoment_offset_param[i_name_].link_offset_centroid.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
+  std::cerr << "[" << m_profile.instance_name << "]   link_offset_mass = " << m_forcemoment_offset_param[i_name_].link_offset_mass << "[kg]" << std::endl;
+};
+
 bool RemoveForceSensorLinkOffset::setForceMomentOffsetParam(const std::string& i_name_, const RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_)
 {
   std::cerr << "[" << m_profile.instance_name << "] setForceMomentOffsetParam [" << i_name_ << "]" << std::endl;
@@ -232,10 +240,7 @@ bool RemoveForceSensorLinkOffset::setForceMomentOffsetParam(const std::string& i
     memcpy(m_forcemoment_offset_param[i_name_].moment_offset.data(), i_param_.moment_offset.get_buffer(), sizeof(double) * 3);
     memcpy(m_forcemoment_offset_param[i_name_].link_offset_centroid.data(), i_param_.link_offset_centroid.get_buffer(), sizeof(double) * 3);
     m_forcemoment_offset_param[i_name_].link_offset_mass = i_param_.link_offset_mass;
-    std::cerr << "[" << m_profile.instance_name << "]   force_offset = " << m_forcemoment_offset_param[i_name_].force_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
-    std::cerr << "[" << m_profile.instance_name << "]   moment_offset = " << m_forcemoment_offset_param[i_name_].moment_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
-    std::cerr << "[" << m_profile.instance_name << "]   link_offset_centroid = " << m_forcemoment_offset_param[i_name_].link_offset_centroid.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
-    std::cerr << "[" << m_profile.instance_name << "]   link_offset_mass = " << m_forcemoment_offset_param[i_name_].link_offset_mass << "[kg]" << std::endl;
+    printForceMomentOffsetParam(i_name_);
   } else {
     std::cerr << "[" << m_profile.instance_name << "]   No such limb"<< std::endl;
   }
@@ -256,6 +261,55 @@ bool RemoveForceSensorLinkOffset::getForceMomentOffsetParam(const std::string& i
   return true;
 }
 
+bool RemoveForceSensorLinkOffset::loadForceMomentOffsetParams(const std::string& filename)
+{
+  std::cerr << "[" << m_profile.instance_name << "] loadForceMomentOffsetParams" << std::endl;
+  std::ifstream ifs(filename.c_str());
+  if (ifs.is_open()){
+    while(ifs.eof()==0){
+      std::string tmps;
+      ForceMomentOffsetParam tmpp;
+      if ( ifs >> tmps ) {
+          if ( m_forcemoment_offset_param.find(tmps) != m_forcemoment_offset_param.end()) {
+              for (size_t i = 0; i < 3; i++) ifs >> tmpp.force_offset(i);
+              for (size_t i = 0; i < 3; i++) ifs >> tmpp.moment_offset(i);
+              for (size_t i = 0; i < 3; i++) ifs >> tmpp.link_offset_centroid(i);
+              ifs >> tmpp.link_offset_mass;
+              m_forcemoment_offset_param[tmps] = tmpp;
+              std::cerr << "[" << m_profile.instance_name << "]   " << tmps << "" << std::endl;
+              printForceMomentOffsetParam(tmps);
+          } else {
+              std::cerr << "[" << m_profile.instance_name << "] no such (" << tmps << ")" << std::endl;
+              return false;
+          }
+      }
+    }
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] failed to open(" << filename << ")" << std::endl;
+    return false;
+  }
+  return true;
+};
+
+bool RemoveForceSensorLinkOffset::dumpForceMomentOffsetParams(const std::string& filename)
+{
+  std::cerr << "[" << m_profile.instance_name << "] dumpForceMomentOffsetParams" << std::endl;
+  std::ofstream ofs(filename.c_str());
+  if (ofs.is_open()){
+    for ( std::map<std::string, ForceMomentOffsetParam>::iterator it = m_forcemoment_offset_param.begin(); it != m_forcemoment_offset_param.end(); it++ ) {
+      ofs << it->first << " ";
+      ofs << it->second.force_offset[0] << " " << it->second.force_offset[1] << " " << it->second.force_offset[2] << " ";
+      ofs << it->second.moment_offset[0] << " " << it->second.moment_offset[1] << " " << it->second.moment_offset[2] << " ";
+      ofs << it->second.link_offset_centroid[0] << " " << it->second.link_offset_centroid[1] << " " << it->second.link_offset_centroid[2] << " ";
+      ofs << it->second.link_offset_mass << std::endl;
+    }
+  } else {
+    std::cerr << "[" << m_profile.instance_name << "] failed to open(" << filename << ")" << std::endl;
+    return false;
+  }
+  return true;
+};
+
 /*
 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onAborting(RTC::UniqueId ec_id)
 {

diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h
@@ -103,6 +103,8 @@ class RemoveForceSensorLinkOffset
   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
   bool setForceMomentOffsetParam(const std::string& i_name_, const OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam &i_param_);
   bool getForceMomentOffsetParam(const std::string& i_name_, OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_);
+  bool loadForceMomentOffsetParams(const std::string& filename);
+  bool dumpForceMomentOffsetParams(const std::string& filename);
 
  protected:
   // Configuration variable declaration
@@ -152,7 +154,6 @@ class RemoveForceSensorLinkOffset
   // </rtc-template>
 
  private:
-  void updateRootLinkPosRot (const hrp::Vector3& rpy);
   struct ForceMomentOffsetParam {
     hrp::Vector3 force_offset, moment_offset, link_offset_centroid;
     double link_offset_mass;
@@ -162,6 +163,9 @@ class RemoveForceSensorLinkOffset
         link_offset_centroid(hrp::Vector3::Zero()), link_offset_mass(0)
     {};
   };
+  void updateRootLinkPosRot (const hrp::Vector3& rpy);
+  void printForceMomentOffsetParam(const std::string& i_name_);
+
   std::map<std::string, ForceMomentOffsetParam> m_forcemoment_offset_param;
   static const double grav = 9.80665; /* [m/s^2] */
   double m_dt;

diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt
@@ -27,6 +27,12 @@ If the robot has imu sensors, this component uses estimated attitude
 value from "rpy" input data port. 
 Otherwise, this component assumes that the robot's rpy equals zero. 
 
+\subsection parameterfile Parameter File
+
+This component can load parameters from a file through OpenHRP::RemoveForceSensorLinkOffsetService::loadForceMomentOffsetParams().
+This component can also dump parameter through OpenHRP::RemoveForceSensorLinkOffsetService::dumpForceMomentOffsetParams(). 
+
+Each line of file should be 11 components, "sensorname force_offset_x force_offset_y force_offset_z moment_offset_x moment_offset_y moment_offset_z link_offset_centroid_x link_offset_centroid_y link_offset_centroid_z link_offset_mass".
 
 <table>
 <tr><th>implementation_id</th><td>RemoveForceSensorLinkOffset</td></tr>

diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp
@@ -25,6 +25,16 @@ CORBA::Boolean RemoveForceSensorLinkOffsetService_impl::getForceMomentOffsetPara
   return m_rmfsoff->getForceMomentOffsetParam(std::string(i_name_), *i_param_);
 }
 
+CORBA::Boolean RemoveForceSensorLinkOffsetService_impl::loadForceMomentOffsetParams(const char *filename)
+{
+	return m_rmfsoff->loadForceMomentOffsetParams(std::string(filename));
+};
+
+CORBA::Boolean RemoveForceSensorLinkOffsetService_impl::dumpForceMomentOffsetParams(const char *filename)
+{
+	return m_rmfsoff->dumpForceMomentOffsetParams(std::string(filename));
+};
+
 void RemoveForceSensorLinkOffsetService_impl::rmfsoff(RemoveForceSensorLinkOffset *i_rmfsoff)
 {
 	m_rmfsoff = i_rmfsoff;

diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h
@@ -18,6 +18,8 @@ class RemoveForceSensorLinkOffsetService_impl
   //
   CORBA::Boolean setForceMomentOffsetParam(const char* i_name_, const OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam &i_param_);
   CORBA::Boolean getForceMomentOffsetParam(const char *i_name_, OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam_out i_param_);
+  CORBA::Boolean loadForceMomentOffsetParams(const char *fiename);
+  CORBA::Boolean dumpForceMomentOffsetParams(const char *fiename);
   //
   void rmfsoff(RemoveForceSensorLinkOffset *i_rmfsoff);
 private:

diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py.in b/sample/SampleRobot/samplerobot_remove_force_offset.py.in
@@ -46,3 +46,11 @@ def demo():
     print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm < 1e-2
     fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data)
     print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm < 1e-2
+
+    # 4. dump and load parameter file
+    ret=hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
+    if ret:
+        print "dumpForceMomentOffsetParams => OK"
+    ret=hcf.rmfo_svc.loadForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
+    if ret:
+        print "loadForceMomentOffsetParams => OK"