def animate_trajectories(trajs, times, endWaitTime=5.0, speed=0.2):
global world
active = 0
for i in range(len(trajs)):
vis.add(
"traj" + str(i), trajs[i].getLinkTrajectory(
END_EFFECTOR_LINK,
0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION))
vis.hide("traj" + str(i))
vis.hideLabel("traj" + str(i))
vis.show()
t0 = time.time()
if len(times) == 1:
tnext = float('inf')
else:
tnext = times[active + 1]
while vis.shown():
t = time.time()
if t - t0 > tnext:
nextactive = (active + 1) % len(times)
vis.hide("traj" + str(active))
vis.hide("traj" + str(nextactive), False)
active = nextactive
if nextactive + 1 == len(times):
tnext += endWaitTime
else:
tnext += times[active + 1] - times[active]
vis.lock()
world.robot(0).setConfig(trajs[active].eval((t - t0) * speed,
endBehavior='loop'))
vis.unlock()
time.sleep(0.01)
print("Done.")
def sphere_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = sphere_staggered_grid(N)
else:
G = sphere_grid(N)
for n in G.nodes():
x = G.node[n]['params']
vis.add(str(n), x)
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
xi = G.node[i]['params']
xj = G.node[j]['params']
vis.add(str(i) + '-' + str(j), trajectory.Trajectory([0, 1], [xi, xj]))
vis.hideLabel(str(i) + '-' + str(j))
dist = vectorops.distance(xi, xj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def show_friction_cone(folder="FrictionCones/StanfordMicroSpineUnit"):
V = Volume()
V.load(folder)
for C in V.convex_decomposition:
print "Convex decomposition"
print np.amin(C.points,axis=0)
print np.amax(C.points,axis=0)
Chull = np.vstack([V.convex_hull.points[u] for u in V.convex_hull.vertices])
#print Chull
Chull *= 0.05
for i,C in enumerate(V.convex_decomposition):
if len(V.convex_decomposition) == 1:
g = 0
else:
g = float(i)/(len(V.convex_decomposition)-1)
#for j,v in enumerate(C.vertices):
# vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05))
# vis.setColor("pt%d,%d"%(i,j),1,g,0)
# vis.hideLabel("pt%d,%d"%(i,j))
for j,pt in enumerate(C.points):
v = j
vis.add("pt%d,%d"%(i,j),vectorops.mul(C.points[v,:].tolist(),0.05))
vis.setColor("pt%d,%d"%(i,j),1,g,0)
vis.hideLabel("pt%d,%d"%(i,j))
for i in xrange(Chull.shape[0]):
vis.add("CH%d"%(i,),Chull[i,:].tolist())
vis.hideLabel("CH%d"%(i,))
vis.add("origin",(0,0,0))
vis.setColor("origin",0,1,0)
vis.dialog()
def visualize_cost(x, y, cost, name, with_z=0, hm=None, color=None, hide_label=False):
traj = trajectory.Trajectory(milestones=[[x, y, with_z], [x, y, with_z + cost]])
vis.add(name, traj)
if color:
VisUtils.set_color(name, color)
if hide_label:
vis.hideLabel(name)
def RobotCOMPlot(SimRobot, vis):
COMPos_start = SimRobot.getCom()
COMPos_end = COMPos_start[:]
COMPos_end[2] = COMPos_end[2] - 7.50
vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end]))
vis.hideLabel("COM", True)
vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("COM", 'width', 5.0)
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection):
# This function is used to plot PIP from COM to intersections
Edge_Index = str(i)
vis.add("PIPEdgefromCOM:" + Edge_Index,
Trajectory([0, 1], [COM_Pos, Intersection]))
vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def ConvexEdgesPlot(SimRobot, convex_edges_list, vis):
Convex_Edges_Number = len(convex_edges_list) / 2
COMPos = SimRobot.getCom()
for i in range(0, Convex_Edges_Number):
EdgeA = convex_edges_list[2 * i]
EdgeB = convex_edges_list[2 * i + 1]
# Three edges to be added: A->B, A -> COM, B-> COM
Edge_Index = str(i)
vis.add("Edge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB]))
vis.hideLabel("Edge:" + Edge_Index, True)
vis.setAttribute("Edge:" + Edge_Index, 'width', 5.0)
def visualize_in_klampt(self, height_map, step=10):
for x_inx in range(0, self.x_indices - 1, step):
for y_inx in range(0, self.y_indices - 1, step):
x_world = self.get_x_from_xindex(x_inx)
y_world = self.get_y_from_yindex(y_inx)
cost = self.np_cmap_arr[x_inx][y_inx]
name = str(x_world) + "," + str(y_world)
if cost > 0.001:
base_z = height_map.height_at_xy(x_world, y_world)
traj = trajectory.Trajectory(
milestones=[[x_world, y_world, base_z +
0.001], [x_world, y_world, base_z +
cost]])
vis.add(name, traj)
vis.hideLabel(name)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
Traj = []
for i in range(RowStart, RowEnd):
point_i = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_i[0] = ReachableContact_i[0]
point_i[1] = ReachableContact_i[1]
point_i[2] = ReachableContact_i[2]
Traj.append(point_i)
TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path"
vis.add(TransName, Trajectory([0, 1], Traj))
vis.hideLabel(TransName, True)
vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
TransName = "Step" + str(StepNo) + "Limb" + str(
LimbNo) + "TransPoint:" + str(i)
vis.add(TransName, point_start)
vis.hideLabel(TransName, True)
vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0,
1.0)
def ContactDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
ContactName = "Stage" + str(StepNo) + "LinkNo" + str(
LimbNo) + "Point:" + str(i)
vis.add(ContactName, point_start)
vis.hideLabel(ContactName, True)
vis.setColor(ContactName, 65.0 / 255.0, 199.0 / 255.0, 244.0 / 255.0,
1.0)
def ContactDataPlot(vis, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
# RowEnd = min(RowNo, 99)
RowEnd = RowNo
for i in range(RowStart, RowEnd):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
vis.add("Point_" + str(i), point_start)
vis.hideLabel("Point_" + str(i), True)
vis.setColor("Point_" + str(i), 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def visualize_in_klampt(self, step=5, xmin=4, xmax=12, ymin=8, ymax=12):
"""Visualize the height map using klampt"""
for x_inx in range(self.get_xindex_from_x(xmin),
self.get_xindex_from_x(xmax), step):
for y_inx in range(self.get_yindex_from_y(ymin),
self.get_yindex_from_y(ymax), step):
x_world = self.get_x_from_xindex(x_inx)
y_world = self.get_y_from_yindex(y_inx)
z = self.np_arr[x_inx][y_inx]
if z > 0:
name = str(x_world) + ", " + str(y_world)
traj = trajectory.Trajectory(
milestones=[[x_world, y_world, 0],
[x_world, y_world, z]])
vis.add(name, traj)
vis.hideLabel(name)
def draw_node(self, pos, colour=(1, 0, 0, 1), size=15, label=None):
"""Draw a node in the klampt visualiser
:param pos: the position of the node
:param colour: the colour of the node
:param size: the size of the node
:param label: if given, add label to the node
"""
from klampt import vis
from klampt.model.coordinates import Point
if label is None:
label = repr(pos)
vis.add(label, Point(pos), keepAppearance=True)
vis.setAttribute(label, "size", size)
vis.setAttribute(label, "color", colour)
vis.hideLabel(label)
def PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos, vis):
scale = 0.25
Edge_Index = str(i)
vis.add("PIPEdge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB]))
vis.hideLabel("PIPEdge:" + Edge_Index, True)
vis.setAttribute("PIPEdge:" + Edge_Index, 'width', 7.5)
vis.add("PIPEdgeCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, EdgeCOM]))
vis.hideLabel("PIPEdgeCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgeCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgeCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
# Local Coordinates
Edgex_i = [0.0, 0.0, 0.0]
Edgex_i[0] = EdgeCOM[0] + scale * Edgex[0]
Edgex_i[1] = EdgeCOM[1] + scale * Edgex[1]
Edgex_i[2] = EdgeCOM[2] + scale * Edgex[2]
vis.add("PIPEdgex:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgex_i]))
vis.hideLabel("PIPEdgex:" + Edge_Index, True)
vis.setAttribute("PIPEdgex:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgex:" + Edge_Index, 1.0, 0.0, 0.0, 1.0)
Edgey_i = [0.0, 0.0, 0.0]
Edgey_i[0] = EdgeCOM[0] + scale * Edgey[0]
Edgey_i[1] = EdgeCOM[1] + scale * Edgey[1]
Edgey_i[2] = EdgeCOM[2] + scale * Edgey[2]
vis.add("PIPEdgey:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgey_i]))
vis.hideLabel("PIPEdgey:" + Edge_Index, True)
vis.setAttribute("PIPEdgey:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgey:" + Edge_Index, 155.0 / 255.0, 244.0 / 255.0,
66.0 / 255.0, 1.0)
Edgez_i = [0.0, 0.0, 0.0]
Edgez_i[0] = EdgeCOM[0] + scale * Edgez[0]
Edgez_i[1] = EdgeCOM[1] + scale * Edgez[1]
Edgez_i[2] = EdgeCOM[2] + scale * Edgez[2]
# print Edgez_i
vis.add("PIPEdgez:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgez_i]))
vis.hideLabel("PIPEdgez:" + Edge_Index, True)
vis.setAttribute("PIPEdgez:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgez:" + Edge_Index, 68.0 / 255.0, 65.0 / 255.0,
244.0 / 255.0, 1.0)
def draw_path(self, pos1, pos2, colour=None):
"""Draw a path (line) in the klampt visualiser
:param pos1: the position of the start of line
:param pos2: the position of the end of line
:param colour: the colour of the line
"""
from klampt import vis
from klampt.model.trajectory import Trajectory
unique_label = repr((pos1, pos2))
label = repr((pos1, pos2))
if label not in self.drawn_label:
vis.add(label, Trajectory(milestones=[pos1, pos2]), keepAppearance=True)
if colour is not None:
vis.setAttribute(label, "color", colour)
vis.hideLabel(label)
def visRoadMap(self):
vis.show()
edgeList = self.G.edges()
cnt = 0
for e in edgeList:
n1 = e[0]
n2 = e[1]
tr = trajectory.Trajectory()
tr.milestones.append([n1.x, n1.y, n1.z])
tr.milestones.append([n2.x, n2.y, n2.z])
fName = "a" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setAttribute(fName, "width", 0.5)
vis.setColor(fName, 0.4940, 0.1840, 0.5560)
cnt = cnt + 1
self.rmCnt = cnt
def WeightedContactDataPlot(vis, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
print i
vis.add("PointWeights_" + str(i),
Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel("PointWeights_" + str(i), True)
vis.setColor("PointWeights_" + str(i), 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("PointWeights_" + str(i), 'width', 5.0)
def so3_grid_test(N=5, staggered=True):
from klampt import vis
from klampt.model import trajectory
if staggered:
G = so3_staggered_grid(N)
else:
G = so3_grid(N)
vispt = [1, 0, 0]
for n in G.nodes():
R = G.node[n]['params']
trans = so3.apply(R, vispt)
#trans = so3.axis_angle(R)[0]
#trans = vectorops.unit(so3.quaternion(R)[:3])
vis.add(str(n), [R, trans])
vis.hideLabel(str(n))
#draw edges?
minDist = float('inf')
maxDist = 0.0
for i, j in G.edges():
Ri = G.node[i]['params']
Rj = G.node[j]['params']
tmax = 9
times = range(tmax + 1)
milestones = []
for t in times:
u = float(t) / tmax
trans = so3.apply(so3.interpolate(Ri, Rj, u), vispt)
milestones.append(trans)
vis.add(
str(i) + '-' + str(j), trajectory.Trajectory(times, milestones))
vis.hideLabel(str(i) + '-' + str(j))
dist = so3.distance(Ri, Rj)
if dist > maxDist:
maxDist = dist
print "Max dispersion at", i, j, ":", dist
if dist < minDist:
minDist = dist
print "Number of points:", G.number_of_nodes()
print "Min/max dispersion:", minDist, maxDist
vis.run()
def visualize_in_klampt(self):
from klampt import vis
height = 0.25
for xyz in self.scatter:
name = f"{xyz[0]}{xyz[1]}{xyz[2]}"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + height]])
vis.add(name, traj)
vis.hideLabel(name)
for i in range(len(self.scatter) - 4, len(self.scatter)):
xyz = self.scatter[i]
name = f"{xyz[0]}{xyz[1]}{xyz[2]}_2"
traj = trajectory.Trajectory(
milestones=[[xyz[0], xyz[1], xyz[2] +
0.001], [xyz[0], xyz[1], xyz[2] + 1]])
vis.add(name, traj)
vis.setColor(name, 1, 0, 0, 1)
def ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj):
StartTime = ImpulseObj[0]
EndTime = ImpulseObj[1]
ImpulForce = ImpulseObj[2]
DurationTime = EndTime - StartTime
OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0])
ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime
EndPoint = OriPoint[:]
EndPoint[0] += ImpulForce[0] * ImpulseScale
EndPoint[1] += ImpulForce[1] * ImpulseScale
EndPoint[2] += ImpulForce[2] * ImpulseScale
if SimulationTime < StartTime:
pass
elif SimulationTime > EndTime:
vis.hide("Impulse", True)
else:
vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint]))
vis.hideLabel("Impulse", True)
vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0)
vis.setAttribute("Impulse", 'width', 5.0)
def WeightedContactDataPlot(vis, StepNo, LimbNo, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
ContactName = "Stage" + str(StepNo) + "LinkNo" + str(
LimbNo) + "Point:" + str(i)
print i
vis.add(ContactName, Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel(ContactName, True)
vis.setColor(ContactName, 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute(ContactName, 'width', 5.0)
def visualize(self, style="none", hide_label=True):
if style == "none":
milestones = []
for p in list(self.shapely_poly.exterior.coords):
z = self.height_map.height_at_xy(p[0], p[1]) + 0.01
milestones.append([p[0], p[1], z])
path = trajectory.Trajectory(milestones=milestones)
if not self.name:
self.name = "Support Triangle " + str(random.randint(1, 1000))
vis.add(self.name, path)
if hide_label:
vis.hideLabel(self.name)
elif style == "dashed":
dashes = 10.0
xyz_points = list(self.shapely_poly.exterior.coords)
for i in range(len(xyz_points)):
x, y = xyz_points[i][0], xyz_points[i][1]
xyz_points[i] = [x, y, self.height_map.height_at_xy(x, y) + 0.01]
self.visualize_dashed_line(xyz_points[0], xyz_points[1], "1-2", dashes)
self.visualize_dashed_line(xyz_points[0], xyz_points[2], "1-3", dashes)
self.visualize_dashed_line(xyz_points[1], xyz_points[2], "2-3", dashes)
print("Saving SDF to",fn)
geometryopt.dump_grid_mat(linkgeoms[i].grid,fn)
qinit = robot.getConfig()
vis.add("qsoln",qinit)
vis.setColor("qsoln",0,1,0,0.5)
#edit configuration as target
vis.edit(("world",robot.getName()))
#should we draw?
if DRAW_GRID_AND_PC:
for i,g in enumerate(constraints[0].robot.geometry):
if g is not None:
vis.add("grid "+str(i),g.grid)
vis.setColor("grid "+str(i),0,1,0,0.5)
vis.hideLabel("grid "+str(i))
#vis.add("pc "+str(i),g.pc)
#vis.setColor("pc "+str(i),1,1,0,0.5)
for i,obs in enumerate(obstaclegeoms):
vis.add("envpc "+str(i),obs.pc)
vis.setColor("envpc "+str(i),1,1,0,0.5)
vis.hideLabel("envpc "+str(i))
settings = None
settings = SemiInfiniteOptimizationSettings()
#if you use qinit = random-collision-free, you'll want to set this higher
settings.max_iters = 5
settings.minimum_constraint_value = -0.02
vis.addPlot("timing")
random.seed(seed)
np.random.seed(seed)
W.sampleInnerBound(s, i)
for i, s in enumerate(outer_sizes):
random.seed(seed)
np.random.seed(seed)
W.sampleOuterBound(s, i + 2)
innermax = max(len(W.inner_bounds) for W in hand_wrench_spaces.itervalues())
outermax = max(len(W.outer_bounds) for W in hand_wrench_spaces.itervalues())
decompmax = max(
len(W.convex_decomposition) for W in hand_wrench_spaces.itervalues())
assert decompmax == 1, "Can only handle convex wrench spaces at the moment"
vis.visualization._frontend.clearColor = (1, 1, 1, 1)
vis.add("world", world)
vis.hideLabel("world")
while True:
edit_config()
#test_stability_running_time()
#break
#print test_stability()
#print test_stability_traditional()
links = hand_types.keys()
indices = WrenchSpaceIndices(links, [0] * len(links), 'inner')
for i in xrange(innermax):
indices.indices = [i] * len(links)
sp = compute_support_polygon(indices, (0, 0, -9.8))
if sp is not None:
points = [(p[0], p[1], 0) for p in sp]
points.append(points[0])
name = "inner " + str(inner_sizes[i])
ctest2.clearx()
raw_input("Press enter to continue... ")
"""
timescale = 10
trajinit.times = [timescale * v for v in trajinit.times]
if trajinit is not trajsolved:
trajsolved.times = [timescale * v for v in trajsolved.times]
vis.add(
"Initial trajectory",
trajinit.getLinkTrajectory(
END_EFFECTOR_LINK,
0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION))
#vis.add("Initial trajectory",trajinit)
vis.setColor("Initial trajectory", 1, 1, 0, 0.5)
vis.hideLabel("Initial trajectory")
vis.setAttribute("Initial trajectory", "width", 2)
eetrajopt = trajsolved.getLinkTrajectory(
END_EFFECTOR_LINK, 0.1).getPositionTrajectory(END_EFFECTOR_LOCAL_POSITION)
vis.add("Solution trajectory", eetrajopt)
#vis.add("Solution trajectory",trajsolved)
vis.hideLabel("Solution trajectory")
vis.animate(("world", robot.getName()), trajsolved)
params[0] = sorted(params[0], key=lambda x: x[2])
for i, p in enumerate(params[0]):
link = p[0]
env = p[1]
t = p[2]
wpt = p[3:6]
eept = eetrajopt.eval(t * timescale)
def queryPRM(self, qI, qG):
vis.show()
vis.lock()
vis.add("Initial", [qI.x, qI.y, qI.z])
vis.setAttribute("Initial", "size", 14)
vis.setColor("Initial", 0, 0.4470, 0.7410)
vis.add("Goal", [qG.x, qG.y, qG.z])
vis.setAttribute("Goal", "size", 14)
vis.setColor("Goal", 0.8500, 0.3250, 0.0980)
vis.unlock()
vis.show(False)
if (self.localPlanner(qI, qG)):
print("Direct path found")
cnt = 0
tr = trajectory.Trajectory()
tr.milestones.append([qI.x, qI.y, qI.z])
tr.milestones.append([qG.x, qG.y, qG.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0, 0.4470, 0.7410)
cnt = cnt + 1
self.pathCnt = cnt
flagIn = True
self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht)
vis.show()
while (flagIn):
inText = ''
while not (inText == 'y' or inText == 'n'):
inText = raw_input("Run Path (y/n) ?")
if inText == 'n':
flagIn = False
break
if inText == 'y':
self.localPlanner(qI, qG, True)
return True
self.pathCnt = 0
#vis.show(False)
nodeList = list(self.G)
minNodeI = None
minDist = 100000
for nodeG in nodeList:
dist = self.distMetric(qI, nodeG)
if dist < self.maxDist:
if (self.localPlanner(qI, nodeG)):
if (minDist > dist):
minNodeI = nodeG
minDist = dist
if minNodeI == None:
return False
minNodeG = None
minDist = 100000
for nodeG in nodeList:
dist = self.distMetric(qG, nodeG)
if dist < self.maxDist:
if (self.localPlanner(qG, nodeG)):
if (minDist > dist):
minNodeG = nodeG
minDist = dist
if minNodeG == None:
return False
try:
shortest_path = nx.dijkstra_path(self.G,
source=minNodeI,
target=minNodeG)
except:
return False
shortest_path = self.smoothPath(shortest_path)
vis.show()
cnt = 0
tr = trajectory.Trajectory()
tr.milestones.append([qI.x, qI.y, qI.z])
tr.milestones.append([minNodeI.x, minNodeI.y, minNodeI.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0, 0.4470, 0.7410)
cnt = cnt + 1
prevNode = None
for nodeP in shortest_path:
if prevNode == None:
prevNode = nodeP
continue
tr = trajectory.Trajectory()
tr.milestones.append([prevNode.x, prevNode.y, prevNode.z])
tr.milestones.append([nodeP.x, nodeP.y, nodeP.z])
prevNode = nodeP
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
vis.setColor(fName, 0.4660, 0.6740, 0.1880)
cnt = cnt + 1
tr = trajectory.Trajectory()
tr.milestones.append([qG.x, qG.y, qG.z])
tr.milestones.append([minNodeG.x, minNodeG.y, minNodeG.z])
fName = "p" + str(cnt)
vis.add(fName, tr)
vis.hideLabel(fName)
#vis.setAttribute(fName,"width",1.0)
vis.setColor(fName, 0.8500, 0.3250, 0.0980)
cnt = cnt + 1
self.pathCnt = cnt
flagIn = True
self.env.setConfig(qI.x, qI.y, qI.z, qI.sc, qI.tht)
vis.show()
while (flagIn):
inText = ''
while not (inText == 'y' or inText == 'n'):
inText = raw_input("Run Path (y/n) ?")
if inText == 'n':
flagIn = False
break
if inText == 'y':
self.runPath(qI, minNodeI, minNodeG, qG, shortest_path)
return True
