def __init__(self):
GLPluginInterface.__init__(self)
robot.randomizeConfig()
#sensor.kinematicSimulate(world,0.01)
sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7)
self.rgb=None
self.depth=None
self.compute_pc = False
self.pc = None
self.pc_appearance = None
self.original_view = None
def randomize():
robot.randomizeConfig()
sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7)
def plot_rgb():
self.rgb,self.depth = processDepthSensor(sensor)
if self.rgb is not None:
plt.imshow(self.rgb)
plt.show()
def plot_depth():
self.rgb,self.depth = processDepthSensor(sensor)
if self.depth is not None:
plt.imshow(self.depth)
plt.show()
def toggle_point_cloud():
self.compute_pc = not self.compute_pc
def save_point_cloud():
if self.pc != None:
if isinstance(self.pc,klampt.Geometry3D):
print("Saving to sensortest_temp.pcd")
self.pc.saveFile("sensortest_temp.pcd")
else:
print("Saving to sensortest_temp.csv")
import numpy
numpy.savetxt("sensortest_temp.csv",self.pc)
input("Saved...")
def toggle_view():
if self.original_view is None:
self.original_view = self.view
v = sensing.camera_to_viewport(sensor,robot)
self.window.program.set_view(v)
self.view = v
else:
self.window.program.set_view(self.original_view)
self.view = self.original_view
self.original_view = None
def print_view():
print("Tgt",self.view.camera.tgt)
print("Rot",self.view.camera.rot)
self.add_action(randomize,'Randomize configuration',' ')
if HAVE_PYPLOT:
self.add_action(plot_rgb,'Plot color','c')
self.add_action(plot_depth,'Plot depth','d')
self.add_action(toggle_point_cloud,'Toggle point cloud drawing','p')
self.add_action(save_point_cloud,'Save point cloud','s')
self.add_action(toggle_view,'Toggle views','v')
self.add_action(print_view,'Print current view','p')
def __init__(self):
GLPluginInterface.__init__(self)
self.add_action(setHalfSpeed, "0.5x speed", '-')
self.add_action(setDoubleSpeed, "2x speed", '+')
self.add_action(setParabolic, "Unsmoothed, parabolic", 'p')
self.add_action(setMinJerk, "Start/stop minimum jerk", 'm')
self.add_action(setStartStop, "Start/stop trapezoidal", 't')
self.add_action(setCosine, "Start/stop cosine", 'c')
self.add_action(setHermite, "Hermite spline created manually", 'h')
def initialize(self):
GLPluginBase.initialize(self)
self.images = {}
self.images['Cartesian position'] = GLTexture(
"UI/Resources/cartesian-translation.png")
self.images['Cartesian rotation'] = GLTexture(
"UI/Resources/cartesian-rotation.png")
self.images['Joint angles'] = GLTexture("UI/Resources/joint.png")
self.images['left'] = GLTexture("UI/Resources/left-arm.png")
self.images['right'] = GLTexture("UI/Resources/right-arm.png")
return True
def initialize(self):
GLPluginBase.initialize(self)
self.images = {}
self.images['Cartesian position'] = GLTexture("UI/Resources/cartesian-translation.png")
self.images['Cartesian rotation'] = GLTexture("UI/Resources/cartesian-rotation.png")
self.images['Joint angles'] = GLTexture("UI/Resources/joint.png")
self.images['left'] = GLTexture("UI/Resources/left-arm.png")
self.images['right'] = GLTexture("UI/Resources/right-arm.png")
self.images['arm'] = GLTexture("UI/Resources/ArmMode.png")
self.images['base'] = GLTexture("UI/Resources/BaseMode.png")
self.images['auto'] = GLTexture("UI/Resources/AutoMode.png")
# Logging images
self.images['log']=[None,None]
self.images['log'][1] = GLTexture("UI/Resources/log-on.png")
self.images['log'][0] = GLTexture("UI/Resources/log-off.png")
return True
def display(self):
GLPluginInterface.display(self)
if self.pc is not None and self.compute_pc:
#manual drawing of native or numpy point clouds
t0 = time.time()
if isinstance(self.pc, klampt.Geometry3D):
#Geometry3D drawing
self.pc_appearance.drawWorldGL(self.pc)
else:
glDisable(GL_LIGHTING)
glPointSize(5.0)
glColor3f(0, 0, 0)
glBegin(GL_POINTS)
for pt in self.pc:
if len(pt) == 6:
glColor3f(*pt[3:6])
if read_local:
glVertex3fv(se3.apply(T, pt[0:3]))
else:
glVertex3fv(pt[0:3])
glEnd()
#print "Draw PC time",time.time()-t0
return True
def initialize(self):
GLPluginBase.initialize(self)
return True
def __init__(self,taskGen):
GLPluginBase.__init__(self)
self.taskGen = taskGen
def __init__(self, world):
GLPluginInterface.__init__(self)
self.world = world
self.q = world.robot(0).getConfig()
def __init__(self,
taskGen): #taskGen is an instance ofSkittlesTaskGenerator
GLPluginBase.__init__(self)
self.taskGen = taskGen