|
#!/usr/bin/env python3
|
|
|
|
import threading # multi-threading stuff
|
|
import time # sleep()
|
|
import json # parsing json files
|
|
from PyQt5.QtWidgets import QApplication,QWidget # to manipualte some Qt elements
|
|
from pymove4dgui import * # some tools for move4d written in python
|
|
from move4d import * # the core move4d library wrapper
|
|
import move4dogre # the GUI library wrapper
|
|
|
|
import sys
|
|
import numpy as np # for math, vectors...
|
|
|
|
pepper = None
|
|
human = None
|
|
app = None
|
|
base= None
|
|
solutions=[]
|
|
|
|
def run(args=None):
|
|
# specify that we want to use the global varaibles for these names
|
|
global pepper
|
|
global human
|
|
global app
|
|
global base
|
|
app=move4dogre.main(args) # create and launch the app with given CLI arguments
|
|
|
|
pepper=app.pro.getActiveScene().getRobotByName("PEPPER_ROBOT")
|
|
human=app.pro.getActiveScene().getRobotByNameContaining('HUMAN')
|
|
|
|
# wait for move4d gui to be initialized. The semaphore will be released after rendering the first frame.
|
|
sema = threading.Semaphore(0)
|
|
app.pushMethodCall(lambda obj: sema.release())
|
|
sema.acquire()
|
|
|
|
# some settings of the 3D view
|
|
base=MoveOgre.OgreBase.getInstance()
|
|
# MoveOgre.FrameCreator.getInstance().setGroupVisibility("RobotFrames",False) # uncomment to hide the frames at each joint
|
|
# base.setHudVisible(False) # uncomment to hide the HUD
|
|
|
|
# examples for setting the robo configurations
|
|
|
|
#q=pepper.getCurrentPos()
|
|
#q[6]=7
|
|
#q[7]=8.5
|
|
#pepper.setAndUpdate(q)
|
|
#pepper.setInitialPosition(q)
|
|
|
|
#q=human.getCurrentPos()
|
|
#q[7]=7.9
|
|
#q[6]= 7.5
|
|
#q[8]=1.2
|
|
#human.setAndUpdate(q)
|
|
#human.setInitialPosition(q)
|
|
|
|
return app
|
|
|
|
def reset():
|
|
'''reset the agents to their initial position'''
|
|
human.setAndUpdate(human.getInitialPosition())
|
|
pepper.setAndUpdate(pepper.getInitialPosition())
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
run()
|