def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
def init(self,world):
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.plugin = MyWidgetPlugin(self)
self.plugin.addWidget(self.robotPoser)
self.robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
base_link_names = ['base_x','base_y','base_yaw']
self.left_arm_link_indices = [self.robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [self.robot.link(l).index for l in right_arm_link_names]
self.base_link_indices = [self.robot.link(l).index for l in base_link_names]
if any(v < 0 for v in self.left_arm_link_indices+self.right_arm_link_indices):
raise ValueError("Robot in world does not have Baxter's torso?")
# == subscribe marker position
self.marker = Marker()
self.base_height = 1.1
self.marker_pos = [0.0, 0.0, 0.0]
self.marker_pos_tf = [0.0, 0.0, 0.0]
rospy.init_node('marker_listener', anonymous=True)
rospy.Subscriber("/Marker_glove", Marker, self.callback, None)
#== auto tracking parameter
self.tracking_speed_ratio = 1.0
self.tracking_vel = [0.0, 0.0, 0.0]
self.left_ee_t = [0.0, 0.0, 0.0]
self.right_ee_t = [0.0, 0.0, 0.0]
self.reactive_vel_left_ee = [0.0, 0.0, 0.0]
self.reactive_vel_right_ee = [0.0, 0.0, 0.0]
return True
def init(self, world):
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.plugin = MyWidgetPlugin(self)
self.plugin.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
base_link_names = ['base_x', 'base_y', 'base_yaw']
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.base_link_indices = [robot.link(l).index for l in base_link_names]
if any(v < 0 for v in self.left_arm_link_indices +
self.right_arm_link_indices):
raise ValueError("Robot in world does not have Baxter's torso?")
return True
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
def __init__(self,world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
self.add_action(self.print_config,'Print config',' ')
self.add_action(self.save_world,'Save world','s')
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
#start up the poser in the currently commanded configuration
q = motion.robot.getKlamptCommandedPosition()
world.robot(0).setConfig(q)
self.world = world
self.robot = world.robot(0)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
self.roadMap = ([], [])
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_names = left_arm_link_names
self.right_arm_link_names = right_arm_link_names
missing_left_arm_names = [
'left_upper_forearm_visual', 'left_upper_elbow_visual'
]
missing_right_arm_names = [
'right_upper_forearm_visual', 'right_upper_elbow_visual'
]
#self.left_arm_link_names+=missing_left_arm_names
#self.right_arm_link_names+=missing_right_arm_names
self.left_arm_link_names = [
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_upper_forearm_visual', 'left_gripper:base'
]
self.right_arm_link_names = [
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_upper_forearm_visual', 'right_gripper:base'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
self.collider = WorldCollider(self.world)
def __init__(self, world, serviceThread):
GLWidgetProgram.__init__(self, "Manual poser")
self.world = world
self.serviceThread = serviceThread
self.oldTime = 0.0
for i in range(10):
q = self.serviceThread.qcmdGetter.get()
if q != None:
break
time.sleep(0.1)
if q == None:
print "Warning, system state service doesn't seem to have .robot.command.q"
print "Press enter to continue"
raw_input()
else:
world.robot(0).setConfig(q)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
for i in range(10):
ee1 = self.serviceThread.ee1Getter.get()
ee2 = self.serviceThread.ee1Getter.get()
if ee1 != None and ee2 != None:
break
time.sleep(0.1)
if ee1 == None:
print "Warning, system state service doesn't seem to have .robot.endEffectors.0.xform.sensed"
print "Press enter to continue"
raw_input()
if ee2 == None:
print "Warning, system state service doesn't seem to have .robot.endEffectors.1.xform.sensed"
print "Press enter to continue"
raw_input()
def __init__(self, world):
GLWidgetProgram.__init__(self, world, "Manual poser")
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
"left_upper_shoulder",
"left_lower_shoulder",
"left_upper_elbow",
"left_lower_elbow",
"left_upper_forearm",
"left_lower_forearm",
"left_wrist",
]
right_arm_link_names = [
"right_upper_shoulder",
"right_lower_shoulder",
"right_upper_elbow",
"right_lower_elbow",
"right_upper_forearm",
"right_lower_forearm",
"right_wrist",
]
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"Manual poser")
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
motion.robot.left_limb.enableSelfCollisionAvoidance(False)
motion.robot.right_limb.enableSelfCollisionAvoidance(False)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
self.outputFileLeft = open("sag_calibration_left.csv","w")
self.outputFileRight = open("sag_calibration_right.csv","w")
#write headers
self.outputFileLeft.write("time,")
self.outputFileLeft.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileLeft.write(",")
self.outputFileLeft.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileLeft.write("\n")
self.outputFileRight.write("time,")
self.outputFileRight.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileRight.write(",")
self.outputFileRight.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileRight.write("\n")
self.t0 = time.time()
self.lastLeftMove = -100
self.lastRightMove = -100
def __init__(self,world):
GLWidgetProgram.__init__(self,"Manual poser")
self.world = world
q = motion.robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
head_pan_link_name = 'head'
self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
self.head_pan_link_index = robot.link(head_pan_link_name)
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w,vp.h = 400,600
vis.setViewport(vp)
vis.dialog()
vp.w,vp.h = 640,480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
class MyGLViewer(GLWidgetProgram):
def __init__(self,world):
GLWidgetProgram.__init__(self,"Manual poser")
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line will draw the robot's sensed configuration in the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
self.refresh()
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = world.robot(0)
self.subrobots = []
for i in range(6):
self.subrobots.append(
SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
(res, val) = editors.run(
editors.SelectionEditor("subrobot",
self.subrobots[0]._links,
description="sub robot links",
world=world,
robot=self.robot))
if res:
print("Return value", val)
self.subrobots[0]._links = val
self.startConfig = self.robot.getConfig()
self.robotWidget = RobotPoser(world.robot(0))
self.robotWidget.setActiveDofs(self.subrobots[0]._links)
self.addWidget(self.robotWidget)
def __init__(self,world):
GLWidgetProgram.__init__(self,"Manual poser")
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"Manual poser")
self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
#motion.robot.left_limb.enableSelfCollisionAvoidance(False)
#motion.robot.right_limb.enableSelfCollisionAvoidance(False)
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
class MyGLViewer(GLWidgetPlugin):
def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = world.robot(0)
self.subrobots = []
for i in range(6):
self.subrobots.append(
SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
(res, val) = editors.run(
editors.SelectionEditor("subrobot",
self.subrobots[0]._links,
description="sub robot links",
world=world,
robot=self.robot))
if res:
print("Return value", val)
self.subrobots[0]._links = val
self.startConfig = self.robot.getConfig()
self.robotWidget = RobotPoser(world.robot(0))
self.robotWidget.setActiveDofs(self.subrobots[0]._links)
self.addWidget(self.robotWidget)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
class MyGLViewer(GLWidgetProgram):
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"My GL program")
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotWidget)
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
print "Config:",config
self.world.robot(0).setConfig(config)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
class MyGLViewer(GLWidgetProgram):
def __init__(self, world):
GLWidgetProgram.__init__(self, world, "My GL program")
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotWidget)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
print "Config:", config
self.world.robot(0).setConfig(config)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
class MyGLViewer(GLWidgetPlugin):
def __init__(self,world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
self.add_action(self.print_config,'Print config',' ')
self.add_action(self.save_world,'Save world','s')
def print_config(self):
config = self.robotWidget.get()
print("Config:",config)
self.world.robot(0).setConfig(config)
def save_world(self):
fn = "widgets_test_world.xml"
print("Saving file to",fn)
self.world.saveFile(fn)
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
class MyGLViewer(GLWidgetPlugin):
def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
print "Config:", config
self.world.robot(0).setConfig(config)
elif c == 's':
fn = "widgets_test_world.xml"
print "Saving file to", fn
self.world.saveFile(fn)
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):
if simworld == None and planworld == None:
# Dummy GLViewer
print "simworld and planworld are None"
return
GLRealtimeProgram.__init__(self, "My GL program")
self.simworld = simworld
self.planworld = planworld
self.sim = sim
self.widgetMaster = WidgetSet()
self.widgetButton = 2 # right-clicks
self.draggingWidget = False
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(self.simworld.robot(0))
self.widgetMaster.add(self.robotWidget)
self.simulate = True
if self.sim == None:
self.simulate = False
self.controller = controller
self.command_queue = Queue()
if helper != None:
print "helper is not none"
helper.run()
self.points = None
self.extraControllers = {}
class KeyboardAutoTrackTaskGenerator(TaskGenerator):
"""Allows the user to interact with the model by right clicking
using the mouse and pressing space bar to send the milestone.
Also demonstrates how to write a Python plugin task with an OpenGL
widget.
"""
def __init__(self):
self.plugin = None
self.q = None
def callback(self, data):
self.marker = data
self.marker_pos[0] = self.marker.pose.position.x
self.marker_pos[1] = self.marker.pose.position.y
self.marker_pos[2] = self.marker.pose.position.z
#=== transform to world coordinate
marker = se3.apply(kinect_frame_to_pedestal, tuple(self.marker_pos))
self.marker_pos_tf = list(marker)
self.marker_pos_tf[2] = self.marker_pos_tf[2] + self.base_height
# print "marker_pos_tf = ", self.marker_pos_tf
def name(self): return "Keyboard"
def init(self,world):
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.plugin = MyWidgetPlugin(self)
self.plugin.addWidget(self.robotPoser)
self.robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
base_link_names = ['base_x','base_y','base_yaw']
self.left_arm_link_indices = [self.robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [self.robot.link(l).index for l in right_arm_link_names]
self.base_link_indices = [self.robot.link(l).index for l in base_link_names]
if any(v < 0 for v in self.left_arm_link_indices+self.right_arm_link_indices):
raise ValueError("Robot in world does not have Baxter's torso?")
# == subscribe marker position
self.marker = Marker()
self.base_height = 1.1
self.marker_pos = [0.0, 0.0, 0.0]
self.marker_pos_tf = [0.0, 0.0, 0.0]
rospy.init_node('marker_listener', anonymous=True)
rospy.Subscriber("/Marker_glove", Marker, self.callback, None)
#== auto tracking parameter
self.tracking_speed_ratio = 1.0
self.tracking_vel = [0.0, 0.0, 0.0]
self.left_ee_t = [0.0, 0.0, 0.0]
self.right_ee_t = [0.0, 0.0, 0.0]
self.reactive_vel_left_ee = [0.0, 0.0, 0.0]
self.reactive_vel_right_ee = [0.0, 0.0, 0.0]
return True
def start(self):
self._status = 'ok'
self.robotPoser.set(self.world.robot(0).getConfig())
self.serviceThread = ServiceThread()
self.serviceThread.start()
return True
def stop(self):
if self.serviceThread:
self.serviceThread.kill()
print "Waiting for thread join..."
self.serviceThread.join()
print "Done"
self.serviceThread = None
def getEE(self):
R1,t1 = self.serviceThread.eeGetter_left.get()
R2,t2 = self.serviceThread.eeGetter_right.get()
self.left_ee_t = list(t1)
self.right_ee_t = list(t2)
print "left hand EE = ", self.left_ee_t
print "right hand EE = ", self.right_ee_t
def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio):
reactive_vel = [0.0, 0.0, 0.0]
pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand))
pos_diff_norm = vectorops.norm(pos_diff)
if pos_diff_norm >= 0.02:
vel = vectorops.mul(pos_diff, vel_ratio)
reactive_vel = list(vel)
return reactive_vel
def get(self):
res = self.do_logic()
return res
def do_logic(self):
rospy.Subscriber("/Marker_glove", Marker, self.callback, None)
''' retract arm to initial position '''
if self.plugin.initialPose:
self.plugin.initialPose = False
self.q = self.robotPoser.get()
if self.plugin.selectedLimb == "both":
for i in range(len(self.left_arm_link_indices)):
idx = self.left_arm_link_indices[i]
self.q[idx] = q_init_left[i]
for i in range(len(self.right_arm_link_indices)):
idx = self.right_arm_link_indices[i]
self.q[idx] = q_init_right[i]
print "set both arms to initial pose"
if self.plugin.selectedLimb == "left":
for i in range(len(self.left_arm_link_indices)):
idx = self.left_arm_link_indices[i]
self.q[idx] = q_init_left[i]
print "set left arms to initial pose"
if self.plugin.selectedLimb == "right":
for i in range(len(self.right_arm_link_indices)):
idx = self.right_arm_link_indices[i]
self.q[idx] = q_init_right[i]
print "set right arms to initial pose"
self.robotPoser.set(self.q)
if self.plugin.tuckArmPose:
self.plugin.tuckArmPose = False
self.q = self.robotPoser.get()
if self.plugin.selectedLimb == "both":
for i in range(len(self.left_arm_link_indices)):
idx = self.left_arm_link_indices[i]
self.q[idx] = q_tuckarm_left[i]
for i in range(len(self.right_arm_link_indices)):
idx = self.right_arm_link_indices[i]
self.q[idx] = q_tuckarm_right[i]
print "Tuck both arms"
if self.plugin.selectedLimb == "left":
for i in range(len(self.left_arm_link_indices)):
idx = self.left_arm_link_indices[i]
self.q[idx] = q_tuckarm_left[i]
print "Tuck left arm"
if self.plugin.selectedLimb == "right":
for i in range(len(self.right_arm_link_indices)):
idx = self.right_arm_link_indices[i]
self.q[idx] = q_tuckarm_right[i]
print "Tuck right arm"
self.robotPoser.set(self.q)
''' Control - Robot arm and mobile base'''
if self.plugin.sendMilestone:
self.plugin.sendMilestone = False
self.q = self.robotPoser.get()
qcmd = self.world.robot(0).getConfig()
baseMoved = any(qcmd[i] != self.q[i] for i in self.base_link_indices)
leftArmMoved = any(qcmd[i] != self.q[i] for i in self.left_arm_link_indices)
rightArmMoved = any(qcmd[i] != self.q[i] for i in self.right_arm_link_indices)
moved = []
if baseMoved: moved.append('base')
if leftArmMoved: moved.append('left')
if rightArmMoved: moved.append('right')
targets = []
if baseMoved: targets.append([self.q[i] for i in self.base_link_indices])
if leftArmMoved: targets.append([self.q[i] for i in self.left_arm_link_indices])
if rightArmMoved: targets.append([self.q[i] for i in self.right_arm_link_indices])
print "Sending milestone",self.q,"parts",",".join(moved)
#CONSTRUCT THE TASK HERE
msg = {}
msg['type'] = 'JointPose'
msg['parts'] = moved
msg['positions'] = targets
msg['speed'] = 1
msg['safe'] = 0
return msg
''' Control - gripper '''
if self.plugin.gripperControl == True:
self.plugin.gripperControl = False
if self.plugin.gripperState == "open":
p = [1.0, 1.0, 1.0, 1.0]
if self.plugin.selectedLimb == "both":
p = p + p
elif self.plugin.gripperState == "close":
p = [0.3, 0.3, 0.3, 1.0]
if self.plugin.selectedLimb == "both":
p = p + p
msg = {}
msg['type'] = 'Gripper'
msg['limb'] = self.plugin.selectedLimb
msg['position'] = p
msg['force'] = 0.2
msg['speed'] = 0.5
return msg
''' Auto tracking '''
if self.plugin.trackPosition == True:
self.plugin.trackPosition_lastState = True
self.plugin.trackPosition = False
print "move to target: ", self.marker_pos_tf
self.getEE()
if self.plugin.selectedLimb == "both":
self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
print "Use both left or right arm to track"
linear_vel = self.reactive_vel_left_ee + self.reactive_vel_right_ee
angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
if self.plugin.selectedLimb == "left":
self.reactive_vel_left_ee = self.cal_reactive_vel(self.left_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
print "Move left arm to target at velocity: ", self.reactive_vel_left_ee
linear_vel = self.reactive_vel_left_ee
angular_vel = [0.0, 0.0, 0.0]
if self.plugin.selectedLimb == "right":
self.reactive_vel_right_ee = self.cal_reactive_vel(self.right_ee_t, self.marker_pos_tf, self.tracking_speed_ratio)
print "Move right arm to target"
linear_vel = self.reactive_vel_right_ee
angular_vel = [0.0, 0.0, 0.0]
msg = {}
msg['type'] = 'CartesianVelocity'
msg['limb'] = self.plugin.selectedLimb
msg['linear'] = linear_vel
msg['angular'] = angular_vel
return msg
if self.plugin.trackPosition_lastState == True:
self.plugin.trackPosition_lastState = False
print "last command is cartesian velocity ... "
if self.plugin.selectedLimb == "both":
linear_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
angular_vel = [0.0, 0.0, 0.0] + [0.0, 0.0, 0.0]
if self.plugin.selectedLimb == "left":
linear_vel = [0.0, 0.0, 0.0]
angular_vel = [0.0, 0.0, 0.0]
if self.plugin.selectedLimb == "right":
linear_vel = [0.0, 0.0, 0.0]
angular_vel = [0.0, 0.0, 0.0]
msg = {}
msg['type'] = 'CartesianVelocity'
msg['limb'] = self.plugin.selectedLimb
msg['linear'] = linear_vel
msg['angular'] = angular_vel
return msg
return None
def glPlugin(self):
return self.plugin
vis.show()
while vis.shown():
time.sleep(0.01)
"""
print "Doing a dialog..."
vis.setWindowTitle("Dialog test")
print "calling dialog()"
vis.dialog()
print "Doing a split screen program..."
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.show()
while vis.shown():
time.sleep(0.1)
print "Showing a dialog, back to normal..."
vis.setPlugin(None)
vis.dialog()
print "Showing again, back to normal..."
vis.setWindowTitle("Basic visualization test")
vis.show()
while vis.shown():
class MyGLViewer(GLWidgetProgram):
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line with draw the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
self.world.terrain(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
q0 = motion.robot.getKlamptCommandedPosition()
t1 = time.time()
#collisions = obstaclecollision(WorldCollider(self.world),self.world.robot(0),q0,q)
collides = bisection(WorldCollider(self.world),
self.world.robot(0), q0, q)
print "Obstacle collision detection done in time", +time.time() - t1
exit(0)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
#if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
if collides:
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def subscribe(self):
rospy.init_node('subscribe', anonymous=True)
rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)
def callback(self, data):
if self.newPC == None:
data_out = pc2.read_points(data, skip_nans=True)
#data_out = pc2.read_points(data)
pc = PointCloud()
pc.propertyNames.append('rgba')
t0 = time.time()
points = list(data_out)
#filtered = self.radiusfilter(points,0.1,0)
filtered = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 10)
print "Noise cleaned up in time", time.time() - t0
#filtered = points
for p in filtered:
coordinates = (p[0], p[1], p[2])
if vectorops.norm(coordinates) > 0.4:
index = pc.addPoint(coordinates)
i = struct.unpack("i", struct.pack("f", p[3]))[0]
pc.setProperty(index, 0, i)
#print "Point cloud copied to Klampt format in time",time.time()-t0
q = (0.566222, 0.423455, 0.424871, 0.5653)
R1 = so3.from_quaternion(q)
t1 = (0.228665, 0.0591513, 0.0977748)
T1 = (R1, t1)
R2 = so3.rotation((0, 0, 1), math.pi / 2)
t2 = (0, 0, 1)
T2 = (R2, t2)
T = se3.mul(T2, T1)
(R, t) = T
pc.transform(R, t)
#print "Point cloud transformed in time",time.time()-t0
self.newPC = pc
else:
print "Not ready yet to receive new point cloud..."
# Color data will be lost
def statisticalfilter(self, points, k, mul):
pclCloud = pcl.PointCloud()
pclCloud.from_list(points)
fil = pclCloud.make_statistical_outlier_filter()
fil.set_mean_k(k)
fil.set_std_dev_mul_thresh(mul)
filtered = fil.filter().to_list()
return filtered
# Under construction
def radiusfilter(self, points, radius, num):
filtered = []
coordinates = []
for p in points:
coordinates.append([p[0], p[1], p[2]])
#print np.array(coordinates).reshape(-len(coordinates),3)
tree = KDTree(np.array(coordinates))
#tree = KDTree(np.random.random((10, 3)), leaf_size=2)
for i, p in enumerate(coordinates):
#print tree.query_radius(p,r=radius,count_only=True)
if tree.query_radius(coordinates[i], r=radius,
count_only=True) >= num:
filtered.append(points[i])
return points
def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
filtered = []
dictionary = {}
for i, p in enumerate(points):
x = p[0]
y = p[1]
z = p[2]
xindex = int(math.floor(x / gridX))
yindex = int(math.floor(y / gridY))
zindex = int(math.floor(z / gridZ))
key = (xindex, yindex, zindex)
if key not in dictionary:
dictionary[key] = []
else:
dictionary[key].append(i)
for key, value in dictionary.iteritems():
if len(value) >= num:
for j in value:
filtered.append(points[j])
return filtered
#def countNeighbors(self, points, radius):
# dictionary = defaultdict(int)
# for i in range(len(points)-1):
# for j in range(i+1,len(points)):
# pointA = points[i]
# pointB = points[j]
# if(vectorops.distance((pointA[0],pointA[1],pointA[2]),(pointB[0],pointB[1],pointB[2]))<radius):
# dictionary[i]+=1
# dictionary[j]+=1
# return dictionary
def idle(self):
if self.newPC != None:
#print "Setting the terrain geometry"
self.world.terrain(0).geometry().setPointCloud(self.newPC)
#print "Setting the collision margin"
self.world.terrain(0).geometry().setCollisionMargin(0.01)
self.newPC = None
#print "Done"
self.refresh()
class MyGLViewer(GLWidgetProgram):
def __init__(self, world, serviceThread):
GLWidgetProgram.__init__(self, "Manual poser")
self.world = world
self.serviceThread = serviceThread
for i in range(10):
q = self.serviceThread.qcmdGetter.get()
if q != None:
break
time.sleep(0.1)
if q == None:
print "Warning, system state service doesn't seem to have .robot.command.q"
print "Press enter to continue"
raw_input()
else:
world.robot(0).setConfig(q)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
def display(self):
#Put your display handler here
#the example draws the posed robot in grey, the sensed
#configuration in transparent red and the commanded
#configuration in transparent green
#this line will draw the world
GLWidgetProgram.display(self)
q = self.serviceThread.qsnsGetter.get()
if q:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
[1, 0, 0, 0.5])
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
q = self.serviceThread.qcmdGetter.get()
if q:
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#CONSTRUCT THE TASK HERE
msg = {}
msg['type'] = 'JointPose'
msg['limb'] = 'both'
msg['position'] = [
q[i] for i in self.left_arm_link_indices +
self.right_arm_link_indices
]
msg['speed'] = 1
msg['safe'] = 0
print "Sending message", msg
#SEND THE TASK TO THE SYSTEM STATE SERVICE
self.serviceThread.taskSetter.set(msg)
elif c == 'q':
self.serviceThread.kill()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
class MyGLViewer(GLWidgetPlugin):
def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = world.robot(0)
self.subrobots = []
for i in range(6):
self.subrobots.append(
SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
(res, val) = editors.run(
editors.SelectionEditor("subrobot",
self.subrobots[0]._links,
description="sub robot links",
world=world,
robot=self.robot))
if res:
print("Return value", val)
self.subrobots[0]._links = val
self.startConfig = self.robot.getConfig()
self.robotWidget = RobotPoser(world.robot(0))
self.robotWidget.setActiveDofs(self.subrobots[0]._links)
self.addWidget(self.robotWidget)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def __init__(self, world):
GLWidgetProgram.__init__(self, world, "My GL program")
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotWidget)
class MyGLViewer(GLWidgetProgram):
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
#start up the poser in the currently commanded configuration
q = motion.robot.getKlamptCommandedPosition()
world.robot(0).setConfig(q)
self.world = world
self.robot = world.robot(0)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
self.roadMap = ([], [])
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_names = left_arm_link_names
self.right_arm_link_names = right_arm_link_names
missing_left_arm_names = [
'left_upper_forearm_visual', 'left_upper_elbow_visual'
]
missing_right_arm_names = [
'right_upper_forearm_visual', 'right_upper_elbow_visual'
]
#self.left_arm_link_names+=missing_left_arm_names
#self.right_arm_link_names+=missing_right_arm_names
self.left_arm_link_names = [
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_upper_forearm_visual', 'left_gripper:base'
]
self.right_arm_link_names = [
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_upper_forearm_visual', 'right_gripper:base'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
self.collider = WorldCollider(self.world)
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line with draw the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
self.world.terrain(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
V, E = self.roadMap
positions = []
glDisable(GL_LIGHTING)
glColor3f(0, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
#print len(V)
#P = []
#for j in range(len(V)/2):
# P.append(V[j])
#for v in V:
# self.robot.setConfig(v)
# positions.append(self.robot.link("left_gripper:base").getTransform()[1])
#print "endEffector:", self.robot.getLink("left_gripper:base").getTransform()[1]
# glVertex3f(*self.robot.getLink("left_gripper:base").getTransform()[1])
#for (i,j) in E:
# glVertex3f(*positions[i])
# glVertex3f(*positions[j])
#glEnd()
#glEnable(GL_LIGHTING)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#print "space q:", q
q0 = motion.robot.getKlamptCommandedPosition()
#print q0
#collider = WorldCollider(self.world)
collisions = obstaclecollision(WorldCollider(self.world),
self.world.robot(0), q0, q)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
if not self.firstTimeHack and selfcollision(
self.world.robot(0), q0, q):
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
print
print
print "Moving", q0, "->", q
print
print
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
print "space prg:", qrg
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
elif c == 'p':
"""given a target object position, automatically moves the end effector close to the target object"""
#print "Joint limits:", self.robot.getJointLimits()
start = self.world.robot(0).getConfig()
#print "end effector original position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
#print "start", start
"""target position currently fixed at (0.8,0.1,1) for testing"""
target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
100, 0.1, 100)
if target == None:
# print "Cannot solve IK"
return
self.robot.setConfig(target)
#print "ik result:", target
#print "end effector position:", self.robot.link('left_gripper:base').getWorldPosition((0,0,0))
path = self.getCollisionFreePath(start, target, 10)
#print "path:", path
for q in path:
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
if qlg:
robot.left_gripper.command(qlg, [1] * len(qlg),
[1] * len(qlg))
if qrg:
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def subscribe(self):
rospy.init_node('subscribe', anonymous=True)
rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)
def callback(self, data):
if self.newPC == None:
data_out = pc2.read_points(data, skip_nans=False)
pc = PointCloud()
pc.propertyNames.append('rgba')
t0 = time.time()
points = list(data_out)
""" needs better external calibration"""
q = (0.564775, 0.425383, 0.426796, 0.563848)
R1 = so3.from_quaternion(q)
t1 = (0.283078, 0.043398, 1.01 - 0.00182539)
(Rinv, tinv) = se3.inv((R1, t1))
denoised = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 50)
print "Noise filtered in time", time.time() - t0
filtered = denoised
print "Robot arms filtered in time", time.time() - t0
counting = 0
for p in filtered:
if p == None:
counting = counting + 1
continue
coordinates = (p[0], p[1], p[2])
index = pc.addPoint(coordinates)
i = struct.unpack("i", struct.pack("f", p[3]))[0]
pc.setProperty(index, 0, i)
print "^^^^^^^^", counting
print "Point cloud copied to Klampt format in time", time.time(
) - t0
pc.transform(R1, t1)
print "Point cloud transformed in time", time.time() - t0
self.newPC = pc
else:
print "Not ready yet to receive new point cloud..."
# def statisticalfilter(self,points,k,mul):
# pclCloud = pcl.PointCloud()
# pclCloud.from_list(points)
# fil = pclCloud.make_statistical_outlier_filter()
# fil.set_mean_k(k)
# fil.set_std_dev_mul_thresh(mul)
# filtered = fil.filter().to_list()
# return filtered
# def radiusfilter(self,points,radius,num):
# filtered = []
# coordinates = []
# for p in points:
# coordinates.append([p[0],p[1],p[2]])
# tree = KDTree(np.array(coordinates))
# for i,p in enumerate(coordinates):
# if tree.query_radius(coordinates[i],r=radius,count_only=True)>=num:
# filtered.append(points[i])
# return points
def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
""" a simple voxelgrid filter to get rid of noise in point cloud data"""
dictionary = {}
for i, p in enumerate(points):
x = p[0]
y = p[1]
z = p[2]
if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31:
points[i] = None
continue
xindex = int(math.floor(x / gridX))
yindex = int(math.floor(y / gridY))
zindex = int(math.floor(z / gridZ))
key = (xindex, yindex, zindex)
if key not in dictionary:
dictionary[key] = []
dictionary[key].append(i)
for key, value in dictionary.iteritems():
if len(value) < num:
for j in value:
points[j] = None
return points
def robotSelfFilter(self, points, Rinv, tinv):
""" Get rid of points that belong to the robot's arms"""
robot = self.world.robot(0)
arm_link_names = self.left_arm_link_names + self.right_arm_link_names
bbMargin = 0.1
#upperThreshold = 0.1 # Need to tune
#lowerThreshold = -0.1 # Need to tune
armBBs = []
transformedG = []
maxArmDepth = 0
for i in arm_link_names:
currentT = robot.link(i).getTransform()
(newR, newt) = se3.mul((Rinv, tinv), currentT)
g = robot.link(i).geometry()
g.setCurrentTransform(newR, newt)
transformedG.append(g)
myBB = g.getBB()
if myBB[1][2] > maxArmDepth:
maxArmDepth = myBB[1][2]
expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin,
myBB[0][2] - bbMargin),
(myBB[1][0] + bbMargin, myBB[1][1] + bbMargin,
myBB[1][2] + bbMargin))
armBBs.append(expandedBB)
allIndices = self.indicesOfInterest(armBBs)
count = 0
self.count2 = 0
for j, indices in enumerate(allIndices):
if indices == None:
continue
for i in indices:
p = points[i]
if p == None:
continue
x = p[0]
y = p[1]
z = p[2]
if vectorops.norm((x, y, z)) > maxArmDepth + 0.3:
continue
points[i] = None
continue
#point = (x,y,z)
#if(z<1.2) and self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold):
# count = count + 1
# points[i] = None
return points
# def insideGeometryBB(self, point, bb, bbMargin):
# mins = vectorops.sub(bb[0],bbMargin)
# maxes = vectorops.sub(bb[1], -bbMargin)
# insideX = point[0]>=mins[0] and point[0]<=maxes[0]
# insideY = point[1]>=mins[1] and point[1]<=maxes[1]
# insideZ = point[2]>=mins[2] and point[2]<=maxes[2]
# return insideX and insideY and insideZ
# def insideGeometry(self, point, g, upperThreshold, lowerThreshold):
# direction = vectorops.unit(point)
# (hit, pt) = g.rayCast((0,0,0), direction)
# depthPoint = vectorops.norm(point)
# depthHit = vectorops.norm(pt)
# diff = depthPoint - depthHit
# if hit:
# self.count2 = self.count2 + 1
# if diff < upperThreshold and diff > lowerThreshold: # If point lies inside robot body
# return True
# return False
def indicesOfInterest(self, armBBs):
""" Get indices of point cloud data that belong to robot's arms"""
allIndices = []
for i, bb in enumerate(armBBs):
xs = (bb[0][0], bb[1][0])
ys = (bb[0][1], bb[1][1])
zs = (bb[0][2], bb[1][2])
allIndices.append(self.indicesOfSingleBB(xs, ys, zs))
return allIndices
def indicesOfSingleBB(self, xs, ys, zs):
""" Get indices of point cloud data that belong to the bounding box of a single arm link"""
indices = []
xminAngle = 180
xmaxAngle = -180
yminAngle = 180
ymaxAngle = -180
for x in xs:
for z in zs:
xangle = math.degrees(math.atan2(x, z)) + 35.3
if xangle < xminAngle:
xminAngle = xangle
if xangle > xmaxAngle:
xmaxAngle = xangle
if xminAngle > 70.6 or xmaxAngle < 0:
return None
if xminAngle < 0:
xminAngle = 0
if xmaxAngle > 70.6:
xmaxAngle = 70.6
for y in ys:
for z in zs:
yangle = math.degrees(math.atan2(y, z)) + 30
if yangle < yminAngle:
yminAngle = yangle
if yangle > ymaxAngle:
ymaxAngle = yangle
if yminAngle > 60 or ymaxAngle < 0:
return None
if yminAngle < 0:
yminAngle = 0
if ymaxAngle > 60:
ymaxAngle = 60
xminPixel = int(math.floor(512 * xminAngle / 70.6))
xmaxPixel = int(math.floor(512 * xmaxAngle / 70.6))
yminPixel = int(math.floor(424 * yminAngle / 60))
ymaxPixel = int(math.floor(424 * ymaxAngle / 60))
for i in range(yminPixel, ymaxPixel):
for j in range(xminPixel, xmaxPixel):
indices.append(512 * i + j)
return indices
def idle(self):
if self.newPC != None:
print "Setting the terrain geometry"
self.world.terrain(0).geometry().setPointCloud(self.newPC)
print "Setting the collision margin"
self.world.terrain(0).geometry().setCollisionMargin(0.01)
self.newPC = None
print "Done"
self.refresh()
def getCollisionFreePath(self, start, goal, iterations):
""" Given a start and a goal configuration, returns a collision-free path between the two configurations"""
""" Currently takes forever to find a path... Needs more work"""
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
planner = cspace.MotionPlan(space)
planner.setEndpoints(start, goal)
planner.planMore(iterations)
V, E = planner.getRoadmap()
self.roadMap = (V, E)
return planner.getPath()
def perturb(self, q, radius):
newq = []
for i in q:
newq.append(i + random.uniform(-radius, radius))
return newq
def getClosestConfig(self, robot, target, iterations, c, numsteps):
""" given a target object position, returns a configuration with end effector close to the target object but without colliding with it"""
cost = 9999
res = None
start = robot.getConfig()
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
for i in range(iterations):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
qmin, qmax = robot.getJointLimits()
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
q = robot.interpolate(end, start, u)
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
return res
class MyGLViewer(GLWidgetProgram):
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"Manual poser")
self.robotPoser = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotPoser)
motion.robot.left_limb.enableSelfCollisionAvoidance(False)
motion.robot.right_limb.enableSelfCollisionAvoidance(False)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.getLink(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.getLink(l).index for l in right_arm_link_names]
self.outputFileLeft = open("sag_calibration_left.csv","w")
self.outputFileRight = open("sag_calibration_right.csv","w")
#write headers
self.outputFileLeft.write("time,")
self.outputFileLeft.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileLeft.write(",")
self.outputFileLeft.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileLeft.write("\n")
self.outputFileRight.write("time,")
self.outputFileRight.write(",".join("qsns"+str(i) for i in range(1,8)))
self.outputFileRight.write(",")
self.outputFileRight.write(",".join("qcmd"+str(i) for i in range(1,8)))
self.outputFileRight.write("\n")
self.t0 = time.time()
self.lastLeftMove = -100
self.lastRightMove = -100
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line with draw the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def idle(self):
GLWidgetProgram.idle(self)
t = time.time()-self.t0
if motion.robot.left_mq.moving():
self.lastLeftMove = t
if motion.robot.right_mq.moving():
self.lastRightMove = t
if t < self.lastLeftMove + 3:
print "Saving left",t
qsensed = motion.robot.left_limb.sensedPosition()
qcmd = motion.robot.left_limb.commandedPosition()
self.outputFileLeft.write(str(t)+",")
self.outputFileLeft.write(','.join(str(v) for v in qsensed+qcmd))
self.outputFileLeft.write('\n')
if t < self.lastRightMove + 3:
print "Saving right",t
qsensed = motion.robot.right_limb.sensedPosition()
qcmd = motion.robot.right_limb.commandedPosition()
self.outputFileRight.write(str(t)+",")
self.outputFileRight.write(','.join(str(v) for v in qsensed+qcmd))
self.outputFileRight.write('\n')
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
self.refresh()
class MyGLViewer(GLWidgetProgram):
def __init__(self, world):
GLWidgetProgram.__init__(self, "Manual poser")
#start up the poser in the currently commanded configuration
q = motion.robot.getKlamptCommandedPosition()
world.robot(0).setConfig(q)
self.world = world
self.robot = world.robot(0)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
self.roadMap = ([], [])
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_names = left_arm_link_names
self.right_arm_link_names = right_arm_link_names
missing_left_arm_names = [
'left_upper_forearm_visual', 'left_upper_elbow_visual'
]
missing_right_arm_names = [
'right_upper_forearm_visual', 'right_upper_elbow_visual'
]
#self.left_arm_link_names+=missing_left_arm_names
#self.right_arm_link_names+=missing_right_arm_names
self.left_arm_link_names = [
'left_upper_forearm', 'left_lower_forearm', 'left_wrist',
'left_upper_forearm_visual', 'left_gripper:base'
]
self.right_arm_link_names = [
'right_upper_forearm', 'right_lower_forearm', 'right_wrist',
'right_upper_forearm_visual', 'right_gripper:base'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.firstTimeHack = True
self.world.makeTerrain('terrain')
self.subscribe()
self.newPC = None
self.collider = WorldCollider(self.world)
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line with draw the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
self.world.terrain(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
V, E = self.roadMap
positions = []
glDisable(GL_LIGHTING)
glColor3f(0, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
#print len(V)
#P = []
#for j in range(len(V)/2):
# P.append(V[j])
for v in V:
self.robot.setConfig(v)
# positions.append(self.robot.link("left_gripper:base").getTransform()[1])
#print "endEffector:", self.robot.getLink("left_gripper:base").getTransform()[1]
glVertex3f(
*self.robot.getLink("left_gripper:base").getTransform()[1])
#for (i,j) in E:
# glVertex3f(*positions[i])
# glVertex3f(*positions[j])
glEnd()
glEnable(GL_LIGHTING)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#print "space q:", q
q0 = motion.robot.getKlamptCommandedPosition()
#print q0
#collider = WorldCollider(self.world)
collisions = obstaclecollision(WorldCollider(self.world),
self.world.robot(0), q0, q)
for i in range(self.world.robot(0).numLinks()):
self.world.robot(0).link(i).appearance().setColor(
0.5, 0.5, 0.5, 1)
if not self.firstTimeHack and selfcollision(
self.world.robot(0), q0, q):
print "Invalid configuration, it self-collides"
elif not self.firstTimeHack and collisions != None:
#clear all links to gray
for pairs in collisions:
print "Link " + str(
pairs[0].getIndex()) + " collides with obstacle"
self.world.robot(0).link(
pairs[0].getIndex()).appearance().setColor(1, 0, 0, 1)
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
print
print
print "Moving", q0, "->", q
print
print
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
print "space prg:", qrg
robot.left_gripper.command(qlg, [1] * len(qlg), [1] * len(qlg))
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
elif c == 'p':
print "Joint limits:", self.robot.getJointLimits()
start = self.world.robot(0).getConfig()
print "end effector original position:", self.robot.link(
'left_gripper:base').getWorldPosition((0, 0, 0))
print "start", start
target = self.getClosestConfig(self.world.robot(0), (0.8, 0.1, 1),
100, 0.1, 100)
if target == None:
print "Cannot solve IK"
return
self.robot.setConfig(target)
print "ik result:", target
print "end effector position:", self.robot.link(
'left_gripper:base').getWorldPosition((0, 0, 0))
path = self.getCollisionFreePath(start, target, 10)
print "path:", path
#q = path[1]
#robot = motion.robot
#robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
#robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
# #print
# #print
# #print "Moving",q0,"->",q
# #print
# #print
#qlg = robot.left_gripper.configFromKlampt(q)
# #print "q is:", q
#qrg = robot.right_gripper.configFromKlampt(q)
# #print qrg
#if qlg:
# robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
#if qrg:
# robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
for q in path:
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
#print
#print
#print "Moving",q0,"->",q
#print
#print
qlg = robot.left_gripper.configFromKlampt(q)
#print "q is:", q
qrg = robot.right_gripper.configFromKlampt(q)
#print qrg
if qlg:
robot.left_gripper.command(qlg, [1] * len(qlg),
[1] * len(qlg))
if qrg:
robot.right_gripper.command(qrg, [1] * len(qrg),
[1] * len(qrg))
print "getKlamptCommandedPosition:", robot.getKlamptCommandedPosition(
)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def subscribe(self):
rospy.init_node('subscribe', anonymous=True)
rospy.Subscriber("/kinect2/sd/points", PointCloud2, self.callback)
def callback(self, data):
if self.newPC == None:
data_out = pc2.read_points(data, skip_nans=False)
#data_out = pc2.read_points(data)
#print data.width, data.height
pc = PointCloud()
pc.propertyNames.append('rgba')
t0 = time.time()
points = list(data_out)
#print points
#sys.exit(0)
#print len(points)
#print "converted to list in time", time.time()-t0
q = (0.564775, 0.425383, 0.426796, 0.563848)
R1 = so3.from_quaternion(q)
##t1 = (0.24539, 0.0893425, 1.00112145)
#t1 = (0.24539, -0.05, 1.00112145)
####t1 = (0.273078, 0.0893398, 1.0-0.00182539)
t1 = (0.283078, 0.043398, 1.01 - 0.00182539)
#filtered = self.radiusfilter(points,0.1,0)
(Rinv, tinv) = se3.inv((R1, t1))
#for p in points:
# print p
#tm = se3.apply((R1,t1), p)
#t = se3.apply((Rinv, tinv), tm)
#print t
#self.xmin = 100
#self.xmax = 0
#self.ymin = 100
#self.ymax = 0
denoised = self.voxelgridfilter(points, 0.1, 0.1, 0.1, 50)
print "Noise filtered in time", time.time() - t0
#filtered = points
filtered = denoised
#print self.xmin, self.xmax, self.ymin, self.ymax
#filtered = self.robotSelfFilter(denoised, Rinv, tinv)
#print len(denoised)-len(filtered)
print "Robot arms filtered in time", time.time() - t0
#print len(filtered)
counting = 0
for p in filtered:
if p == None:
counting = counting + 1
continue
coordinates = (p[0], p[1], p[2])
#if vectorops.norm(coordinates)>0.31:
index = pc.addPoint(coordinates)
i = struct.unpack("i", struct.pack("f", p[3]))[0]
pc.setProperty(index, 0, i)
print "^^^^^^^^", counting
print "Point cloud copied to Klampt format in time", time.time(
) - t0
#q = (0.566222,0.423455,0.424871,0.5653)
#q = (0.564775, 0.425383, 0.426796, 0.563848)
#R1 = so3.from_quaternion(q)
#t1 = (0.228665,0.0591513,0.0977748)
#t1 = (0.24539, 0.0893425, 0.00112145)
#t1 = (0.24539, 0.0893425, 1.00112145)
#T1 = (R1, t1)
#R2 = so3.rotation((0,0,1),math.pi/2)
#t2 = (0,0,1)
#T2 = (R2, t2)
#T = se3.mul(T2,T1)
#(R,t)=T
#pc.transform(R,t)
pc.transform(R1, t1)
print "Point cloud transformed in time", time.time() - t0
self.newPC = pc
else:
print "Not ready yet to receive new point cloud..."
# Color data is lost
def statisticalfilter(self, points, k, mul):
pclCloud = pcl.PointCloud()
pclCloud.from_list(points)
fil = pclCloud.make_statistical_outlier_filter()
fil.set_mean_k(k)
fil.set_std_dev_mul_thresh(mul)
filtered = fil.filter().to_list()
return filtered
def radiusfilter(self, points, radius, num):
filtered = []
coordinates = []
for p in points:
coordinates.append([p[0], p[1], p[2]])
#print np.array(coordinates).reshape(-len(coordinates),3)
tree = KDTree(np.array(coordinates))
#tree = KDTree(np.random.random((10, 3)), leaf_size=2)
for i, p in enumerate(coordinates):
#print tree.query_radius(p,r=radius,count_only=True)
if tree.query_radius(coordinates[i], r=radius,
count_only=True) >= num:
filtered.append(points[i])
return points
def voxelgridfilter(self, points, gridX, gridY, gridZ, num):
#filtered = []
dictionary = {}
for i, p in enumerate(points):
x = p[0]
y = p[1]
z = p[2]
#xangle = math.degrees(math.atan2(x,z))+35.3
#yangle = math.degrees(math.atan2(y,z)) + 30
#if xangle>self.xmax:
# self.xmax = xangle
#if xangle<self.xmin:
# self.xmin = xangle
#if yangle>self.ymax:
# self.ymax = yangle
#if yangle<self.ymin:
# self.ymin = yangle
if math.isnan(x) or vectorops.norm((x, y, z)) < 0.31:
points[i] = None
continue
xindex = int(math.floor(x / gridX))
yindex = int(math.floor(y / gridY))
zindex = int(math.floor(z / gridZ))
key = (xindex, yindex, zindex)
if key not in dictionary:
dictionary[key] = []
dictionary[key].append(i)
for key, value in dictionary.iteritems():
#print value
if len(value) < num:
for j in value:
points[j] = None
#print points[j]
return points
def robotSelfFilter(self, points, Rinv, tinv):
#t1 = time.time()
#filtered = []
robot = self.world.robot(0)
#camPos = (0.24539, 0.0893425, 0.00112145) # Not needed anymore in camera frame
arm_link_names = self.left_arm_link_names + self.right_arm_link_names
bbMargin = 0.1
upperThreshold = 0.1 # Need to tune
lowerThreshold = -0.1 # Need to tune
armBBs = []
transformedG = []
#testPoint = (0.26839444608864566, -0.6676082722078356, 1.2409759988857147)
#testPoint = (0.6351629925287235, -0.8301655827368236, 1.2909760000026105)
#print se3.apply((Rinv, tinv), testPoint)
maxArmDepth = 0
for i in arm_link_names:
#robot.link(i).appearance().setColor(0,0,1,1)
currentT = robot.link(i).getTransform()
#pt = se3.apply(currentT, (0,0,0.1))
#print se3.apply((Rinv, tinv), pt)
#print currentT
(newR, newt) = se3.mul((Rinv, tinv), currentT)
#print se3.apply((newR,newt), (0,0,0.1))
g = robot.link(i).geometry()
g.setCurrentTransform(newR, newt)
#g.transform(Rinv, tinv)
transformedG.append(g)
myBB = g.getBB()
if myBB[1][2] > maxArmDepth:
maxArmDepth = myBB[1][2]
expandedBB = ((myBB[0][0] - bbMargin, myBB[0][1] - bbMargin,
myBB[0][2] - bbMargin),
(myBB[1][0] + bbMargin, myBB[1][1] + bbMargin,
myBB[1][2] + bbMargin))
armBBs.append(expandedBB)
#robot.link(arm_link_names[10]).appearance().setColor(0,0,1,1)
#print armBBs[10]
#print "Arm geometry transformed in time: ",time.time()-t1
allIndices = self.indicesOfInterest(armBBs)
#for i, p in enumerate(points):
#print points[15000]
count = 0
self.count2 = 0
for j, indices in enumerate(allIndices):
#print indices[len(indices)-1]
if indices == None:
continue
#print len(indices)
for i in indices:
#print points[i][3]
#if points[i] !=None:
# newpoint = (points[i][0], points[i][1], points[i][2], -1.7014118346e+38)
# points[i] = newpoint
#continue
p = points[i]
if p == None:
continue
x = p[0]
y = p[1]
z = p[2]
if vectorops.norm((x, y, z)) > maxArmDepth + 0.3:
continue
points[i] = None
continue
point = (x, y, z)
#discard = False
#for j,bb in enumerate(armBBs):
#if(z<1.2):
#g = robot.link(j).geometry()
#g.transform(Rinv, tinv) # geometry transformed to camera frame
#if(self.insideGeometryBB(point,bb,bbMargin)):
# if(self.insideGeometry(point,transformedG[j],upperThreshold,lowerThreshold)):
# discard = True
#break
#pass
#if discard:
# points[i] = None
#print "**********************"
# count2 = count2+1
if (z < 1.2) and self.insideGeometry(point, transformedG[j],
upperThreshold,
lowerThreshold):
#print "detected"
count = count + 1
points[i] = None
print "detected: ", count
print "hits: ", self.count2
return points
def insideGeometryBB(self, point, bb, bbMargin):
#t1 = time.time()
mins = vectorops.sub(bb[0], bbMargin)
maxes = vectorops.sub(bb[1], -bbMargin)
#print "bounding box checked first two lines in time: ",time.time()-t1
insideX = point[0] >= mins[0] and point[0] <= maxes[0]
insideY = point[1] >= mins[1] and point[1] <= maxes[1]
insideZ = point[2] >= mins[2] and point[2] <= maxes[2]
#print "bounding box checked in time: ",time.time()-t1
#print maxes[0], maxes[1]
#print "point:", point[0], point[1]
return insideX and insideY and insideZ
def insideGeometry(self, point, g, upperThreshold, lowerThreshold):
#t1 = time.time()
#print "using rayCast"
direction = vectorops.unit(point)
#print g.getBB()[0][0]
(hit, pt) = g.rayCast((0, 0, 0), direction)
depthPoint = vectorops.norm(point)
depthHit = vectorops.norm(pt)
diff = depthPoint - depthHit
#print "rayCast done in: ",time.time()-t1
#self.count2 = self.count2 + 1
if hit:
#print "hit!!!!!!"
self.count2 = self.count2 + 1
if diff < upperThreshold and diff > lowerThreshold: # If point lies inside robot body
return True
return False
def indicesOfInterest(self, armBBs):
allIndices = []
for i, bb in enumerate(armBBs):
#if i<len(self.left_arm_link_names):
# print self.left_arm_link_names[i]
xs = (bb[0][0], bb[1][0])
ys = (bb[0][1], bb[1][1])
zs = (bb[0][2], bb[1][2])
allIndices.append(self.indicesOfSingleBB(xs, ys, zs))
#print allIndices
return allIndices
def indicesOfSingleBB(self, xs, ys, zs):
#print xs, ys, zs
indices = []
xminAngle = 180
xmaxAngle = -180
yminAngle = 180
ymaxAngle = -180
for x in xs:
for z in zs:
xangle = math.degrees(math.atan2(x, z)) + 35.3
if xangle < xminAngle:
xminAngle = xangle
if xangle > xmaxAngle:
xmaxAngle = xangle
#print "*********x: ", xminAngle, xmaxAngle
if xminAngle > 70.6 or xmaxAngle < 0:
return None
if xminAngle < 0:
xminAngle = 0
if xmaxAngle > 70.6:
xmaxAngle = 70.6
for y in ys:
for z in zs:
yangle = math.degrees(math.atan2(y, z)) + 30
if yangle < yminAngle:
yminAngle = yangle
if yangle > ymaxAngle:
ymaxAngle = yangle
#print "*********y: ", yminAngle, ymaxAngle
#print "*********y coordiantes: ", ys
if yminAngle > 60 or ymaxAngle < 0:
return None
if yminAngle < 0:
yminAngle = 0
if ymaxAngle > 60:
ymaxAngle = 60
#print "*********",xminAngle, xmaxAngle, yminAngle, ymaxAngle
#print "******* lalalala"
xminPixel = int(math.floor(512 * xminAngle / 70.6))
xmaxPixel = int(math.floor(512 * xmaxAngle / 70.6))
yminPixel = int(math.floor(424 * yminAngle / 60))
ymaxPixel = int(math.floor(424 * ymaxAngle / 60))
#print "*********", xminPixel, xmaxPixel, yminPixel, ymaxPixel
for i in range(yminPixel, ymaxPixel):
for j in range(xminPixel, xmaxPixel):
indices.append(512 * i + j)
#print "******** indices: ", indices[len(indices)-1]
return indices
#def countNeighbors(self, points, radius):
# dictionary = defaultdict(int)
# for i in range(len(points)-1):
# for j in range(i+1,len(points)):
# pointA = points[i]
# pointB = points[j]
# if(vectorops.distance((pointA[0],pointA[1],pointA[2]),(pointB[0],pointB[1],pointB[2]))<radius):
# dictionary[i]+=1
# dictionary[j]+=1
# return dictionary
def idle(self):
if self.newPC != None:
print "Setting the terrain geometry"
self.world.terrain(0).geometry().setPointCloud(self.newPC)
print "Setting the collision margin"
self.world.terrain(0).geometry().setCollisionMargin(0.01)
self.newPC = None
print "Done"
self.refresh()
#def calibrate(self, point):
# q = (0.566222,0.423455,0.424871,0.5653)
# R = so3.from_quaternion(q)
# t = (0.228665,0.0591513,0.0977748)
# T = (R, t)
# newPoint = se3.apply(T,point)
# x = newPoint[0]
# y = newPoint[1]
# z = newPoint[2]
# return (-y,x,z)
def getCollisionFreePath(self, start, goal, iterations):
#MotionPlan.setOptions(type="rrt", perturbationRadius=2.0, bidirectional=True)
#MotionPlan.setOptions(type="prm", knn=10, connectionThreshold=0.25)
MotionPlan.setOptions(type="sbl",
perturbationRadius=2.0,
connectionThreshold=0.5,
bidirectional=True)
#MotionPlan.setOptions(type="lazyrrg*")
#space = ObstacleCSpace(self.collider, self.robot)
#planner = MotionPlan(space)
#planner = robotplanning.planToConfig(self.world, self.robot, goal, type="prm", knn=10, connectionThreshold=0.1)
print "milestone 1"
space = robotcspace.RobotCSpace(self.robot, WorldCollider(self.world))
jointLimits = self.robot.getJointLimits()
lower = jointLimits[0]
higher = jointLimits[1]
for i in range(12):
lower[i] = 0
higher[i] = 0
newLimits = (lower, higher)
space.bound = zip(*newLimits)
print "milestone 2"
planner = cspace.MotionPlan(space)
print "milestone 3"
planner.setEndpoints(start, goal)
print "before planning"
planner.planMore(iterations)
print "after planning"
V, E = planner.getRoadmap()
self.roadMap = (V, E)
print "No. of vertices:", len(V)
print "Vertices:", V
print "Edges:", E
return planner.getPath()
def perturb(self, q, radius):
newq = []
for i in q:
newq.append(i + random.uniform(-radius, radius))
return newq
def getClosestConfig(self, robot, target, iterations, c, numsteps):
cost = 9999
res = None
start = robot.getConfig()
#goal = ik.objective(robot.link("left_gripper:base"), local = [(0,0,0)], world = [target])
#goal = ik.objective(robot.link("left_wrist"), local = [(0,0,0)], world = [target])
s = IKSolver(robot)
objective = IKObjective()
objective.setFixedPoint(
robot.link("left_gripper:base").getIndex(), (0, 0, 0), target)
s.add(objective)
s.setActiveDofs([12, 13, 14, 15, 16, 18, 19])
print "Active DOFs:", s.getActiveDofs()
for i in range(iterations):
#if ik.solve(goal):
(ret, iters) = s.solve(100, 1e-4)
if ret:
end = robot.getConfig()
print "*****************************", end
qmin, qmax = robot.getJointLimits()
#print qmin
#print qmax
flag = False
for k in range(len(end)):
if end[k] < qmin[k] or end[k] > qmax[k]:
flag = True
break
if flag:
start = self.perturb(start, 0.1)
robot.setConfig(start)
continue
for j in xrange(numsteps + 1):
u = float(j) / numsteps
print "u is:", u
q = robot.interpolate(end, start, u)
#q = end
print "interpolated q is:", q
if not inCollision(WorldCollider(self.world), robot, q):
newcost = vectorops.distance(
q, end) + c * vectorops.distance(q, start)
if newcost < cost:
res = q
cost = newcost
break
start = self.perturb(start, 0.1)
robot.setConfig(start)
print "res is:", res
return res
class MyGLViewer(GLWidgetProgram):
def __init__(self,world):
GLWidgetProgram.__init__(self,"Manual poser")
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = ['left_upper_shoulder','left_lower_shoulder','left_upper_elbow','left_lower_elbow','left_upper_forearm','left_lower_forearm','left_wrist']
right_arm_link_names = ['right_upper_shoulder','right_lower_shoulder','right_upper_elbow','right_lower_elbow','right_upper_forearm','right_lower_forearm','right_wrist']
self.left_arm_link_indices = [robot.link(l).index for l in left_arm_link_names]
self.right_arm_link_indices = [robot.link(l).index for l in right_arm_link_names]
self.firstTimeHack = True
def display(self):
#Put your display handler here
#the current example draws the sensed robot in grey and the
#commanded configurations in transparent green
#this line with draw the world
robot = motion.robot
q = robot.getKlamptSensedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
GLWidgetProgram.display(self)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
q = robot.getKlamptCommandedPosition()
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
q0 = motion.robot.getKlamptCommandedPosition()
if not self.firstTimeHack and selfcollision(self.world.robot(0),q0,q):
print "Invalid configuration, it self-collides"
else:
self.firstTimeHack = False
robot = motion.robot
robot.left_mq.setRamp(robot.left_limb.configFromKlampt(q))
robot.right_mq.setRamp(robot.right_limb.configFromKlampt(q))
qlg = robot.left_gripper.configFromKlampt(q)
qrg = robot.right_gripper.configFromKlampt(q)
robot.left_gripper.command(qlg,[1]*len(qlg),[1]*len(qlg))
robot.right_gripper.command(qrg,[1]*len(qrg),[1]*len(qrg))
#robot.left_mq.setRamp([q[i] for i in self.left_arm_link_indices])
#robot.right_mq.setRamp([q[i] for i in self.right_arm_link_indices])
elif c == 'q':
motion.robot.shutdown()
exit(0)
else:
GLWidgetProgram.keyboardfunc(self,c,x,y)
self.refresh()
class CustomGLViewer(GLRealtimeProgram):
def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):
if simworld == None and planworld == None:
# Dummy GLViewer
print "simworld and planworld are None"
return
GLRealtimeProgram.__init__(self, "My GL program")
self.simworld = simworld
self.planworld = planworld
self.sim = sim
self.widgetMaster = WidgetSet()
self.widgetButton = 2 # right-clicks
self.draggingWidget = False
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(self.simworld.robot(0))
self.widgetMaster.add(self.robotWidget)
self.simulate = True
if self.sim == None:
self.simulate = False
self.controller = controller
self.command_queue = Queue()
if helper != None:
print "helper is not none"
helper.run()
self.points = None
self.extraControllers = {}
def idle(self):
if self.extraControllers != {}:
remove_indices = []
for controller in self.extraControllers:
self.dt = 0.01
controller.sim.simulate(self.dt)
glutPostRedisplay()
if controller.remainingTime() <= 0:
remove_indices.append(controller)
for controller in remove_indices:
del self.extraControllers[controller]
print "controller removed"
if self.simulate:
self.dt = 0.01
self.sim.simulate(self.dt)
glutPostRedisplay()
self.widgetMaster.idle()
if self.widgetMaster.wantsRedraw():
print "widgetMaster wants redraw"
self.refresh()
def printStuff(self):
print "shelf xform:", knowledge.shelf_xform
print self.simworld.terrain(0).geometry()
# print "terrain xform:", self.simworld.terrain(0).geometry().getTransform(), "\n\n"
def updatePoints(self, points):
self.points = points
def addController(self, new_controller, rgb):
self.extraControllers[new_controller] = rgb
def glShowPointCloud(self, pc, downsample_rate=5, pt_size=None):
# print glGetFloatv(GL_CURRENT_COLOR)
pc = np.array(pc)
# print 'Rendering %d points'%pc.shape[0]
try:
d = pc.shape[1]
assert d == 3 or d == 4, "Unrecognized point cloud shape: " + str(pc.shape)
pc = pc[::downsample_rate, :]
pc = pc.tolist()
glDisable(GL_LIGHTING)
glColor3f(0, 1, 0)
if pt_size is not None:
glPointSize(pt_size)
glBegin(GL_POINTS)
for p in pc:
if d == 4:
r, g, b = pcl_float_to_rgb(p[3])
r /= 255.0
g /= 255.0
b /= 255.0
glColor3f(r, g, b)
glVertex3f(p[0], p[1], p[2])
glEnd()
glEnable(GL_LIGHTING)
glColor3f(1, 0, 0)
except:
# print 'Insufficient data'
pass
def updateBox(self, index, vertices):
BOX_COORDS[index] = vertices
def display(self):
global BOX_COORDS
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [1, 0, 0, 0.5])
# only 1 robot in this case, but still use for-loop for generality
for i in xrange(self.simworld.numRobots()):
r = self.controller.robotModel
q = self.controller.getCommandedConfig()
if self.widgetMaster.wantsRedraw():
# restart sim - start it with the new robot config
# if simulating
if self.sim != None:
# put more checks on mouse function here
# might accidentally reset if you mouse over
q = self.robotWidget.get()
# print 'self.sim is not None'
self.controller.robotModel.setConfig(q)
self.simworld.robot(0).setConfig(q)
self.controller.setLinear(q, 0.01)
else:
self.robotWidget.set(q)
# q = self.controller.getSensedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
for controller in self.extraControllers:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, self.extraControllers[controller])
r = controller.robotModel
q = controller.getCommandedConfig()
r.setConfig(q)
r.drawGL(False)
glDisable(GL_BLEND)
for i in xrange(self.simworld.numTerrains()):
self.simworld.terrain(i).drawGL()
if self.points != None:
self.glShowPointCloud(self.points)
for i in BOX_COORDS:
self.draw_wire_box(*BOX_COORDS[i])
self.widgetMaster.drawGL(self.viewport())
def draw_wire_box(self, bmin, bmax):
"""Helper: draws a wireframe box"""
glBegin(GL_LINE_LOOP)
glVertex3f(bmin[0], bmin[1], bmin[2])
glVertex3f(bmin[0], bmin[1], bmax[2])
glVertex3f(bmin[0], bmax[1], bmax[2])
glVertex3f(bmin[0], bmax[1], bmin[2])
glEnd()
glBegin(GL_LINE_LOOP)
glVertex3f(bmax[0], bmin[1], bmin[2])
glVertex3f(bmax[0], bmin[1], bmax[2])
glVertex3f(bmax[0], bmax[1], bmax[2])
glVertex3f(bmax[0], bmax[1], bmin[2])
glEnd()
glBegin(GL_LINES)
glVertex3f(bmin[0], bmin[1], bmin[2])
glVertex3f(bmax[0], bmin[1], bmin[2])
glVertex3f(bmin[0], bmin[1], bmax[2])
glVertex3f(bmax[0], bmin[1], bmax[2])
glVertex3f(bmin[0], bmax[1], bmax[2])
glVertex3f(bmax[0], bmax[1], bmax[2])
glVertex3f(bmin[0], bmax[1], bmin[2])
glVertex3f(bmax[0], bmax[1], bmin[2])
glEnd()
def keyboardfunc(self, c, x, y):
# print c,"pressed"
# print 'int value =', ord(c)
self.widgetMaster.keypress(c)
self.refresh()
if c == "`":
self.simulate = not self.simulate
print "Simulating:", self.simulate
else:
self.command_queue.put(c)
# print int(c)
if c == chr(27):
# c == esc
# self.picking_thread.join()
exit(0)
glutPostRedisplay()
def mousefunc(self, button, state, x, y):
# print "mouse",button,state,x,y
if button == self.widgetButton:
if state == 0:
# print 'state was zero'
if self.widgetMaster.beginDrag(x, self.height - y, self.viewport()):
# print 'beginning drag'
self.draggingWidget = True
else:
# print 'state of button not zero'
if self.draggingWidget:
# print 'dragging widget true - ending drag'
self.widgetMaster.endDrag()
self.draggingWidget = False
if self.widgetMaster.wantsRedraw():
self.refresh()
self.lastx, self.lasty = x, y
return
GLRealtimeProgram.mousefunc(self, button, state, x, y)
def motionfunc(self, x, y, dx, dy):
if self.draggingWidget:
self.widgetMaster.drag(dx, -dy, self.viewport())
if self.widgetMaster.wantsRedraw():
self.refresh()
else:
res = self.widgetMaster.hover(x, self.height - y, self.viewport())
if self.widgetMaster.wantsRedraw():
self.refresh()
GLRealtimeProgram.motionfunc(self, x, y, dx, dy)
def specialfunc(self, c, x, y):
# Put your keyboard special character handler here
# print c,"pressed"
self.widgetMaster.keypress(c)
self.refresh()
class MyGLViewer(GLWidgetProgram):
def __init__(self, world, serviceThread):
GLWidgetProgram.__init__(self, "Manual poser")
self.world = world
self.serviceThread = serviceThread
self.oldTime = 0.0
for i in range(10):
q = self.serviceThread.qcmdGetter.get()
if q != None:
break
time.sleep(0.1)
if q == None:
print "Warning, system state service doesn't seem to have .robot.command.q"
print "Press enter to continue"
raw_input()
else:
world.robot(0).setConfig(q)
self.robotPoser = RobotPoser(world.robot(0))
self.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
for i in range(10):
ee1 = self.serviceThread.ee1Getter.get()
ee2 = self.serviceThread.ee1Getter.get()
if ee1 != None and ee2 != None:
break
time.sleep(0.1)
if ee1 == None:
print "Warning, system state service doesn't seem to have .robot.endEffectors.0.xform.sensed"
print "Press enter to continue"
raw_input()
if ee2 == None:
print "Warning, system state service doesn't seem to have .robot.endEffectors.1.xform.sensed"
print "Press enter to continue"
raw_input()
def display(self):
#Put your display handler here
#the example draws the posed robot in grey, the sensed
#configuration in transparent red and the commanded
#configuration in transparent green
#this line will draw the world
GLWidgetProgram.display(self)
q = self.serviceThread.qsnsGetter.get()
ee1 = self.serviceThread.ee1Getter.get()
ee2 = self.serviceThread.ee1Getter.get()
# print "End Effector 1 = ", ee1
if q:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
[1, 0, 0, 0.5])
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
#draw commanded configuration
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [0, 1, 0, 0.5])
q = self.serviceThread.qcmdGetter.get()
if q:
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
self.getMsg()
def getMsg(self):
deviceState = self.serviceThread.updater.deviceState
ee1 = self.serviceThread.ee1Getter.get()
ee2 = self.serviceThread.ee1Getter.get()
ee = [ee1, ee2]
haptic_task_type = "cartesian"
if haptic_task_type == "cartesian":
msg = self.renderCartesianVelocityMsg(deviceState)
elif haptic_task_type == "multiIK":
msg = self.renderMultiIKMsg(deviceState, ee)
# === Generate cartesian velocity task ===
for i, dstate in enumerate(deviceState):
if dstate['buttonDown']:
if dstate['newupdate']:
if haptic_task_type == "cartesian":
print "send cartesian control msg: ", msg
elif haptic_task_type == "multiIK":
print "send Multi-IK control msg: ", msg
self.serviceThread.taskSetter.set(msg)
# === Generate Multi-IK task ===
def renderCartesianVelocityMsg(self, deviceState):
"""
Prepare a task message for Controller dispatcher - Send a multiIK Task
"""
msg = {}
msg['type'] = 'CartesianVelocity'
msg['limb'] = 'both'
# msg['speed'] = 1
msg['linear'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg['angular'] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
msg['safe'] = 0
if len(deviceState) == 0: return
for i, dstate in enumerate(deviceState):
if dstate['buttonDown']:
if dstate['newupdate']:
msg['linear'][0 + i * 3:3 + i * 3] = dstate['linearVel']
msg['angular'][0 + i * 3:3 + i * 3] = dstate['angularVel']
return msg
def renderMultiIKMsg(self, deviceState, ee):
"""
Prepare a task message for Controller dispatcher - Send a multiIK Task
"""
msg = {}
msg['components'] = []
if len(deviceState) == 0: return
for i, dstate in enumerate(deviceState):
if dstate['buttonDown']:
if dstate['newupdate']:
ikgoal = {}
ikgoal['link'] = endEffectorIndices[i]
Rlocal, tlocal = endEffectorLocalTransforms[i]
ikgoal['localPosition'] = tlocal
ikgoal['endPosition'] = [
dstate['moveToTransform'][1][k] + ee[i][1][k]
for k in range(0, 3)
]
msg['components'].append(ikgoal)
return msg
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
if c == 'h':
print '[space]: send the current posed milestone'
print 'q: clean quit'
elif c == ' ':
q = self.robotPoser.get()
#CONSTRUCT THE TASK HERE
msg = {}
msg['type'] = 'JointPose'
msg['limb'] = 'both'
msg['position'] = [
q[i] for i in self.left_arm_link_indices +
self.right_arm_link_indices
]
msg['speed'] = 1
msg['safe'] = 0
print "Sending message", msg
#SEND THE TASK TO THE SYSTEM STATE SERVICE
self.serviceThread.taskSetter.set(msg)
elif c == 'q':
self.serviceThread.kill()
exit(0)
elif c == 'i':
ql = [0.25, -0.5, -0.5, 1.5, -0.25, 1.0, 0.5]
qr = [-0.25, -0.5, 0.5, 1.5, 0.25, 1.0, 0.5]
msg = {}
msg['type'] = 'JointPose'
msg['limb'] = 'both'
msg['position'] = ql + qr
msg['speed'] = 1
msg['safe'] = 0
print "Sending message", msg
self.serviceThread.taskSetter.set(msg)
else:
GLWidgetProgram.keyboardfunc(self, c, x, y)
self.refresh()
def __init__(self,world):
GLWidgetProgram.__init__(self,world,"My GL program")
self.poseWidget = PointPoser()
self.widgetMaster.add(self.poseWidget)
self.robotWidget = RobotPoser(world.robot(0))
self.widgetMaster.add(self.robotWidget)
class MousePoserTaskGenerator(TaskGenerator):
"""Allows the user to interact with the model by right clicking
using the mouse and pressing space bar to send the milestone.
Also demonstrates how to write a Python plugin task with an OpenGL
widget.
"""
def __init__(self):
self.plugin = None
def name(self):
return "MousePoser"
def init(self, world):
self.world = world
self.robotPoser = RobotPoser(world.robot(0))
self.plugin = MyWidgetPlugin(self)
self.plugin.addWidget(self.robotPoser)
robot = world.robot(0)
left_arm_link_names = [
'left_upper_shoulder', 'left_lower_shoulder', 'left_upper_elbow',
'left_lower_elbow', 'left_upper_forearm', 'left_lower_forearm',
'left_wrist'
]
right_arm_link_names = [
'right_upper_shoulder', 'right_lower_shoulder',
'right_upper_elbow', 'right_lower_elbow', 'right_upper_forearm',
'right_lower_forearm', 'right_wrist'
]
base_link_names = ['base_x', 'base_y', 'base_yaw']
self.left_arm_link_indices = [
robot.link(l).index for l in left_arm_link_names
]
self.right_arm_link_indices = [
robot.link(l).index for l in right_arm_link_names
]
self.base_link_indices = [robot.link(l).index for l in base_link_names]
if any(v < 0 for v in self.left_arm_link_indices +
self.right_arm_link_indices):
raise ValueError("Robot in world does not have Baxter's torso?")
return True
def start(self):
self._status = 'ok'
self.robotPoser.set(self.world.robot(0).getConfig())
return True
def stop(self):
pass
def get(self):
if self.plugin.initialPose:
self.plugin.initialPose = False
q = self.robotPoser.get()
for i in range(len(self.left_arm_link_indices)):
idx = self.left_arm_link_indices[i]
q[idx] = q_init_left[i]
for i in range(len(self.right_arm_link_indices)):
idx = self.right_arm_link_indices[i]
q[idx] = q_init_right[i]
self.robotPoser.set(q)
print "set up to initial pose"
if self.plugin.sendMilestone:
self.plugin.sendMilestone = False
q = self.robotPoser.get()
qcmd = self.world.robot(0).getConfig()
baseMoved = any(qcmd[i] != q[i] for i in self.base_link_indices)
leftArmMoved = any(qcmd[i] != q[i]
for i in self.left_arm_link_indices)
rightArmMoved = any(qcmd[i] != q[i]
for i in self.right_arm_link_indices)
moved = []
if baseMoved: moved.append('base')
if leftArmMoved: moved.append('left')
if rightArmMoved: moved.append('right')
targets = []
if baseMoved:
targets.append([q[i] for i in self.base_link_indices])
if leftArmMoved:
targets.append([q[i] for i in self.left_arm_link_indices])
if rightArmMoved:
targets.append([q[i] for i in self.right_arm_link_indices])
print "Sending milestone", q, ", moving parts:", ",".join(moved)
#CONSTRUCT THE TASK HERE
msg = {}
msg['type'] = 'JointPose'
msg['parts'] = moved
msg['positions'] = targets
msg['speed'] = 1
#TEST: only allow safe configurations (self collision checking)
msg['safe'] = 1
return msg
return None
def glPlugin(self):
return self.plugin
