diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index c7069f1dd07..272c0e6de97 100644 --- 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 fs; + }; + /** * @struct StepParam * @brief Step parameter for one step @@ -43,6 +52,15 @@ module OpenHRP double heel_angle; }; + /** + * @struct StepParams + * @brief Step parameters for multi step + */ + struct StepParams + { + sequence sps; + }; + /** * @struct FootstepSequence * @brief Sequence of foot step. @@ -50,6 +68,13 @@ module OpenHRP typedef sequence FootstepSequence; typedef sequence StepParamSequence; + /** + * @struct FootstepsSequence + * @brief Sequence of foot steps. + */ + typedef sequence FootstepsSequence; + typedef sequence StepParamsSequence; + /** * @enum SupportLegState * @brief State of support leg. @@ -218,19 +243,19 @@ module OpenHRP /** * @brief Set footsteps. Returns without waiting for whole steps to be executed. - * @param fs is sequence of FootStep structure. + * @param fss is sequence of FootStep structure. overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking. * @return true if set successfully, false otherwise */ - boolean setFootSteps(in FootstepSequence fs, in long overwrite_fs_idx); + boolean setFootSteps(in FootstepsSequence fss, in long overwrite_fs_idx); /** * @brief Set footsteps. Returns without waiting for whole steps to be executed. - * @param fs is sequence of FootStepWithParam structure. + * @param fss is sequence of FootStepWithParam structure. overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking. * @return true if set successfully, false otherwise */ - boolean setFootStepsWithParam(in FootstepSequence fs, in StepParamSequence sps, in long overwrite_fs_idx); + boolean setFootStepsWithParam(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 index acce48577b8..f1ed502f591 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -591,7 +591,7 @@ void AutoBalancer::getTargetParameters() m_contactStates.data[contact_states_index_map["lleg"]] = true; break; default: - std::cerr << "not implemented yet " << std::endl; + if (DEBUGP) std::cerr << "not implemented yet " << std::endl; break; } m_controlSwingSupportTime.data[contact_states_index_map["rleg"]] = gg->get_current_swing_time(0); @@ -1012,84 +1012,116 @@ bool AutoBalancer::releaseEmergencyStop () return true; } -bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx) +bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx) { - OpenHRP::AutoBalancerService::StepParamSequence sps; - sps.length(fs.length()); + 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 < sps.length(); i++) sps[i].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height()); - for (size_t i = 0; i < sps.length(); i++) sps[i].step_time = gg->get_default_step_time(); - for (size_t i = 0; i < sps.length(); i++) sps[i].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle()); - for (size_t i = 0; i < sps.length(); i++) sps[i].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle()); - setFootStepsWithParam(fs, sps, overwrite_fs_idx); + 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) +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 << "] setFootSteps" << std::endl; + 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 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_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(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); + } + } + } - // Initial footstep Snapping - coordinates tmpfs, initial_support_coords, initial_input_coords, fstrans; - 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; + // Get footsteps + std::vector< std::vector > fs_vec_list; + std::vector< std::vector > leg_name_vec_list; + for (size_t i = 0; i < fss.length(); i++) { + std::vector fs_vec; + std::vector 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(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 (!gg->get_footstep_coords_by_index(initial_support_coords, overwrite_fs_idx-1)) { - std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl; + 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 > 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 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 { - // If walking, snap initial leg to current ABC foot coords. - initial_support_coords = ikp[std::string(fs[0].leg)].target_end_coords; - } - memcpy(initial_input_coords.pos.data(), fs[0].pos, sizeof(double)*3); - initial_input_coords.rot = (Eigen::Quaternion(fs[0].rot[0], fs[0].rot[1], fs[0].rot[2], fs[0].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z) - - // Get footsteps - std::vector fs_vec; - std::vector leg_name_vec; - for (size_t i = 0; i < fs.length(); i++) { - std::string leg(fs[i].leg); - if (leg == "rleg" || leg == "lleg") { - memcpy(tmpfs.pos.data(), fs[i].pos, sizeof(double)*3); - tmpfs.rot = (Eigen::Quaternion(fs[i].rot[0], fs[i].rot[1], fs[i].rot[2], fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z) - initial_input_coords.transformation(fstrans, tmpfs); - tmpfs = initial_support_coords; - tmpfs.transform(fstrans); - leg_name_vec.push_back(leg); - fs_vec.push_back(tmpfs); - } else { - std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl; - return false; - } - } - if (sps.length() != fs_vec.size()) { - std::cerr << "[" << m_profile.instance_name << "] StepParam length " << sps.length () << " != Footstep length " << fs_vec.size() << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl; return false; } - std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl; - std::vector< std::vector > fnsl; - for (size_t i = 0; i < fs_vec.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. - fnsl.push_back(boost::assign::list_of(step_node(leg_name_vec[i], fs_vec[i], sps[i].step_height, sps[i].step_time, sps[i].toe_angle, sps[i].heel_angle))); - } - 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() diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index c84f6b07a03..798ac4f372e 100644 --- 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 index 7ea4f8b2c4b..9b4558d391d 100644 --- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp +++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp @@ -29,14 +29,14 @@ CORBA::Boolean AutoBalancerService_impl::emergencyStop() return m_autobalancer->emergencyStop(); }; -CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx) +CORBA::Boolean AutoBalancerService_impl::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx) { - return m_autobalancer->setFootSteps(fs, 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) +CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx) { - return m_autobalancer->setFootStepsWithParam(fs, sps, overwrite_fs_idx); + return m_autobalancer->setFootStepsWithParam(fss, spss, overwrite_fs_idx); } void AutoBalancerService_impl::waitFootSteps() diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h index 7eb96e7af7a..2657a55777a 100644 --- a/rtc/AutoBalancer/AutoBalancerService_impl.h +++ b/rtc/AutoBalancer/AutoBalancerService_impl.h @@ -19,8 +19,8 @@ class AutoBalancerService_impl CORBA::Boolean goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth); CORBA::Boolean goStop(); CORBA::Boolean emergencyStop(); - CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, 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 setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx); + CORBA::Boolean setFootStepsWithParam(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 index 758e6817f1b..d0f7673cc14 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -25,6 +25,9 @@ namespace rats leg_type l_r; coordinates worldcoords; double step_height, step_time, toe_angle, heel_angle; + step_node () : l_r(RLEG), worldcoords(coordinates()), + step_height(), step_time(), + toe_angle(), heel_angle(){}; step_node (const leg_type _l_r, const coordinates& _worldcoords, const double _step_height, const double _step_time, const double _toe_angle, const double _heel_angle) @@ -700,7 +703,7 @@ namespace rats coordinates tmp_swg_mid, tmp_sup_mid; multi_mid_coords(tmp_swg_mid, swg_coords); multi_mid_coords(tmp_sup_mid, sup_coords); - mid_coords(ret, 0.5, tmp_swg_mid, tmp_sup_mid); + mid_coords(ret, static_cast(sup_coords.size()) / (swg_coords.size() + sup_coords.size()), tmp_swg_mid, tmp_sup_mid); }; std::vector get_current_support_states () const { @@ -944,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& 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; @@ -956,6 +959,7 @@ namespace rats void print_footstep_nodes_list (const std::vector< std::vector > _footstep_nodes_list) const { for (size_t i = 0; i < _footstep_nodes_list.size(); i++) { + std::cerr << "foot step index : " << i << std::endl; for (size_t j = 0; j < _footstep_nodes_list.at(i).size(); j++) { std::cerr << _footstep_nodes_list.at(i).at(j) << std::endl; } diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 530929d964a..84061b41d39 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -146,13 +146,13 @@ def demoGaitGeneratorGoVelocity(): def demoGaitGeneratorSetFootSteps(): print >> sys.stderr, "3. setFootSteps" - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.3,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() print >> sys.stderr, " setFootSteps()=>OK" @@ -190,11 +190,11 @@ def demoGaitGeneratorSetParam(): def demoGaitGeneratorNonDefaultStrideStop(): print >> sys.stderr, "7. non-default stride" - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() checkActualBaseAttitude() print >> sys.stderr, " Non default Stride()=>OK" @@ -221,7 +221,8 @@ def demoGaitGeneratorStopStartSyncCheck(): print >> sys.stderr, "9. Stop and start auto balancer sync check2" print >> sys.stderr, " Check 9-1 Sync after setFootSteps" hcf.startAutoBalancer(); - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]); + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]); hcf.abc_svc.waitFootSteps(); hcf.stopAutoBalancer(); print >> sys.stderr, " Sync after setFootSteps => OK" @@ -230,7 +231,8 @@ def demoGaitGeneratorStopStartSyncCheck(): hcf.seq_svc.setJointAngles(open_stride_pose, 2.0); hcf.waitInterpolation(); hcf.startAutoBalancer(); - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]); + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")])]); hcf.abc_svc.waitFootSteps(); hcf.stopAutoBalancer(); print >> sys.stderr, " Sync from setJointAngle at the beginning => OK" @@ -276,32 +278,32 @@ def demoGaitGeneratorChangeStepParam(): ggp.toe_angle = 50; ggp.heel_angle = 50; hcf.abc_svc.setGaitGeneratorParam(ggp); - hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")], - [OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0)]) + hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])], + [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=2.0, toe_angle=0.0, heel_angle=0.0)])]) hcf.abc_svc.waitFootSteps() - hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")], - [OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]) + hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])], + [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.1, step_time=1.0, toe_angle=0.0, heel_angle=0.0)])]) hcf.abc_svc.waitFootSteps() - hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")], - [OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=20.0, heel_angle=5.0), - OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=10.0, heel_angle=10.0)]) + hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,0.09,0], [1,0,0,0], "lleg")])], + [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=20.0, heel_angle=5.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=1.0, toe_angle=10.0, heel_angle=10.0)])]) hcf.abc_svc.waitFootSteps() hcf.abc_svc.setGaitGeneratorParam(ggp_org); checkActualBaseAttitude() @@ -321,12 +323,12 @@ def demoGaitGeneratorOverwriteFootsteps(overwrite_offset_idx = 1): def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init_fs = False): if init_fs: - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0, -0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.1, 0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.3, 0.09,0], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([0.4,-0.09,0], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([0.4, 0.09,0], [1,0,0,0], "lleg")]); + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0, -0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.1, 0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.2,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.3, 0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.4, 0.09,0], [1,0,0,0], "lleg")])]); print >> sys.stderr, " Overwrite footsteps ", overwrite_offset_idx # Get remaining footstep [remain_fs, current_fs_idx]=hcf.abc_svc.getRemainingFootstepSequence()[1:] @@ -349,11 +351,11 @@ def demoGaitGeneratorOverwriteFootstepsBase(axis, overwrite_offset_idx = 1, init fpos2=list(numpy.array(support_fs.pos) + numpy.array(pos_offset)) fpos3=list(numpy.array(support_fs.pos) + numpy.array([0, 2.0*(0.09 if support_fs.leg =='rleg' else -0.09) ,0]) + numpy.array(pos_offset2)) fpos4=list(numpy.array(support_fs.pos) + numpy.array(pos_offset2)) - new_fs =[OpenHRP.AutoBalancerService.Footstep(support_fs.pos, [1,0,0,0], support_fs.leg), - OpenHRP.AutoBalancerService.Footstep(fpos1, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg"), - OpenHRP.AutoBalancerService.Footstep(fpos2, [1,0,0,0], support_fs.leg), - OpenHRP.AutoBalancerService.Footstep(fpos3, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg"), - OpenHRP.AutoBalancerService.Footstep(fpos4, [1,0,0,0], support_fs.leg)] + new_fs =[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(support_fs.pos, [1,0,0,0], support_fs.leg)]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos1, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos2, [1,0,0,0], support_fs.leg)]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos3, [1,0,0,0], "lleg" if support_fs.leg =='rleg' else "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep(fpos4, [1,0,0,0], support_fs.leg)])] hcf.abc_svc.setFootSteps(new_fs, overwrite_fs_idx); def demoGaitGeneratorHandFixWalking(): diff --git a/sample/SampleRobot/samplerobot_collision_detector.py b/sample/SampleRobot/samplerobot_collision_detector.py index 6e60e203fd9..3581b8f8e2d 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -89,9 +89,11 @@ def demoCollisionMask (): hcf.startAutoBalancer() print >> sys.stderr, " 5.1 Collision mask among legs : Check RLEG_ANKLE_R - LLEG_ANKLE_R" print >> sys.stderr, " Desired behavior : Robot stops when legs collision." - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg"),OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg"),OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); print >> sys.stderr, " => Successfully mask works. Legs joints stops when collision." print >> sys.stderr, " 5.2 Collision mask between leg and arm : Check RLEG_HIP_R and RARM_WRIST*" @@ -110,9 +112,11 @@ def demoCollisionMask (): hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); hcf.waitInterpolation(); print >> sys.stderr, " Desired behavior : Next, arm keeps stopping and legs stops." - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg"),OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg"),OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0],[1,0,0,0],"lleg")])]) hcf.abc_svc.waitFootSteps(); print >> sys.stderr, " => Successfully mask works with combined situation." hcf.stopAutoBalancer() diff --git a/sample/SampleRobot/samplerobot_terrain_walk.py b/sample/SampleRobot/samplerobot_terrain_walk.py index ade247879ac..ea8d15bb2a2 100755 --- a/sample/SampleRobot/samplerobot_terrain_walk.py +++ b/sample/SampleRobot/samplerobot_terrain_walk.py @@ -49,15 +49,15 @@ def stairWalk(stair_height = 0.1524): init_step_z = 0 for step_idx in [1,2,3,4]: setupGaitGeneratorParam(step_idx%2==1) - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")])]) hcf.abc_svc.waitFootSteps() - hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg"), - OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg"), - OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]) + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])]) hcf.abc_svc.waitFootSteps() init_step_x = init_step_x + stair_stride_x + floor_stride_x init_step_z = init_step_z + stair_height @@ -67,7 +67,16 @@ def demoSlopeUpDown(): print "Start stlop up down" setupGaitGeneratorParam() hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); - fsList=[OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.0953,0.09,0.030712], [0.991445,0.0,-0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.28848,-0.09,0.082475], [0.991445,0.0,-0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.38508,0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.38508,-0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.54959,0.09,0.125863], [0.991445,0.0,0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.74277,-0.09,0.074099], [0.991445,0.0,0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.79107,0.09,0.061158], [0.991445,0.0,0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([2.05,-0.09,0.0], [1.0,0.0,0.0,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([2.05,0.09,0.0], [1.0,0.0,0.0,0.0], "lleg")] + fsList=[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.0953,0.09,0.030712], [0.991445,0.0,-0.130526,0.0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.28848,-0.09,0.082475], [0.991445,0.0,-0.130526,0.0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.38508,0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.38508,-0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.54959,0.09,0.125863], [0.991445,0.0,0.130526,0.0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.74277,-0.09,0.074099], [0.991445,0.0,0.130526,0.0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.79107,0.09,0.061158], [0.991445,0.0,0.130526,0.0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([2.05,-0.09,0.0], [1.0,0.0,0.0,0.0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([2.05,0.09,0.0], [1.0,0.0,0.0,0.0], "lleg")])] # set st Parameter # stp1 = hcf.st_svc.getParameter() # stp1.k_tpcc_p=[0.05, 0.05] @@ -76,7 +85,7 @@ def demoSlopeUpDown(): # hcf.st_svc.setParameter(stp1) # hcf.st_svc.startStabilizer () for idx in range(len(fsList)-1): - hcf.abc_svc.setFootSteps([fsList[idx],fsList[idx+1]]) + hcf.setFootSteps([fsList[idx],fsList[idx+1]]) hcf.abc_svc.waitFootSteps() hcf.abc_svc.stopAutoBalancer();