Add test code to check ST transition problem.

rtc/Stabilizer/Stabilizer.cpp

@@ -1737,9 +1737,10 @@ void Stabilizer::sync_2_idle ()
 
 void Stabilizer::startStabilizer(void)
 {
+ waitSTTransition(); // Wait until all transition has finished
  {
  Guard guard(m_mutex);
- if ( transition_count == 0 && control_mode == MODE_IDLE ) {
+ if ( control_mode == MODE_IDLE ) {
  std::cerr << "[" << m_profile.instance_name << "] " << "Start ST" << std::endl;
  sync_2_st();
  }
@@ -1750,18 +1751,14 @@ void Stabilizer::startStabilizer(void)
 
 void Stabilizer::stopStabilizer(void)
 {
- bool is_stop_st = false;
+ waitSTTransition(); // Wait until all transition has finished
  {
  Guard guard(m_mutex);
- if ( transition_count == 0 && (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
+ if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
  std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST" << std::endl;
- control_mode = MODE_SYNC_TO_IDLE;
- is_stop_st = true;
+ control_mode = (control_mode == MODE_ST) ? MODE_SYNC_TO_IDLE : MODE_IDLE;
  }
  }
- if (is_stop_st) {
- while (control_mode != MODE_IDLE) { usleep(10); };
- }
  waitSTTransition();
  std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST DONE" << std::endl;
 }
@@ -2335,7 +2332,15 @@ void Stabilizer::setBoolSequenceParamWithCheckContact (std::vector<bool>& st_boo
 
 void Stabilizer::waitSTTransition()
 {
- while (transition_count != 0) usleep(10);
+ // Wait condition
+ // 1. Check transition_count : Wait until transition is finished
+ // 2. Check control_mode : Once control_mode is SYNC mode, wait until control_mode moves to the next mode (MODE_AIR or MODE_IDLE)
+ bool flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE);
+ while (transition_count != 0 ||
+ (flag ? !(control_mode == MODE_IDLE || control_mode == MODE_AIR) : false) ) {
+ usleep(10);
+ flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE);
+ }
  usleep(10);
 }
 

sample/SampleRobot/samplerobot_stabilizer.py

@@ -90,6 +90,17 @@ def demoSetParameter():
  print >> sys.stderr, " setParameter() => OK", vcheck
  assert(vcheck)
 
+def changeContactDecisionThre (thre):
+ stp = hcf.st_svc.getParameter()
+ stp.contact_decision_threshold=thre
+ hcf.st_svc.setParameter(stp)
+
+def mimicInTheAir ():
+ changeContactDecisionThre(10000) # [N]
+
+def mimicOnTheGround ():
+ changeContactDecisionThre(50) # [N], default
+
 def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-stabilizer-check-param"):
  hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname)
 
@@ -316,6 +327,47 @@ def demoSTTurnWalk ():
  else:
  print >> sys.stderr, " This sample is neglected in High-gain mode simulation"
 
+
+def demoSTTransitionAirGround ():
+ # This example is from YoheiKakiuchi's comment : https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102#issuecomment-284609203
+ print >> sys.stderr, "9. ST Transition (in the air and on the ground)"
+ # Init
+ stp_org = hcf.st_svc.getParameter()
+ stp = hcf.st_svc.getParameter()
+ stp.transition_time = 0.1; # for fast checking
+ hcf.st_svc.setParameter(stp)
+ # Tests
+ print >> sys.stderr, " 9-1. Check in the air"
+ hcf.startStabilizer()
+ mimicInTheAir()
+ hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition
+ cmode1 = hcf.st_svc.getParameter().controller_mode
+ vcheck1 = (cmode1 == OpenHRP.StabilizerService.MODE_AIR)
+ print >> sys.stderr, " 9-2. Check on the ground"
+ mimicOnTheGround()
+ hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition
+ cmode2 = hcf.st_svc.getParameter().controller_mode
+ vcheck2 = (cmode2 == OpenHRP.StabilizerService.MODE_ST)
+ print >> sys.stderr, " 9-3. Check in the air and then stopST"
+ mimicInTheAir()
+ hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until in the air flag is invoked in onExecute
+ hcf.stopStabilizer()
+ cmode3 = hcf.st_svc.getParameter().controller_mode
+ vcheck3 = (cmode3 == OpenHRP.StabilizerService.MODE_IDLE)
+ print >> sys.stderr, " 9-4. Check on the ground"
+ mimicOnTheGround()
+ hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until on the ground flag is invoked in onExecute
+ hcf.startStabilizer()
+ cmode4 = hcf.st_svc.getParameter().controller_mode
+ vcheck4 = (cmode4 == OpenHRP.StabilizerService.MODE_ST)
+ # Finsh
+ hcf.st_svc.setParameter(stp_org)
+ vcheck_list = [vcheck1, vcheck2, vcheck3, vcheck4]
+ print >> sys.stderr, " ST Transition Air Ground vcheck = ", vcheck_list, ", cmode = ", [cmode1, cmode2, cmode3, cmode4]
+ if all(vcheck_list):
+ print >> sys.stderr, " ST Transition Air Ground => OK"
+ assert(all(vcheck_list))
+
 def demo():
  OPENHRP3_DIR=check_output(['pkg-config', 'openhrp3.1', '--variable=prefix']).rstrip()
  if os.path.exists(OPENHRP3_DIR+"/share/OpenHRP-3.1/sample/model/sample1_bush.wrl"):
@@ -329,6 +381,7 @@ def demo():
  demoSTStairWalk()
  demoSTToeHeelWalk()
  demoSTTurnWalk()
+ demoSTTransitionAirGround()
  else:
  print >> sys.stderr, "Skip st test because of missing sample1_bush.wrl"