def __init__(self, world, robot):
if KLAMPT_VERSION >= 0.7:
GLBaseClass.__init__(self)
else:
GLBaseClass.__init__(self, "Redundancy resolution test")
self.world = world
self.robot = robot
assert robot.index == 0, "Can only work with the 0'th robot in the world"
self.resolution = RedundancyResolutionGraph(world)
self.mode = 'interpolate'
self.configs = None
self.folder = None
self.settings = None
self.drawJointLimitColors = True
self.jointLimitColoringRatio = 0.1
self.jointLimitColor = (1, 0.5, 0, 1)
self.jointLimits = robot.getJointLimits()
self.drawWorkspaceRoadmap = False
self.solveConstraint = True
self.clippingplanes = (0.1, 50)
self.rotationAsDepth = False
self.pointWidget = PointPoser()
self.xformWidget = TransformPoser()
self.roadmapDisplayList = CachedGLObject()
self.disconnectionDisplayList = CachedGLObject()
self.movie_frame = None
self.movie_rotate = False
self.walk_path = None
self.walk_workspace_path = None
self.walk_progress = None
self.temp_config = None
self.useboundary = None
def __init__(self, world, robot):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = robot
assert robot.index == 0, "Can only work with the 0'th robot in the world"
self.resolution = RedundancyResolutionGraph(world)
self.mode = 'interpolate'
self.configs = None
self.folder = None
self.settings = None
self.drawWorkspaceRoadmap = False
self.solveConstraint = True
self.clippingplanes = (0.1, 50)
self.rotationAsDepth = False
self.pointWidget = PointPoser()
self.xformWidget = TransformPoser()
self.roadmapDisplayList = CachedGLObject()
self.disconnectionDisplayList = CachedGLObject()
self.movie_frame = None
self.movie_rotate = False
self.walk_path = None
self.walk_workspace_path = None
self.walk_progress = None
self.temp_config = None
self.useboundary = None
def __init__(self,folder="FrictionCones/StanfordMicroSpineUnit"): GLWidgetPlugin.__init__(self) self.factory = GeneralizedFrictionConeFactory() self.factory.load(folder) self.contacts = [] self.widgets = self.klamptwidgetmaster self.cmWidget = PointPoser() self.cmWidget.set((0,0,0.5)) self.contactWidgets = [TransformPoser()] self.widgets.add(self.cmWidget) self.widgets.add(self.contactWidgets[0]) self.wrenches = None self.contactSize = 0.02 self.feasible = None
def mousefunc(self, button, state, x, y):
#Put your mouse handler here
#the current example prints out the list of objects clicked whenever
#you right click
GLWidgetProgram.mousefunc(self, button, state, x, y)
self.reSolve = False
dragging = False
if NEW_KLAMPT:
dragging = self.widgetPlugin.klamptwidgetdragging
else:
dragging = self.draggingWidget
if not dragging and button == 2 and state == 0:
#down
clicked = self.click_world(x, y)
if clicked is not None and isinstance(clicked[0], RobotModelLink):
#make a new widget
link, wpt = clicked
lpt = se3.apply(se3.inv(link.getTransform()), wpt)
self.ikIndices.append(len(self.ikWidgets))
self.ikWidgets.append(PointPoser())
self.ikWidgets[-1].set(wpt)
self.widgetMaster.add(self.ikWidgets[-1])
self.ikProblem.addObjective(
ik.objective(link, local=lpt, world=wpt))
GLWidgetProgram.mousefunc(self, button, state, x, y)
self.refresh()
return
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):
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')
class GLRedundancyProgram(GLBaseClass):
def __init__(self, world, robot):
if KLAMPT_VERSION >= 0.7:
GLBaseClass.__init__(self)
else:
GLBaseClass.__init__(self, "Redundancy resolution test")
self.world = world
self.robot = robot
assert robot.index == 0, "Can only work with the 0'th robot in the world"
self.resolution = RedundancyResolutionGraph(world)
self.mode = 'interpolate'
self.configs = None
self.folder = None
self.settings = None
self.drawJointLimitColors = True
self.jointLimitColoringRatio = 0.1
self.jointLimitColor = (1, 0.5, 0, 1)
self.jointLimits = robot.getJointLimits()
self.drawWorkspaceRoadmap = False
self.solveConstraint = True
self.clippingplanes = (0.1, 50)
self.rotationAsDepth = False
self.pointWidget = PointPoser()
self.xformWidget = TransformPoser()
self.roadmapDisplayList = CachedGLObject()
self.disconnectionDisplayList = CachedGLObject()
self.movie_frame = None
self.movie_rotate = False
self.walk_path = None
self.walk_workspace_path = None
self.walk_progress = None
self.temp_config = None
self.useboundary = None
def initialize(self):
GLBaseClass.initialize(self)
if KLAMPT_VERSION >= 0.7:
self.window.program.clearColor = [1, 1, 1, 1]
else:
self.clearColor = [1, 1, 1, 1]
assert self.resolution.domain != None
print "Domain", self.resolution.domain
if len(self.resolution.domain[0]) == 6:
#if a 2D problem and want to show depth, turn this to true
active = [
i for i, (a, b) in enumerate(zip(*self.resolution.domain)[:3])
if b != a
]
if len(active) <= 2:
self.rotationAsDepth = True
else:
self.rotationAsDepth = False
link = self.resolution.ikTemplate.objectives[0].link()
self.xformWidget.enableTranslation(True)
self.xformWidget.enableRotation(True)
print "Initial transform", self.robot.link(link).getTransform()
#self.xformWidget.set(*self.robot.link(link).getTransform())
if KLAMPT_VERSION >= 0.7:
self.addWidget(self.xformWidget)
else:
self.widgetMaster.add(self.xformWidget)
else:
self.rotationAsDepth = False
link = self.resolution.ikTemplate.objectives[0].link()
local, world = self.resolution.ikTemplate.objectives[
0].getPosition()
print "Initial position", self.resolution.robot.link(
link).getWorldPosition(local)
self.pointWidget.set(
self.resolution.robot.link(link).getWorldPosition(local))
if KLAMPT_VERSION >= 0.7:
self.addWidget(self.pointWidget)
else:
self.widgetMaster.add(self.pointWidget)
return True
def run(self):
"""Standardized interface for running program"""
if KLAMPT_VERSION >= 0.7:
vp = vis.getViewport()
#Square screen
#vp.w,vp.h = 800,800
#For saving HD quality movies
vp.w, vp.h = 1024, 768
vp.clippingplanes = self.clippingplanes
vis.setViewport(vp)
#vis.run(program)
vis.setPlugin(self)
vis.show()
while vis.shown():
time.sleep(0.1)
vis.setPlugin(None)
vis.kill()
else:
#Square screen
#self.width,self.height = 800,800
#For saving HD quality movies
self.width, self.height = 1024, 768
GLBaseClass.run(self)
def workspaceToPoint(self, x):
if self.rotationAsDepth:
return (x[0], x[4], x[2])
else:
return x[:3]
def drawConfig(self, q=None):
if self.drawJointLimitColors:
testq = q
if q is None:
testq = self.robot.getConfig()
oldColors = []
for i in xrange(self.robot.numLinks()):
oldColors.append(self.robot.link(i).appearance().getColor())
if self.robot.getJointType(i) == 'normal':
a, b = self.jointLimits[0][i], self.jointLimits[1][i]
u = 1.0
u = min(1, (testq[i] - a) / (self.jointLimitColoringRatio *
(b - a)))
u = min(1, (b - testq[i]) / (self.jointLimitColoringRatio *
(b - a)))
if u != 1:
u = max(u, 0.0)
c = vectorops.add(
vectorops.mul(oldColors[i], u),
vectorops.mul(self.jointLimitColor, 1 - u))
self.robot.link(i).appearance().setColor(*c)
if q is not None:
self.robot.setConfig(q)
self.robot.drawGL()
if self.drawJointLimitColors:
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(*oldColors[i])
def display(self):
if self.configs == None:
self.drawConfig(None)
else:
for q in self.configs:
self.drawConfig(q)
if self.walk_workspace_path != None:
if self.temp_config:
glEnable(GL_BLEND)
#glDisable(GL_DEPTH_TEST)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(1, 0, 0, 0.5)
self.drawConfig(self.temp_config)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(0.5, 0.5, 0.5, 1)
#glEnable(GL_DEPTH_TEST)
glDisable(GL_BLEND)
glColor3f(1, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
for w in self.walk_workspace_path.milestones:
if len(w) == 2:
glVertex2f(w[0], w[1])
else:
glVertex3f(w[0], w[1], w[2])
glEnd()
glLineWidth(1.0)
#draw workspace graph
if self.drawWorkspaceRoadmap:
def drawWorkspaceRoadmap(orientation=None):
G = self.resolution.Gw
if orientation is not None:
Rclosest, G = self.resolution.extractOrientedSubgraph(
orientation)
print G.number_of_nodes()
if not self.resolution.hasResolution():
glDisable(GL_LIGHTING)
glPointSize(5.0)
glColor3f(1, 0, 0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
for (i, j) in G.edges_iter():
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
else:
#maxn = max(len(d['qlist']) for n,d in G.nodes_iter(data=True))
#maxe = max(len(d['qlist']) for i,j,d in G.edges_iter(data=True))
#maxn = max(maxn,1)
#maxe = max(maxe,1)
glDisable(GL_LIGHTING)
glPointSize(5.0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
if not self.resolution.isResolvedNode(n):
continue
#u = float(len(d['qlist']))/float(maxn)
#nsubset = sum(1 for iq in d['qlist'] if iq in self.rr.Qsubgraph)
#if nsubset > 1:
# glColor3f(1,0,1)
#else:
# glColor3f(u,nsubset*0.5,0)
glColor3f(1, 0.5, 0)
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glBegin(GL_LINES)
for (i, j, d) in G.edges_iter(data=True):
if not self.resolution.isResolvedNode(
i) or not self.resolution.isResolvedNode(j):
continue
#nsubset = sum(1 for (ia,ib) in d['qlist'] if (ia in self.Qsubgraph and ib in self.Qsubgraph))
#u = float(len(d['qlist']))/float(maxe)
r, g, b = 1, 1, 0
if not self.resolution.isResolvedEdge(i, j):
r, g, b = 1, 0, 1
else:
qd = self.robot.distance(G.node[i]['config'],
G.node[j]['config'])
wd = workspace_distance(G.node[i]['params'],
G.node[j]['params'])
u = 1.0 - math.exp(-max(0.0, 2.0 * qd / wd - 1.0))
if u > 0.8:
r, g, b = 1, 1.0 - u * 5.0, 0.0
else:
r, g, b = u / 0.8, 1, 0.0
#assert (nsubset >=1) == self.resolution.isResolvedEdge(i,j),"Mismatch between Qsubgraph and resolution?"
glColor3f(r, g, b)
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
"""
glEnable(GL_LIGHTING)
q0 = self.robot.getConfig()
for iw,d in self.rr.Gw.nodes_iter(data=True):
qs = [iq for iq in d['qlist'] if iq in self.rr.Qsubgraph]
if len(qs) > 1:
for iq in qs:
self.robot.setConfig(self.rr.Gq.node[iq]['config'])
self.robot.drawGL()
self.robot.setConfig(q0)
glDisable(GL_LIGHTING)
"""
if len(self.resolution.domain[0]) == 6:
if not hasattr(self, 'roadmapDisplayLists_by_orientation'):
self.roadmapDisplayLists_by_orientation = dict()
self.lastROrientation = None
self.lastRMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastROrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastRMbest = mbest
else:
mbest = self.lastRMbest
if mbest not in self.roadmapDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.roadmapDisplayLists_by_orientation[
mbest] = CachedGLObject()
orientation = so3.from_moment(mbest)
self.roadmapDisplayLists_by_orientation[mbest].draw(
drawWorkspaceRoadmap, args=(orientation, ))
else:
#there looks to be a bug in GL display lists for drawing lines...
#self.roadmapDisplayList.draw(drawWorkspaceRoadmap)
drawWorkspaceRoadmap()
else:
#render boundaries only
def drawDisconnections(orientation=None):
bmin, bmax = self.resolution.domain
active = [
i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a
]
if self.useboundary == None:
self.useboundary = (len(active) == 2)
verts, faces = self.resolution.computeDiscontinuities(
self.useboundary, orientation)
if len(active) == 3:
glEnable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glDisable(GL_CULL_FACE)
#if self.walk_workspace_path != None:
# gldraw.setcolor(1,0,1,0.25)
#else:
# gldraw.setcolor(1,0,1,0.5)
for tri in faces:
tverts = [verts[v] for v in tri]
centroid = vectorops.div(vectorops.add(*tverts),
len(tri))
params = [
(x - a) / (b - a)
for (x, a,
b) in zip(centroid, self.resolution.domain[0],
self.resolution.domain[1])
]
if self.walk_workspace_path != None:
gldraw.setcolor(params[0], params[1], params[2],
0.25)
else:
gldraw.setcolor(params[0], params[1], params[2],
1.0)
gldraw.triangle(*tverts)
glEnable(GL_CULL_FACE)
else:
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(1, 0, 1, 0.5)
glLineWidth(4.0)
glBegin(GL_LINES)
for s in faces:
spts = verts[s[0]], verts[s[1]]
glVertex3f(spts[0][0], 0, spts[0][1])
glVertex3f(spts[1][0], 0, spts[1][1])
glEnd()
glLineWidth(1.0)
#draw stabbing lines
"""
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
glBegin(GL_LINES)
for (i,j,d) in self.resolution.Gw.edges_iter(data=True):
if self.resolution.isResolvedEdge(i,j):
continue
if not self.resolution.isResolvedNode(i) or not self.resolution.isResolvedNode(j):
continue
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[i]['params']))
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[j]['params']))
glEnd()
"""
if len(self.resolution.domain[0]) == 6:
if not hasattr(self,
'disconnectionDisplayLists_by_orientation'):
self.disconnectionDisplayLists_by_orientation = dict()
self.lastOrientation = None
self.lastMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastOrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastMbest = mbest
else:
mbest = self.lastMbest
if mbest not in self.disconnectionDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.disconnectionDisplayLists_by_orientation[
mbest] = CachedGLObject()
orientation = so3.from_moment(mbest)
self.disconnectionDisplayLists_by_orientation[mbest].draw(
drawDisconnections, args=(orientation, ))
else:
self.disconnectionDisplayList.draw(drawDisconnections)
bmin, bmax = self.resolution.domain
#draw workspace bound
#glDisable(GL_LIGHTING)
#glColor3f(1,0.5,0)
#gldraw.box(bmin[:3],bmax[:3],lighting=False,filled=False)
GLBaseClass.display(self)
def mousefunc(self, button, state, x, y):
GLBaseClass.mousefunc(self, button, state, x, y)
self.do_click(x, y)
def do_click(self, x, y):
if self.pointWidget.hasFocus():
p = self.pointWidget.get()
if len(self.resolution.domain[0]) == 6:
x = p + [0, 0, 0]
else:
x = p
if self.mode == 'interpolate':
if self.solveConstraint:
qOrig = self.configs[
0] if self.configs is not None else self.robot.getConfig(
)
qx = self.resolution.eval(x, qOrig)
self.configs = [qx if qx is not None else qOrig]
else:
self.configs = self.resolution.interpolate(x, multi=True)
else:
self.resolution.interpolate(x, multi=True)
self.configs = self.resolution.lastqs
self.refresh()
if self.xformWidget.hasFocus():
R, t = self.xformWidget.get()
m = so3.moment(R)
x = t + m
if self.mode == 'interpolate':
if self.solveConstraint:
qOrig = self.configs[
0] if self.configs is not None else self.robot.getConfig(
)
qx = self.resolution.eval(x, qOrig)
self.configs = [qx if qx is not None else qOrig]
else:
self.configs = self.resolution.interpolate(x, multi=True)
else:
self.resolution.interpolate(x, multi=True)
self.configs = self.resolution.lastqs
self.refresh()
def motionfunc(self, x, y, dx=None, dy=None):
if KLAMPT_VERSION >= 0.7:
GLBaseClass.motionfunc(self, x, y, dx, dy)
self.do_click(x, y)
else:
GLBaseClass.motionfunc(self, dx, dy)
self.do_click(self.lastx, self.lasty)
def keyboardfunc(self, c, x, y):
if c == 'h':
print "Keyboard help:"
print "- x: verifies edge checks for existing resolution"
print "- i: toggles between interpolation mode and graph inspection mode"
print "- g: toggles drawing the workspace graph"
print "- w: performs a walk to a random workspace node"
print "- m: saves a real-time movie"
print "- M: saves a 360 spin movie"
elif c == 'x':
self.resolution.verify()
self.disconnectionDisplayList.markChanged()
elif c == 'i':
if self.mode == 'interpolate':
self.mode = 'inspect'
else:
self.mode = 'interpolate'
print "Toggled visualization mode to", self.mode
elif c == 'g':
self.drawWorkspaceRoadmap = not self.drawWorkspaceRoadmap
elif c == 'b':
if self.useboundary: self.useboundary = False
else: self.useboundary = True
self.disconnectionDisplayList.markChanged()
elif c == 'm':
if self.movie_frame is None:
self.movie_frame = 0
self.movie_rotate = False
else:
self.movie_frame = None
elif c == 'M':
if self.movie_frame is None:
self.movie_frame = 0
self.movie_rotate = True
else:
self.movie_frame = None
elif c == 'w':
import random
resolved = []
for iw, d in self.resolution.Gw.nodes(data=True):
if d.get('config', None) is not None:
resolved.append(iw)
if self.walk_workspace_path == None:
#draw boundaries transparent now
self.disconnectionDisplayList.markChanged()
for iters in range(10):
wtgt = random.choice(resolved)
#TEMP: place widgets far away for capturing video
far = [20] * 3
self.pointWidget.set(far)
R, t = self.xformWidget.get()
self.xformWidget.set(R, far[:3])
#get current config
if self.configs != None:
self.robot.setConfig(self.configs[0])
try:
x, p = self.resolution.walkPath(wtgt)
except Exception as e:
import traceback
print "Exception", e
traceback.print_exc()
print "WalkPath failed, trying with a new random point"
continue
self.walk_workspace_path = None
if p != None:
t = 0
times = []
for i, q in enumerate(p):
times.append(t)
if i + 1 < len(p):
t += linf_distance(q, p[i + 1],
self.resolution.spinJoints)
#t += self.robot.distance(q,p[i+1])
#t += 0.1
self.walk_workspace_path = trajectory.Trajectory(times, x)
self.walk_path = trajectory.RobotTrajectory(
self.robot, times, p)
self.walk_progress = 0
if self.temp_config == None:
self.temp_config = p[0]
break
self.refresh()
def idle(self):
t0 = time.time()
if self.movie_frame is not None:
numframes = 180
if self.movie_rotate:
self.window.program.view.camera.rot[
2] += math.pi * 2 / numframes
self.window.program.save_screen("frame%03d.ppm" %
(self.movie_frame))
self.movie_frame += 1
if self.movie_rotate and self.movie_frame >= numframes:
self.movie_frame = None
if self.walk_path != None:
self.walk_progress += 0.02
if self.walk_progress >= self.walk_path.times[-1]:
self.configs = [self.walk_path.milestones[-1]]
self.walk_path = None
else:
#self.configs = [self.walk_path.eval(self.walk_progress)]
self.configs = [
self.resolution.eval(
self.walk_workspace_path.eval(self.walk_progress))
]
if self.configs[0] == None:
self.configs = []
u = self.walk_progress / self.walk_path.times[-1]
qstraight = self.resolution.solve(
self.temp_config,
vectorops.interpolate(
self.walk_workspace_path.milestones[0],
self.walk_workspace_path.milestones[-1], u),
testfeasibility=False)
if qstraight and (
self.resolution.ikTemplate.feasibilityTest == None or
self.resolution.ikTemplate.feasibilityTest(qstraight)):
self.temp_config = qstraight
self.refresh()
t1 = time.time()
if t1 - t0 < 0.02:
time.sleep(0.02 - (t1 - t0))
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 GLStabilityPlugin(GLWidgetPlugin): def __init__(self,folder="FrictionCones/StanfordMicroSpineUnit"): GLWidgetPlugin.__init__(self) self.factory = GeneralizedFrictionConeFactory() self.factory.load(folder) self.contacts = [] self.widgets = self.klamptwidgetmaster self.cmWidget = PointPoser() self.cmWidget.set((0,0,0.5)) self.contactWidgets = [TransformPoser()] self.widgets.add(self.cmWidget) self.widgets.add(self.contactWidgets[0]) self.wrenches = None self.contactSize = 0.02 self.feasible = None def display(self): GLWidgetPlugin.display(self) glEnable(GL_LIGHTING) if self.feasible == True: gldraw.setcolor(0,1,0,lighting=True) else: gldraw.setcolor(1,0,0,lighting=True) glPushMatrix() glTranslatef(*self.cmWidget.get()) radius = 0.05 gldraw.box([-radius]*3,[radius]*3) glPopMatrix() for w in self.contactWidgets: T = w.get() glPushMatrix() try: mat = zip(*se3.homogeneous(T)) mat = sum([list(coli) for coli in mat],[]) glMultMatrixf(mat) self.factory.forceTemplate.drawGL(self.contactSize) except: import traceback traceback.print_exc() exit(-1) glPopMatrix() if self.wrenches != None and len(self.wrenches) == len(self.contactWidgets): glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glColor3f(1,0.5,0) glBegin(GL_LINES) for w,widget in zip(self.wrenches,self.contactWidgets): p = widget.get()[1] glVertex3fv(p) glVertex3fv(vectorops.madd(p,(w[0],w[1],w[2]),1.0/9.8)) glEnd() glColor3f(0,0.5,1) glBegin(GL_LINES) for w,widget in zip(self.wrenches,self.contactWidgets): p = widget.get()[1] glVertex3fv(p) glVertex3fv(vectorops.madd(p,(w[3],w[4],w[5]),1.0/9.8)) glEnd() glEnable(GL_DEPTH_TEST) def solve(self): solver = GeneralizedStabilitySolver(self.factory) for w in self.contactWidgets: print "Transform",w.get() solver.addContactPatch(w.get(),self.contactSize) t0 = time.time() print "Mass 4, CM",self.cmWidget.get() solver.setGravityWrench(4,self.cmWidget.get()) self.wrenches = solver.solve() self.feasible = (self.wrenches != None) print "Feasible:",self.feasible if self.feasible: print self.wrenches assert len(self.wrenches) == len(self.contactWidgets) def transform(self,T): self.cmWidget.set(se3.apply(T,self.cmWidget.get())) for w in self.contactWidgets: w.set(*se3.mul(T,w.get())) def keyboardfunc(self,c,x,y): if c == '?': print "Help:" print "-?: display this message" print "-s: solve for stability" print "-a: add a new contact" print "-r: remove the currrently hovered contact" print "-<: rotate the entire setup ccw about the y axis" print "->: rotate the entire setup cw about the y axis" elif c == 's': self.solve() elif c == 'a': self.contactWidgets.append(TransformPoser()) self.widgets.add(self.contactWidgets[-1]) self.solve() elif c == 'r': removed = False for i,w in enumerate(self.contactWidgets): if w.hasHighlight(): self.widgets.remove(w) self.contactWidgets.pop(i) removed = True break if not removed: print "Warning, no currently hovered widget" else: self.solve() elif c == '<': T = (so3.rotation([0,1,0],math.radians(-5)),[0]*3) self.transform(T) self.solve() elif c == '>': T = (so3.rotation([0,1,0],math.radians(5)),[0]*3) self.transform(T) self.solve() self.refresh()