[idl/AutoBalancerService.idl, AutoBalancer.*, AutoBalancerService_imp…

…l.*, GaitGenerator.h] add setFootStepNodes for multiple legs
fkanehiro · Aug 24, 2015 · 4fd2ec1 · 4fd2ec1
1 parent 0f19613
commit 4fd2ec1
Show file tree

Hide file tree

Showing 6 changed files with 159 additions and 4 deletions.
diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl
@@ -27,6 +27,15 @@ module OpenHRP
       string leg;
     };
 
+    /**
+     * @struct Footsteps
+     * @brief Foot step for multi legs.
+     */
+    struct Footsteps
+    {
+      sequence<Footstep> fs;
+    };
+
     /**
      * @struct StepParam
      * @brief Step parameter for one step
@@ -43,13 +52,29 @@ module OpenHRP
       double heel_angle;
     };
 
+    /**
+     * @struct StepParams
+     * @brief Step parameters for multi step
+     */
+    struct StepParams
+    {
+      sequence<StepParam> sps;
+    };
+
     /**
      * @struct FootstepSequence
      * @brief Sequence of foot step.
      */
     typedef sequence<Footstep> FootstepSequence;
     typedef sequence<StepParam> StepParamSequence;
 
+    /**
+     * @struct FootstepsSequence
+     * @brief Sequence of foot steps.
+     */
+    typedef sequence<Footsteps> FootstepsSequence;
+    typedef sequence<StepParams> StepParamsSequence;
+
     /**
      * @enum SupportLegState
      * @brief State of support leg.
@@ -223,6 +248,7 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean setFootSteps(in FootstepSequence fs, in long overwrite_fs_idx);
+    boolean setFootSteps2(in FootstepsSequence fss, in long overwrite_fs_idx);
 
     /**
      * @brief Set footsteps. Returns without waiting for whole steps to be executed.
@@ -231,6 +257,7 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean setFootStepsWithParam(in FootstepSequence fs, in StepParamSequence sps, in long overwrite_fs_idx);
+    boolean setFootStepsWithParam2(in FootstepsSequence fss, in StepParamsSequence spss, in long overwrite_fs_idx);
 
     /**
      * @brief Wait for whole footsteps are executed.

diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -1025,22 +1025,42 @@ bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequ
   setFootStepsWithParam(fs, sps, overwrite_fs_idx);
 }
 
+bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  OpenHRP::AutoBalancerService::StepParamsSequence spss;
+  spss.length(fss.length());
+  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
+  // If gg_is_walking is true, do not set to 0.
+  for (size_t i = 0; i < spss.length(); i++) {
+      spss[i].sps.length(fss[i].fs.length());
+      for (size_t j = 0; j < spss[i].sps.length(); j++) {
+          spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].step_time = gg->get_default_step_time();
+          spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+          spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
+      }
+  }
+  setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
 bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
 {
   if (!is_stop_mode) {
-    std::cerr << "[" << m_profile.instance_name << "] setFootSteps" << std::endl;
+    std::cerr << "[" << m_profile.instance_name << "] setFootStepList" << std::endl;
 
     // Initial footstep Snapping
     coordinates tmpfs, initial_support_coords, initial_input_coords, fstrans;
     if (gg_is_walking) {
+        std::vector<step_node> initial_support_steps;
         if (overwrite_fs_idx <= 0) {
             std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
             return false;
         }
-        if (!gg->get_footstep_coords_by_index(initial_support_coords, overwrite_fs_idx-1)) {
+        if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
             std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
             return false;
         }
+        initial_support_coords = initial_support_steps.front().worldcoords;
     } else {
         // If walking, snap initial leg to current ABC foot coords.
         initial_support_coords = ikp[std::string(fs[0].leg)].target_end_coords;
@@ -1092,6 +1112,100 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo
   }
 }
 
+bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+    if (!is_stop_mode) {
+        std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
+
+        // Initial footstep Snapping
+        coordinates tmpfs, fstrans;
+        step_node initial_support_step, initial_input_step;
+        {
+            std::vector<step_node> initial_support_steps;
+            if (gg_is_walking) {
+                if (overwrite_fs_idx <= 0) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+                if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
+                    std::cerr << "[" << m_profile.instance_name << "]   Invalid overwrite index = " << overwrite_fs_idx << std::endl;
+                    return false;
+                }
+            } else {
+                // If walking, snap initial leg to current ABC foot coords.
+                for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                    initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0));
+                }
+            }
+            initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
+        }
+        {
+            std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
+            for (size_t i = 0; i < fss[0].fs.length(); i++) {
+                if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
+                    coordinates tmp;
+                    memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
+                    tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
+                }
+            }
+        }
+
+        // Get footsteps
+        std::vector< std::vector<coordinates> > fs_vec_list;
+        std::vector< std::vector<std::string> > leg_name_vec_list;
+        for (size_t i = 0; i < fss.length(); i++) {
+            std::vector<coordinates> fs_vec;
+            std::vector<std::string> leg_name_vec;
+            for (size_t j = 0; j < fss[i].fs.length(); j++) {
+                std::string leg(fss[i].fs[j].leg);
+                if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
+                    memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
+                    tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
+                    initial_input_step.worldcoords.transformation(fstrans, tmpfs);
+                    tmpfs = initial_support_step.worldcoords;
+                    tmpfs.transform(fstrans);
+                } else {
+                    std::cerr << "[" << m_profile.instance_name << "]   No such target : " << leg << std::endl;
+                    return false;
+                }
+                leg_name_vec.push_back(leg);
+                fs_vec.push_back(tmpfs);
+            }
+            leg_name_vec_list.push_back(leg_name_vec);
+            fs_vec_list.push_back(fs_vec);
+        }
+        if (spss.length() != fs_vec_list.size()) {
+            std::cerr << "[" << m_profile.instance_name << "]   StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
+            return false;
+        }
+        std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
+        std::vector< std::vector<step_node> > fnsl;
+        for (size_t i = 0; i < fs_vec_list.size(); i++) {
+            if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
+                std::vector<step_node> tmp_fns;
+                for (size_t j = 0; j < fs_vec_list.at(j).size(); j++) {
+                    tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
+                }
+                fnsl.push_back(tmp_fns);
+            }
+        }
+        if (gg_is_walking) {
+            std::cerr << "[" << m_profile.instance_name << "]  Set overwrite footsteps" << std::endl;
+            gg->set_overwrite_foot_steps_list(fnsl);
+            gg->set_overwrite_foot_step_index(overwrite_fs_idx);
+        } else {
+            std::cerr << "[" << m_profile.instance_name << "]  Set normal footsteps" << std::endl;
+            gg->set_foot_steps_list(fnsl);
+            startWalking();
+        }
+        return true;
+    } else {
+        std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
+        return false;
+    }
+}
+
 void AutoBalancer::waitFootSteps()
 {
   //while (gg_is_walking) usleep(10);

diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h
@@ -94,7 +94,9 @@ class AutoBalancer
   bool goStop();
   bool emergencyStop ();
   bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
+  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
   bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
   void waitFootSteps();
   void waitFootStepsEarly(const double tm);
   bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);

diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp
@@ -34,11 +34,21 @@ CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalance
   return m_autobalancer->setFootSteps(fs, overwrite_fs_idx);
 }
 
+CORBA::Boolean AutoBalancerService_impl::setFootSteps2(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
+{
+  return m_autobalancer->setFootSteps(fss, overwrite_fs_idx);
+}
+
 CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx)
 {
   return m_autobalancer->setFootStepsWithParam(fs, sps, overwrite_fs_idx);
 }
 
+CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam2(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
+{
+  return m_autobalancer->setFootStepsWithParam(fss, spss, overwrite_fs_idx);
+}
+
 void AutoBalancerService_impl::waitFootSteps()
 {
   return m_autobalancer->waitFootSteps();

diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h
@@ -20,7 +20,9 @@ class AutoBalancerService_impl
   CORBA::Boolean goStop();
   CORBA::Boolean emergencyStop();
   CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
+  CORBA::Boolean setFootSteps2(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
   CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
+  CORBA::Boolean setFootStepsWithParam2(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
   void waitFootSteps();
   void waitFootStepsEarly(CORBA::Double tm);
   CORBA::Boolean startAutoBalancer(const OpenHRP::AutoBalancerService::StrSequence& limbs);

diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h
@@ -947,10 +947,10 @@ namespace rats
             return false;
         }
     };
-    bool get_footstep_coords_by_index (coordinates& cs, const size_t idx)
+    bool get_footstep_nodes_by_index (std::vector<step_node>& csl, const size_t idx)
     {
         if (footstep_nodes_list.size()-1 >= idx) {
-            cs = footstep_nodes_list[idx].front().worldcoords;
+            csl = footstep_nodes_list.at(idx);
             return true;
         } else {
             return false;