From 7355bbaccc3c8266777715fe700d13a7335ccd02 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 22 Jun 2017 15:01:42 +0900 Subject: [PATCH] [rtc/AutoBalancer/GaitGenerator, sample/SampleRobot/samplerobot_auto_balancer.py] Fix foot step limitation in case of offset vel is specified. Fix testing code. --- rtc/AutoBalancer/GaitGenerator.cpp | 4 ++-- sample/SampleRobot/samplerobot_auto_balancer.py | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 8df36caea57..45bbc8bee33 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -1002,12 +1002,12 @@ namespace rats dy = std::max(-1 * footstep_param.stride_y / default_step_time, std::min(footstep_param.stride_y / default_step_time, dy )); /* inside step limitation */ if (use_inside_step_limitation) { - if (cur_vel_param.velocity_y > 0) { + if (dy > 0) { if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) dy *= 0.5; } else { if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) dy *= 0.5; } - if (cur_vel_param.velocity_theta > 0) { + if (dth > 0) { if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) dth *= 0.5; } else { if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) dth *= 0.5; diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 596fed781fe..606ded99173 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -670,16 +670,17 @@ def demoGaitGeneratorGrasplessManipMode(): gv_pose_list = [av_fwd, av_bwd, av_left, av_right, av_lturn, av_rturn] ref_footmid_diff = [[50*1e-3,0,0], [-50*1e-3,0,0], - [0, 0.5*50*1e-3,0], # 0.5->inside limitation - [0,-0.5*50*1e-3,0], # 0.5->inside limitation - [0,0, 0.5*10], # 0.5->inside limitation - [0,0,-0.5*10]] # 0.5->inside limitation + [0, 50*1e-3,0], + [0,-50*1e-3,0], + [0,0, 10], + [0,0,-10]] ret=True hcf.abc_svc.waitFootSteps() for idx in range(len(gv_pose_list)): pose = gv_pose_list[idx] prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords - hcf.abc_svc.goVelocity(0,0,0); + yvel = -0.0001 if (idx%2==0) else 0.0001 # Iff even number test, start with rleg. Otherwise, lleg. + hcf.abc_svc.goVelocity(0,yvel,0); hcf.seq_svc.setJointAngles(pose, 0.4) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(pose, 1.6);hcf.waitInterpolation() # Dummy 2step