from klampt import *
from klampt import WidgetSet, RobotPoser
from klampt.glprogram import *
from klampt.glcommon import GLWidgetPlugin
import math
import time
import signal
from sspp.topic import *
ebolabot_root = os.getenv("EBOLABOT_PATH", ".")
sys.path.append(ebolabot_root)
from Common.system_config import EbolabotSystemConfig
import threading
#Configuration variable: where's the state server?
system_state_addr = EbolabotSystemConfig.getdefault_ip(
'state_server_computer_ip', ('localhost', 4568))
class GLWidgetProgram(GLPluginProgram):
"""A program that uses a widget plugin"""
def __init__(self, name):
GLPluginProgram.__init__(self, "Manual poser")
self.widgetPlugin = GLWidgetPlugin()
def initialize(self):
GLPluginProgram.initialize(self)
self.setPlugin(self.widgetPlugin)
def addWidget(self, widget):
self.widgetPlugin.addWidget(widget)
from task_generator import TaskGenerator
from klampt.io import resource
from OpenGL.GL import *
from klampt.vis import gldraw
from klampt.vis.glinterface import GLPluginInterface as GLPluginBase
from klampt.vis.glcommon import GLWidgetPlugin
from klampt.math import so3,se3,vectorops
from sspp.service import Service
from sspp.topic import MultiTopicListener
from UI.utils.gltexture import *
from UI.utils.haptic_client import *
#Configuration variable: where's the state server?
system_state_addr = EbolabotSystemConfig.getdefault_ip('state_server_computer_ip',('localhost',4568))
#Configuration variable: where's the haptic service?
try:
#default 'tcp://192.168.1.128:3456' ?
haptic_service_addr = EbolabotSystemConfig.get_ip('haptic_server_computer_ip')
except Exception:
print "Haptic device server not configured. Please configure the appropriate variable in",EbolabotSystemConfig.system_config_fn
exit(1)
debugHapticTransform = False
#set this -1 for view-centric control, looking at the face of the robot
viewToWorldScaleXY = 1
gamepadBaseTranslationSpeedScale = 0.2
print "Head pan center"
motion.robot.head.setPan(0,0.25)
elif c == '3':
print "Head pan right"
motion.robot.head.setPan(-1.57,0.25)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
self.refresh()
if __name__ == "__main__":
config.parse_args()
print "widget_control.py: manually sends configurations to the Motion module"
print "Press [space] to send milestones. Press q to exit."
print
print "Loading Motion Module model",config.klampt_model
motion.setup(mode=config.mode,klampt_model=config.klampt_model,libpath="./",server_addr=EbolabotSystemConfig.get_ip('motion_computer_ip')[0])
res = motion.robot.startup()
if not res:
print "Error starting up Motion Module"
exit(1)
time.sleep(0.1)
world = WorldModel()
res = world.readFile(config.klampt_model)
if not res:
raise RuntimeError("Unable to load Klamp't model "+config.klampt_model)
viewer = MyGLViewer(world)
viewer.run()