def loadSDFFileAndRunSim(self):
     from ddapp import sceneloader
     filename= os.environ['DRC_BASE'] + '/../drc-testing-data/terrain/terrain_simple.sdf'
     sc=sceneloader.SceneLoader()
     sc.loadSDF(filename)
     import ipab
     msg=ipab.scs_api_command_t()
     msg.command="loadSDF "+filename+"\nsimulate"
     lcmUtils.publish('SCS_API_CONTROL', msg)
Example #2
0
def Tester():
  global listener
  if ikPlanner.pushToMatlab==False:
    print "FAILURE - NOT pushing requests to matlab!"
    exit()
  
  ## Begin Test ##
  
  # Start the simulator
  print "Starting the simulation"
  msg=ipab.scs_api_command_t()
  msg.command="simulate"
  lcmUtils.publish('SCS_API_CONTROL', msg)
  time.sleep(2.0)

  # Wait until the footsteps are produced
  print "Waiting for Matlab to start"

  # Set map mode to horizontal plane
  footstepsPanel.propertiesPanel.children()[1].children()[8].currentIndex=3

  while not om.findObjectByName('step 6'):
    time.sleep(2.0)
    print "Sending footstep plan request"
    run('footstepsPanel.onNewWalkingGoal()')
  t=om.findObjectByName('step 6 frame').transform
  footgoal=numpy.array(t.GetPosition()+t.GetOrientation())
  
  
  # Execute footsteps  
  print "Executing the footstep plan"
  run('footstepsPanel.onExecute()')
  
  # Wait for the motion to finish
  print "Waiting for execution"
  while not footstep_status_ok:
    time.sleep(0.5)

  foot=footstep_pos

  # Check success

  if ((footgoal-foot).__abs__()>1e-3).any():
    print "FAILURE - foot wasn't in the corect position!"
    exit()

  ## End Test ##

  print "Success - the robot arrived at its destination"
  #run("lcmUtils.publish('SCS_API_CONTROL', msgstop)")

  # When everything goes all right, report success
  with open(os.environ.get('SYSTEMTEST_RESULT_FILE'),'w+') as f:
    f.write('1\n')
  time.sleep(0.5)
  run('exit()')
    def loadSDFFileAndRunSim(self, kindOfTerrain):
        from ddapp import sceneloader

        if kindOfTerrain == 'simple':
            filename= os.environ['DRC_BASE'] + '/software/models/worlds/terrain_simple.sdf'
        elif kindOfTerrain == 'uneven':
            filename= os.environ['DRC_BASE'] + '/software/models/worlds/terrain_uneven.sdf'
        
        sc=sceneloader.SceneLoader()
        sc.loadSDF(filename)
        import ipab
        msg=ipab.scs_api_command_t()
        msg.command="loadSDF "+filename+"\nsimulate"
        lcmUtils.publish('SCS_API_CONTROL', msg)
Example #4
0
  foot=footstep_pos

  # Check success

  if ((footgoal-foot).__abs__()>1e-3).any():
    print "FAILURE - foot wasn't in the corect position!"
    exit()

  ## End Test ##

  print "Success - the robot arrived at its destination"
  #run("lcmUtils.publish('SCS_API_CONTROL', msgstop)")

  # When everything goes all right, report success
  with open(os.environ.get('SYSTEMTEST_RESULT_FILE'),'w+') as f:
    f.write('1\n')
  time.sleep(0.5)
  run('exit()')

msgstop=ipab.scs_api_command_t()
msgstop.command="stop"

thr = threading.Thread(target=Tester)
thr.setDaemon(True)
lcmUtils.addSubscriber('IHMC_FOOTSTEP_STATUS', messageClass=ipab.footstep_status_t, callback=footstep_callback);
thr.start()