def collision_checking(world, est_config_list, density):
"""
:param world: {Klampt.WorldModel} -- the world model
:param est_config_list: {list} -- the list of calibration configuration set
:param density: {float} -- the interpolation step, for example: 0.001
:return: path: {list or bool} -- if it is list, this is the path of the calibration. If it is bool(False), this
means that the path has collision
"""
robot = world.robot(0)
q_0 = robot.getConfig()
inter_len = int(1. / density)
path = []
for i in range(len(est_config_list)):
for j in range(1, inter_len + 1):
if i == 0:
milestone = vectorops.interpolate(q_0, est_config_list[i],
j * density)
else:
milestone = vectorops.interpolate(est_config_list[i - 1],
est_config_list[i],
j * density)
robot.setConfig(milestone)
if robot.selfCollides() is True:
return False
else:
path.append(milestone)
return path
def workspace_interpolate(a, b, u):
"""Interpolates either in R^n or SE(3) between points a and b. If len(a)<=3,
this will do cartesian interpolation. Otherwise, len(a)==6 is required and
this will do SE(3) interpolation."""
if len(a) <= 3:
return vectorops.interpolate(a, b, u)
assert len(a) == 6, "Can only interpolate in R2, R3, or SE(3)"
Ra, Rb = so3.from_moment(a[3:]), so3.from_moment(b[3:])
R = so3.interpolate(Ra, Rb, u)
return vectorops.interpolate(a[:3], b[:3], u) + so3.moment(R)
def segment_segment_intersection(seg1, seg2, umin=0, umax=1, vmin=0, vmax=1):
"""Given 2 2D segments (a1,b1) and (a2,b2) returns whether the portion of
seg1 in the range [umin,umax] overlaps seg2 in the range [vmin,vmax].
Returns the point of intersection or None if no intersection exists
"""
a1, b1 = seg1
a2, b2 = seg2
assert len(a1) == 2
assert len(b1) == 2
assert len(a2) == 2
assert len(b2) == 2
d = vectorops.sub(b1, a1)
a, b = vectorops.sub(a2, a1), vectorops.sub(b2, a1)
#now it's centered at a1
#u*d = a + v*(b-a)
e = vectorops.sub(b2, a2)
A = np.array([d, vectorops.mul(e, -1)]).T
try:
uv = np.dot(np.linalg.inv(A), a)
except np.linalg.linalg.LinAlgError:
return None
u, v = uv
if u < umin or u > umax:
return None
if v < umin or v > umax:
return None
return vectorops.interpolate(a1, b1, u)
def segment_point_distance(a, b, x):
"""Returns (distance, parameter) pair between segment (a,b) and point x"""
d = vectorops.sub(b, a)
u = vectorops.dot(vectorops.sub(x, a), d) / vectorops.dot(d, d)
u = min(1.0, max(u, 0.0))
d = vectorops.distance(x, vectorops.interpolate(a, b, u))
return (d, u)
def partway_open_config(self,amount):
"""Returns a finger configuration partway open, with amount in the
range [0 (closed),1 (fully open)].
"""
if self.closed_config is None or self.open_config is None:
raise ValueError("Can't get an opening configuration on a robot that does not define it")
return vectorops.interpolate(self.closed_config,self.open_config,amount)
def bbox(bmin,
bmax,
R=None,
t=None,
world=None,
name=None,
mass=float('inf'),
type='TriangleMesh'):
"""Makes a box from bounds [bmin,bmax].
Args:
bmin (list of 3 floats): the lower corner of the box
center (list of 3 floats): the upper corner of the box
R,t (se3 transform, optional): if given, the box's world coordinates
will be rotated and shifted by this transform.
world (WorldModel, optional): If given, then the box will be a
RigidObjectModel or TerrainModel will be created in this world
name (str, optional): If world is given, this is the name of the object.
Default is 'box'.
mass (float, optional): If world is given and this is inf, then a
TerrainModel will be created. Otherwise, a RigidObjectModel
will be created with automatically determined inertia.
type (str, optional): the geometry type. Defaults to 'TriangleMesh',
but also 'GeometricPrimitive' and 'VolumeGrid' are accepted.
Returns:
Geometry3D, RigidObjectModel, or TerrainModel: A representation
of the box. If a world is given, then either a RigidObjectModel
or TerrainModel is added to the world and returned.
"""
w, d, h = vectorops.sub(bmax, bmin)
center = vectorops.interpolate(bmin, bmax, 0.5)
return box(w, d, h, center, R, t, world, name, mass, type)
def grasp_from_contacts(contact1,contact2):
"""Helper: if you have two contacts, this returns an AntipodalGrasp"""
d = vectorops.unit(vectorops.sub(contact2.x,contact1.x))
grasp = AntipodalGrasp(vectorops.interpolate(contact1.x,contact2.x,0.5),d)
grasp.finger_width = vectorops.distance(contact1.x,contact2.x)
grasp.contact1 = contact1
grasp.contact2 = contact2
return grasp
def interpolate(self, a, b, u):
"""TODO: Implement this interpolation function.
Inputs:
- a: the start configuration
- b: the end configuration
- u: the interpolation parameter in the range from 0 to 1
Out: the interpolated configuration
"""
interpolate_result = vectorops.interpolate(a[0:2], b[0:2], u)
interpolate_angle = interpolateAngle(a[2], b[2], u)
interpolate_result.append(interpolate_angle)
return interpolate_result
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 addBillboard(self,
name="Billboard",
image=[[]],
format='auto',
crange=[0, 1],
colormap='auto',
filter='linear',
size=(1, 1)):
"""Adds a 2D billboard to the world. The image is a 2D array of
values, which is texure-mapped to a quad.
By default, the billboard is centered at (0,0,0) and faces up.
To modify its location or orientation, call ``setTransform`` on it.
Args:
name (str): the name used to refer to this item
image (list of lists or str): a 2D array of single-channel values, (r,g,b) tuples, or (r,g,b,a)
tuples. Rows are listed top to bottom, rows from left to right. Or, can also be a URL.
format (str, optional): The image format. Can be:
* 'auto': autodetect the type from the image. If the image contains values, the format is 'value'.
* 'value': the values are mapped through either 'opacity', 'rainbow', or gradient
color mapping.
* 'rgb': if the image contains values, they are interpreted as RGB values packed in 24 bit
integers. Otherwise, the first 3 channels of the tuple are used.
* 'rgba': if the image contains values, they are interpreted as RGB values packed in 32 bit
integers. Otherwise, they are assumed to be (r,g,b,a) tuples
crange (pair of numbers, optional): the range of the given values / channels. By default [0,1], but if you are using uint8
encoding this should be set to [0,255].
colormap (optional): how the color of the billboard should be set based on the image. Valid values are:
* 'auto': if the image contains values, the gradient ((0,0,0),(1,1,1)) is used. Otherwise
'replace' is used.
* (color1,color2): interpolates between the two given (r,g,b) or (r,g,b,a) tuples.
* 'opacity': sets the alpha channel only.
* 'modulate': the value / rgb / rgba texture modulates the billboard color as set by setColor
filter (str, optional): how values between pixels are interpolated. Either 'nearest' or 'linear'.
size (pair of numbers, optional): the (width,height) pair of the billboard, in world units.
"""
if not isinstance(image, str):
import struct
import base64
bytes = []
w, h = None, None
h = len(image)
for row in image:
if w == None:
w = len(row)
else:
assert w == len(row), "Image is not a 2D array"
pixel = image[0][0]
if format == 'auto':
if hasattr(pixel, '__iter__'):
if len(pixel) == 4:
format = 'rgba'
else:
format = 'rgb'
else:
format = 'value'
else:
if not hasattr(pixel, '__iter__'):
format = 'p' + format
gradient = (type(colormap) != str)
for row in image:
for pixel in row:
if format == 'value':
u = min(
1,
max(0,
(pixel - crange[0]) / (crange[1] - crange[0])))
if gradient:
color = vectorops.interpolate(
gradient[0], gradient[1], u)
r = 0xff * min(1, max(0, color[0]))
g = 0xff * min(1, max(0, color[1]))
b = 0xff * min(1, max(0, color[2]))
packed = (0xff << 24) | (int(b) << 16) | (
int(g) << 8) | int(r)
bytes.append(struct.pack('<I', packed))
else:
val = 0xff * u
bytes.append(struct.pack('B', val))
elif format == 'prgb' or format == 'prgba':
bytes.append(struct.pack('<I', pixel))
elif format == 'rgb':
r = 0xff * min(
1,
max(0, (pixel[0] - crange[0]) /
(crange[1] - crange[0])))
g = 0xff * min(
1,
max(0, (pixel[1] - crange[0]) /
(crange[1] - crange[0])))
b = 0xff * min(
1,
max(0, (pixel[2] - crange[0]) /
(crange[1] - crange[0])))
packed = (0xff << 24) | (int(b) << 16) | (
int(g) << 8) | int(r)
bytes.append(struct.pack('<I', packed))
elif format == 'rgba':
r = 0xff * min(
1,
max(0, (pixel[0] - crange[0]) /
(crange[1] - crange[0])))
g = 0xff * min(
1,
max(0, (pixel[1] - crange[0]) /
(crange[1] - crange[0])))
b = 0xff * min(
1,
max(0, (pixel[2] - crange[0]) /
(crange[1] - crange[0])))
a = 0xff * min(
1,
max(0, (pixel[3] - crange[0]) /
(crange[1] - crange[0])))
packed = (int(a) << 24) | (int(b) << 16) | (
int(g) << 8) | int(r)
bytes.append(struct.pack('<I', packed))
else:
raise ValueError("Invalid format " + format)
image = base64.b64encode(''.join(bytes))
self._do_rpc({
'type': 'add_billboard',
'name': name,
'imagedata': image,
'width': w,
'height': h,
'size': size,
'filter': filter,
'colormap': colormap
})
else:
self._do_rpc({
'type': 'add_billboard',
'name': name,
'image': image,
'size': size,
'filter': filter,
'colormap': colormap
})
self._extras[name] = ('Billboard', image)
def interpolate(self, a, b, u):
return vectorops.interpolate(a[:2], b[:2],
u) + [a[2] + u * so2.diff(b[2], a[2])]
def addBillboard(self,name="Billboard",image=[[]],format='auto',crange=[0,1],colormap='auto',filter='linear',size=(1,1)):
"""Adds a 2D billboard to the world. The image is a 2D array of
values, which is texure-mapped to a quad.
By default, the billboard is centered at (0,0,0) and faces up.
To modify its location or orientation, call ``setTransform`` on it.
Args:
name (str): the name used to refer to this item
image (list of lists or str): a 2D array of single-channel values, (r,g,b) tuples, or (r,g,b,a)
tuples. Rows are listed top to bottom, rows from left to right. Or, can also be a URL.
format (str, optional): The image format. Can be:
* 'auto': autodetect the type from the image. If the image contains values, the format is 'value'.
* 'value': the values are mapped through either 'opacity', 'rainbow', or gradient
color mapping.
* 'rgb': if the image contains values, they are interpreted as RGB values packed in 24 bit
integers. Otherwise, the first 3 channels of the tuple are used.
* 'rgba': if the image contains values, they are interpreted as RGB values packed in 32 bit
integers. Otherwise, they are assumed to be (r,g,b,a) tuples
crange (pair of numbers, optional): the range of the given values / channels. By default [0,1], but if you are using uint8
encoding this should be set to [0,255].
colormap (optional): how the color of the billboard should be set based on the image. Valid values are:
* 'auto': if the image contains values, the gradient ((0,0,0),(1,1,1)) is used. Otherwise
'replace' is used.
* (color1,color2): interpolates between the two given (r,g,b) or (r,g,b,a) tuples.
* 'opacity': sets the alpha channel only.
* 'modulate': the value / rgb / rgba texture modulates the billboard color as set by setColor
filter (str, optional): how values between pixels are interpolated. Either 'nearest' or 'linear'.
size (pair of numbers, optional): the (width,height) pair of the billboard, in world units.
"""
if not isinstance(image,str):
import struct
import base64
bytes = []
w,h = None,None
h = len(image)
for row in image:
if w == None:
w = len(row)
else:
assert w == len(row),"Image is not a 2D array"
pixel = image[0][0]
if format == 'auto':
if hasattr(pixel,'__iter__'):
if len(pixel) == 4:
format = 'rgba'
else:
format = 'rgb'
else:
format = 'value'
else:
if not hasattr(pixel,'__iter__'):
format = 'p'+format
gradient = (type(colormap) != str)
for row in image:
for pixel in row:
if format == 'value':
u = min(1,max(0,(pixel - crange[0]) / (crange[1]-crange[0])))
if gradient:
color = vectorops.interpolate(gradient[0],gradient[1],u)
r = 0xff * min(1,max(0,color[0]))
g = 0xff * min(1,max(0,color[1]))
b = 0xff * min(1,max(0,color[2]))
packed = (0xff << 24) | (int(b) << 16) | (int(g) << 8) | int(r)
bytes.append(struct.pack('<I',packed))
else:
val = 0xff * u
bytes.append(struct.pack('B',val))
elif format == 'prgb' or format == 'prgba':
bytes.append(struct.pack('<I', pixel))
elif format == 'rgb':
r = 0xff * min(1,max(0,(pixel[0] - crange[0]) / (crange[1]-crange[0])))
g = 0xff * min(1,max(0,(pixel[1] - crange[0]) / (crange[1]-crange[0])))
b = 0xff * min(1,max(0,(pixel[2] - crange[0]) / (crange[1]-crange[0])))
packed = (0xff << 24) | (int(b) << 16) | (int(g) << 8) | int(r)
bytes.append(struct.pack('<I', packed))
elif format == 'rgba':
r = 0xff * min(1,max(0,(pixel[0] - crange[0]) / (crange[1]-crange[0])))
g = 0xff * min(1,max(0,(pixel[1] - crange[0]) / (crange[1]-crange[0])))
b = 0xff * min(1,max(0,(pixel[2] - crange[0]) / (crange[1]-crange[0])))
a = 0xff * min(1,max(0,(pixel[3] - crange[0]) / (crange[1]-crange[0])))
packed = (int(a) << 24) | (int(b) << 16) | (int(g) << 8) | int(r)
bytes.append(struct.pack('<I', packed))
else:
raise ValueError("Invalid format "+format)
image = base64.b64encode(''.join(bytes))
self._do_rpc({'type':'add_billboard','name':name,'imagedata':image,'width':w,'height':h,'size':size,'filter':filter,'colormap':colormap})
else:
self._do_rpc({'type':'add_billboard','name':name,'image':image,'size':size,'filter':filter,'colormap':colormap})
self._extras[name] = ('Billboard',image)
def getTorque(t, q, dq):
""" TODO: implement me
Returns a 4-element torque vector given the current time t, the configuration q, and the joint velocities dq to drive
the robot so that it traces out the desired curves.
Recommended order of operations:
1. Monitor and update the current status and stroke
2. Update the stroke_progress, making sure to perform smooth interpolating trajectories
3. Determine a desired configuration given current state using IK
4. Send qdes, dqdes to PID controller
"""
global robot, status, current_stroke, current_stroke_progress, current_frac, stroke_list
# ### PID Implementation Arc Test
# qdes = [0,0,0,0]
# dqdes = [0,0,0,0]
# if t > 0.5:
# #move up
# qdes[1] = 0.3
# if t > 1.5:
# #drop the last link down
# qdes[3] = 0.04
# # print(q, qdes)
# if t > 2.0:
# #move down
# qdes[1] = -0.3
# kP = [20,6,10,4]
# kI = [9,4,5,3]
# kD = [15,2,0.4,2]
# print(q)
# ###
### Draw Strokes
kP = [25, 6, 10, 9]
kI = [10, 3.5, 5, 2]
kD = [15, 2, 0.4, 2]
joint_max_vel = 0.0
small_error_limit = 10**-2.2 # To see fast process set to 10**-1.0
large_error_limit = 10**-1.75 # To see fast process set to 10**-0.85
lift_h = 0.01
write_h = -0.0001
num_frac = 50.0
dqdes = [0, 0, 0, 0]
# State Machine
if status == 'lift up':
cur_target = addDim(stroke_list[current_stroke][-1], lift_h)
qdes = configSolver(cur_target)
if isArrive(q, qdes, dq, dqdes, large_error_limit, large_error_limit):
current_stroke += 1
status = 'move to above'
elif status == 'move to above':
cur_target = addDim(stroke_list[current_stroke][0], lift_h)
qdes = configSolver(cur_target)
if isArrive(q, qdes, dq, dqdes, large_error_limit, large_error_limit):
status = 'push down'
elif status == 'push down':
cur_target = addDim(stroke_list[current_stroke][0], write_h)
qdes = configSolver(cur_target)
if isArrive(q, qdes, dq, dqdes, small_error_limit, small_error_limit):
status = 'writting stroke'
elif status == 'writting stroke':
start_point = addDim(
stroke_list[current_stroke][current_stroke_progress - 1], write_h)
end_point = addDim(
stroke_list[current_stroke][current_stroke_progress], write_h)
cur_target = vectorops.interpolate(start_point, end_point,
1 / num_frac * current_frac)
qdes = configSolver(cur_target)
if isArrive(q, qdes, dq, dqdes, small_error_limit, small_error_limit):
current_frac += 1
if current_frac > num_frac:
current_frac = 0
if current_stroke_progress < len(
stroke_list[current_stroke]) - 1:
current_stroke_progress += 1
else:
current_stroke_progress = 1
if current_stroke == len(stroke_list) - 1:
status = 'finish'
print('finish')
else:
status = 'lift up'
elif status == 'finish':
hold_point = addDim(stroke_list[current_stroke][-1], lift_h)
qdes = configSolver(hold_point)
###
### Test Print
# print('Status: ' + status + " ")
# print('Current Stroke: ',current_stroke)
# print('Stroke Progress: ', current_stroke_progress)
# print('Current frac: ', current_frac)
# print('Current pen tip ', q[3])
# print('Target pen tip ', dqdes[3])
# print('Current pen vel ', dq)
# print('Current target pen vel ', dqdes)
# print('---------------------------')
###
return getPIDTorqueAndAdvance(q, dq, qdes, dqdes, kP, kI, kD,
pid_integrators, dt)
def display(self):
if self.configs == None:
self.robot.drawGL()
else:
for q in self.configs:
self.robot.setConfig(q)
self.robot.drawGL()
if self.walk_workspace_path != None:
if self.temp_config:
self.robot.setConfig(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.robot.drawGL()
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)
assert len(
self.resolution.ikTaskSpace) == 1, "Can only do one task for now"
task = self.resolution.ikTaskSpace[0]
#DEBUG
if hasattr(self.resolution, 'delaunay'):
delaunay = self.resolution.delaunay
delaunay_pts = self.resolution.delaunay_pts
delaunay_epts = self.resolution.delaunay_epts
delaunay_others = self.resolution.delaunay_others
ptset = set(tuple(v) for v in delaunay_pts)
assert len(ptset) == len(
delaunay_pts), "Duplicate points! %d unique / %d" % (
len(ptset), len(delaunay_pts))
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glColor3f(0, 0, 0)
glLineWidth(1.0)
import random
for s in delaunay.simplices:
"""
glBegin(GL_LINE_LOOP)
for v in s:
glVertex3fv(delaunay_pts[v])
glEnd()
"""
centroid = vectorops.div(
vectorops.add(*[delaunay_pts[v] for v in s]), len(s))
assert len(s) == 3
glBegin(GL_LINES)
for i, v in enumerate(s):
neighbors = [w for j, w in enumerate(s) if i < j]
for w in neighbors:
if (v in delaunay_epts and w in delaunay_others) or (
w in delaunay_epts and v in delaunay_others):
glColor3f(1, 1, 1)
else:
glColor3f(0, 0, 0)
glVertex3fv(
vectorops.interpolate(delaunay_pts[v], centroid,
0.1))
glVertex3fv(
vectorops.interpolate(delaunay_pts[w], centroid,
0.1))
glEnd()
glColor3f(0, 1, 0)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_epts:
glVertex3fv(delaunay_pts[v])
glEnd()
glColor3f(0, 0, 1)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_others:
glVertex3fv(delaunay_pts[v])
glEnd()
glEnable(GL_DEPTH_TEST)
#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 = self.resolution.workspaceDistance(
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 task.numConstraints > 3:
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()
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 task.numConstraints > 3:
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:
self.disconnectionDisplayLists_by_orientation[
mbest] = CachedGLObject()
self.disconnectionDisplayLists_by_orientation[mbest].draw(
drawDisconnections, args=(mbest, ))
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)
GLWidgetPlugin.display(self)
