def viewport_template(world):
"""Changes the parameters of the viewport and main window"""
#add the world to the visualizer
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
#this auto-sizes the camera
vis.autoFitCamera()
vis.setWindowTitle("Viewport modification test")
vis.spin(float('inf'))
vis.kill()
world_file = "../data/simulation_test_worlds/drc_rough_terrain_world.xml"
if not world.readFile(world_file):
raise RuntimeError("Unable to load terrain model")
robot_file = "../data/robot_model/robosimian_caesar_new.rob"
if not world.readFile(robot_file):
raise RuntimeError("Unable to load robot model")
vis_window_id = vis.createWindow(world_file)
vis.setWindow(vis_window_id)
vis.add("world", world)
vp = vis.getViewport()
vis.setViewport(vp)
vis.autoFitCamera()
world.robot(0).setConfig([
12.621508747630084,
1.3060978650033888,
0.7271994997360561,
-0.18389666460947365,
-0.2336561986984183,
0.23915345995072382,
0.0,
0.12877367095232392,
-0.24152711808937907,
-1.352324085841938,
-1.3962990011600755,
-1.6288249134647879,
1.0952385369647921,
def vis_world(self):
world = WorldModel()
self.rposer = None
res = world.readFile(self.world_file)
if not res:
raise RuntimeError("Unable to load terrain model")
res = world.readFile(self.robot_file)
if not res:
raise RuntimeError("Unable to load robot model")
vis.createWindow(self.worldname)
vis.setWindowTitle(self.worldname + " visualization")
vis.add("world", world)
vp = vis.getViewport()
# vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.autoFitCamera()
vis.show()
q = [
0,
0.0,
0.5, # torso x y z
0,
0,
0, # torso roll pitch yaw
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # fr
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # br
0,
0,
0,
-0.785398,
-1.5707,
-1.5707,
0.785398,
0, # bl
0,
0,
0,
0.785398,
1.5707,
1.5707,
-0.785398,
0, # fl
]
world.robot(0).setConfig(q)
def load_pcloud(self, save_file):
"""
Converts a geometry to another type, if a conversion is available. The
interpretation of param depends on the type of conversion, with 0
being a reasonable default.
Available conversions are:
PointCloud -> TriangleMesh, if the point cloud is structured. param is the threshold
for splitting triangles by depth discontinuity, by default infinity.
"""
long_np_cloud = np.fromfile(save_file)
print(long_np_cloud.shape, ": shape of long numpy cloud")
num_points = long_np_cloud.shape[0] / 3
np_cloud = np.zeros(shape=(num_points, 3))
pcloud = klampt.PointCloud()
scaling_factor = 0.1
points = []
xs = []
ys = []
zs = []
for x in range(num_points):
i = x * 3
x_val = long_np_cloud[i] * scaling_factor
y_val = long_np_cloud[i + 1] * scaling_factor
z_val = long_np_cloud[i + 2] * scaling_factor
np_cloud[x][0] = x_val
np_cloud[x][1] = y_val
np_cloud[x][2] = z_val
xs.append(x_val)
ys.append(y_val)
zs.append(z_val)
points.append(np_cloud[x])
points.sort(key=lambda tup: tup[2])
x_sorted = sorted(xs) # sorted
y_sorted = sorted(ys) # sorted
z_sorted = sorted(zs) # sorted
xfit = stats.norm.pdf(x_sorted, np.mean(x_sorted), np.std(x_sorted))
yfit = stats.norm.pdf(y_sorted, np.mean(y_sorted), np.std(y_sorted))
zfit = stats.norm.pdf(z_sorted, np.mean(z_sorted), np.std(z_sorted))
# plot with various axes scales
plt.figure(1)
# linear
plt.subplot(221)
plt.plot(x_sorted, xfit)
plt.hist(x_sorted, normed=True)
plt.title("X values")
plt.grid(True)
plt.subplot(222)
plt.plot(y_sorted, yfit)
plt.hist(y_sorted, normed=True)
plt.title("Y values")
plt.grid(True)
plt.subplot(223)
plt.plot(z_sorted, zfit)
plt.hist(z_sorted, normed=True)
plt.title("Z values")
plt.grid(True)
# Format the minor tick labels of the y-axis into empty strings with
# `NullFormatter`, to avoid cumbering the axis with too many labels.
plt.gca().yaxis.set_minor_formatter(NullFormatter())
# Adjust the subplot layout, because the logit one may take more space
# than usual, due to y-tick labels like "1 - 10^{-3}"
plt.subplots_adjust(top=0.92,
bottom=0.08,
left=0.10,
right=0.95,
hspace=0.25,
wspace=0.35)
# plt.show()
median_z = np.median(zs)
threshold = 0.25 * scaling_factor
for point in points:
if np.fabs(point[2] - median_z) < threshold:
pcloud.addPoint(point)
print(pcloud.numPoints(), ": num points")
# Visualize
pcloud.setSetting("width", "3")
pcloud.setSetting("height", str(len(points) / 3))
g3d_pcloud = Geometry3D(pcloud)
mesh = g3d_pcloud.convert("TriangleMesh", 0)
world = robotsim.WorldModel()
vis.add("world", world)
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("PointCloud World")
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.autoFitCamera()
vis.show()
vis.lock()
vis.add("mesh", mesh)
vis.unlock()
while vis.shown():
time.sleep(0.01)
vis.kill()
print("done")