def __init__(self,h):
GLPluginInterface.__init__(self)
self.hand = h
h.load_frames()
self.world = WorldModel()
#load robot
robot = moving_base.make_moving_base_robot(h.robot_file,self.world,floating=True)
#load object
id = self.world.loadElement(object_file)
if id < 0:
raise IOError("Unable to load "+object_file)
assert self.world.numRigidObjects() > 0
object = self.world.rigidObject(0)
object.setTransform(so3.rotation([1,0,0],math.pi/2),[0,0,0.2])
#set up unit frames
coordinates.setWorldModel(self.world)
coordinates.listItems()
hand_coordinates = coordinates.manager().subgroups[robot.getName()]
hand_coordinates.listItems()
self.unit_frames = []
for i,(key,unit) in enumerate(h.all_units):
self.unit_frames.append(hand_coordinates.addFrame("unit"+str(i),parent=hand_coordinates.frame(key),relativeCoordinates=unit.localTransform))
#set up visualization
vis.add("robot",robot)
vis.edit("robot")
vis.add("rigidObject",object)
vis.edit("rigidObject")
print "COM",object.getMass().getCom()
vis.add("COM",se3.apply(object.getTransform(),object.getMass().getCom()))
vis.add("origin",se3.identity())
#vis.add("coordinates",coordinates.manager())
self.config_name = None
#set up friction solver
folder="FrictionCones/"
self.friction_factories = {}
for key,unit in h.all_units:
if unit.type not in self.friction_factories:
self.friction_factories[unit.type] = stability.GeneralizedFrictionConeFactory()
self.friction_factories[unit.type].load(folder + unit.type)
self.contact_units = []
self.wrenches = None
self.torques = None
self.feasible = None
self.angle = 0
self.wrenchSpace = None
self.wrenchSpace6D = None
self.frame_gl_objects = []
self.wrench_space_gl_object = None
def visualize(self):
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.unlock()
while vis.shown():
time.sleep(0.01)
vis.kill()
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def visualize_pc_in_klampt_vis(self, pcloud_fname):
title = pcloud_fname + " klampt world"
vis_window_id = vis.createWindow(title)
vis.setWindowTitle(title)
world = WorldModel()
vis.add("world", world)
pcd = o3d.io.read_point_cloud(pcloud_fname)
print(pcd)
pc_xyzs = np.asarray(pcd.points)
pc_xyzs_as_list = pc_xyzs.flatten().astype("float")
# pc_xyzs_as_list = np.array([1,0,0, 1.1, 0, 0, 0, 1, 0])
pcloud = PointCloud()
pcloud.setPoints(int(len(pc_xyzs_as_list) / 3), pc_xyzs_as_list)
print(pcloud.numPoints())
vis.add("pcloud", pcloud)
# vis.setColor("pcloud", 0, 0, 1, a=1)
# vis.setAttribute("p1", "size", 5.0)
box = klampt.GeometricPrimitive()
box.setAABB([-1, -1, 0], [-0.9, -0.9, 0.2])
g = klampt.Geometry3D(box)
vis.add("box", g)
vis.setColor("box", 0, 0, 1, 0.5)
coordinates.setWorldModel(world)
vis.add("coordinates", coordinates.manager())
vis.show()
while vis.shown():
vis.lock()
vis.unlock()
time.sleep(0.01)
vis.kill()
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")
if __name__ == "__main__":
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
linkindex = 7
localpos = (0.17, 0, 0)
robot = world.robot(0)
link = robot.link(linkindex)
coordinates.setWorldModel(world)
goalpoint = [0, 0, 0]
ptlocal = coordinates.addPoint("ik-constraint-local", localpos,
robot.getName() + ":" + link.getName())
ptworld = coordinates.addPoint("ik-constraint-world", goalpoint, "world")
print coordinates.manager().frames.keys()
vis.add("robot", robot)
vis.add("coordinates", coordinates.manager())
vis.show()
iteration = 0
while vis.shown():
vis.lock()
#set the desired position goalpoint to move in a circle
r = 0.4
t = vis.animationTime()
goalpoint[0], goalpoint[1], goalpoint[2] = 0.8, r * math.cos(
t), 0.7 + r * math.sin(t)
q = solve_ik(link, localpos, goalpoint)
robot.setConfig(q)
#this updates the coordinates module
if __name__ == "__main__":
world = WorldModel()
res = world.readFile("ex2_file.xml")
if not res: raise RuntimeError("Unable to load world file")
linkindex = 7
localpos = (0.17, 0, 0)
robot = world.robot(0)
link = robot.link(linkindex)
coordinates.setWorldModel(world)
goalpoint = [0, 0, 0]
ptlocal = coordinates.addPoint("ik-constraint-local", localpos,
robot.getName() + ":" + link.getName())
ptworld = coordinates.addPoint("ik-constraint-world", goalpoint, "world")
print(list(coordinates.manager().frames.keys()))
vis.add("robot", robot)
vis.add("coordinates", coordinates.manager())
vis.show()
iteration = 0
while vis.shown():
vis.lock()
#set the desired position goalpoint to move in a circle
r = 0.4
t = vis.animationTime()
goalpoint[0], goalpoint[1], goalpoint[2] = 0.8, r * math.cos(
t), 0.7 + r * math.sin(t)
q = solve_ik(link, localpos, goalpoint)
robot.setConfig(q)
#this updates the coordinates module