Merge pull request #848 from snozawa/add_interpolator_name

Add name for interpolator and set name for RTCs using interpolator.
fkanehiro · Oct 9, 2015 · d94de96 · d94de96
2 parents 9b6061e + f0c3a06
commit d94de96
Show file tree

Hide file tree

Showing 7 changed files with 26 additions and 4 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -222,13 +222,17 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
     }
 
     zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
+    zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
     zmp_transition_time = 1.0;
     transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
     transition_interpolator_ratio = 1.0;
     adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
     transition_time = 2.0;
     adjust_footstep_transition_time = 2.0;
     leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
+    leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
     leg_names_interpolator_ratio = 1.0;
 
     // setting stride limitations from conf file

diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
@@ -238,6 +238,7 @@ namespace rats
           zmp_weight_map = boost::assign::map_list_of<leg_type, double>(RLEG, zmp_weight_initial_value[0])(LLEG, zmp_weight_initial_value[1])(RARM, zmp_weight_initial_value[2])(LARM, zmp_weight_initial_value[3]);
           zmp_weight_interpolator = boost::shared_ptr<interpolator>(new interpolator(4, dt));
           zmp_weight_interpolator->set(zmp_weight_initial_value); /* set initial value */
+          zmp_weight_interpolator->setName("GaitGenerator zmp_weight_interpolator");
       };
       ~refzmp_generator()
       {
@@ -623,6 +624,9 @@ namespace rats
         if (swing_foot_rot_ratio_interpolator == NULL) swing_foot_rot_ratio_interpolator = new interpolator(1, dt);
         //if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, dt, interpolator::LINEAR);
         if (toe_heel_interpolator == NULL) toe_heel_interpolator = new interpolator(1, dt);
+        foot_ratio_interpolator->setName("GaitGenerator foot_ratio_interpolator");
+        swing_foot_rot_ratio_interpolator->setName("GaitGenerator swing_foot_rot_ratio_interpolator");
+        toe_heel_interpolator->setName("GaitGenerator toe_heel_interpolator");
       };
       ~leg_coords_generator()
       {

diff --git a/rtc/CollisionDetector/CollisionDetector.cpp b/rtc/CollisionDetector/CollisionDetector.cpp
@@ -255,6 +255,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize()
     m_recover_jointdata = new double[m_robot->numJoints()];
     m_lastsafe_jointdata = new double[m_robot->numJoints()];
     m_interpolator = new interpolator(m_robot->numJoints(), i_dt);
+    m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
     m_link_collision = new bool[m_robot->numLinks()];
 
     for(int i=0; i<m_robot->numJoints(); i++){

diff --git a/rtc/EmergencyStopper/EmergencyStopper.cpp b/rtc/EmergencyStopper/EmergencyStopper.cpp
@@ -125,6 +125,7 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize()
     //default_retrieve_time = 1.0/m_dt;
     m_stop_posture = new double[m_robot->numJoints()];
     m_interpolator = new interpolator(m_robot->numJoints(), recover_time_dt);
+    m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator");
 
     m_q.data.length(m_robot->numJoints());
     for(int i=0; i<m_robot->numJoints(); i++){

diff --git a/rtc/SequencePlayer/interpolator.cpp b/rtc/SequencePlayer/interpolator.cpp
@@ -216,7 +216,7 @@ void interpolator::load(const char *fname, double time_to_start, double scale,
 {
   ifstream strm(fname);
   if (!strm.is_open()) {
-    cerr << "file not found(" << fname << ")" << endl;
+    cerr << "[interpolator " << name << "] file not found(" << fname << ")" << endl;
     return;
   }
   double *vs, ptime=-1,time, tmp;
@@ -353,17 +353,17 @@ void interpolator::get(double *x_, double *v_, double *a_, bool popp)
   if (length!=0){
     double *&vs = q.front();
     if (vs == NULL) {
-      cerr << "interpolator::get vs = NULL, q.size() = " << q.size() 
+      cerr << "[interpolator " << name << "] interpolator::get vs = NULL, q.size() = " << q.size() 
 	   << ", length = " << length << endl;
     }
     double *&dvs = dq.front();
     if (dvs == NULL) {
-      cerr << "interpolator::get dvs = NULL, dq.size() = " << dq.size() 
+      cerr << "[interpolator " << name << "] interpolator::get dvs = NULL, dq.size() = " << dq.size() 
 	   << ", length = " << length << endl;
     }
     double *&ddvs = ddq.front();
     if (ddvs == NULL) {
-      cerr << "interpolator::get ddvs = NULL, ddq.size() = " << ddq.size() 
+      cerr << "[interpolator " << name << "] interpolator::get ddvs = NULL, ddq.size() = " << ddq.size() 
 	   << ", length = " << length << endl;
     }
     memcpy(x_, vs, sizeof(double)*dim);

diff --git a/rtc/SequencePlayer/interpolator.h b/rtc/SequencePlayer/interpolator.h
@@ -59,6 +59,7 @@ class interpolator
   void interpolate(double& remain_t_);
   double deltaT() const { return dt; }
   double dimension() const { return dim; }
+  void setName (const std::string& _name) { name = _name; };
 private:
   // Current interpolation mode
   interpolation_mode imode;
@@ -81,6 +82,8 @@ class interpolator
   double *a0, *a1, *a2, *a3, *a4, *a5;
   // Default average velocity for calc_interpolation_time
   double default_avg_vel;
+  // Interpolator name
+  std::string name;
 
   void hoffarbib(double &remain_t_,
 		 double a0, double a1, double a2,

diff --git a/rtc/SequencePlayer/seqplay.cpp b/rtc/SequencePlayer/seqplay.cpp
@@ -16,6 +16,15 @@ seqplay::seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum, unsigned
     interpolators[TQ] = new interpolator(i_dof, i_dt);
     interpolators[WRENCHES] = new interpolator(6 * i_fnum, i_dt, interpolator::HOFFARBIB, 100); // wrenches = 6 * [number of force sensors]
 	interpolators[OPTIONAL_DATA] = new interpolator(optional_data_dim, i_dt);
+    // Set interpolator name
+    interpolators[Q]->setName("Q");
+    interpolators[ZMP]->setName("ZMP");
+    interpolators[ACC]->setName("ACC");
+    interpolators[P]->setName("P");
+    interpolators[RPY]->setName("RPY");
+    interpolators[TQ]->setName("TQ");
+    interpolators[WRENCHES]->setName("WRENCHES");
+    interpolators[OPTIONAL_DATA]->setName("OPTIONAL_DATA");
     //
 
 #ifdef WAIST_HEIGHT