def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w,vp.h = 400,600
vis.setViewport(vp)
vis.dialog()
vp.w,vp.h = 640,480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
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()
def run(self):
"""Standardized interface for running program"""
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()
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 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)
world = WorldModel()
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,
def oncamerainfo(ci):
vp = vis.getViewport()
kros.from_CameraInfo(ci, vp)
vis.setViewport(vp)
def determine_reachable_points_robot(self,
sampling_places,
world,
robot,
lamp,
collider,
show_vis=False,
neighborhood=0.4,
float_height=0.08,
base_linknum=2):
self.set_robot_link_collision_margins(robot, 0.15, collider, [])
show_vis = True
if (show_vis):
vis.add('world', world)
# eliminating draw distance
vis.lock()
# time.sleep(0.5)
vp = vis.getViewport()
# vp.h = 640
# vp.w = 640
vp.clippingplanes = [0.1, 10000]
tform = pickle.load(open('tform.p', 'rb'))
vp.setTransform(tform)
scale = 1
vp.w = 1853 // scale
vp.h = 1123 // scale
# vis.setViewport(vp)
vis.scene().setViewport(vp)
vis.unlock()
vis.show()
reachable = []
configs = []
# world.terrain(0).geometry().setCollisionMargin(0.05)
for place in tqdm(sampling_places):
robot.setConfig(place)
reachable.append(
self.solve_ik_near_sample(robot,
lamp,
collider,
world,
place,
restarts=10,
tol=1e-2,
neighborhood=neighborhood,
float_height=float_height))
# print('reachable? = {}'.format(reachable[-1]))
cfig = robot.getConfig()
# print(cfig[2])
configs.append(cfig + place.tolist())
# after checking for those margins, we reset the robot to its original configs for general motion planning
# self.set_robot_link_collision_margins(robot,0,collider,self.angular_dofs)
self.set_robot_link_collision_margins(robot, 0, collider, [])
# we also reset the base and environment collision to simplify path planning:
world.terrain(0).geometry().setCollisionMargin(0)
return reachable, configs
if not w.loadFile("../../data/simulation_test_worlds/sensortest.xml"):
print("Error loading test world?")
exit(1)
sim = Simulator(w)
robot = w.robot(0)
cam = sim.controller(0).sensor("rgbd_camera")
link = robot.link(robot.numLinks() - 1)
amplitudes = [random.uniform(0, 2) for i in range(robot.numLinks())]
phases = [random.uniform(0, math.pi * 2) for i in range(robot.numLinks())]
periods = [random.uniform(0.5, 2) for i in range(robot.numLinks())]
qmin, qmax = robot.getJointLimits()
vis.add("world", w)
vis.add("cam", cam)
vp = vis.getViewport()
vp.camera.rot[1] -= 0.5
vis.setViewport(vp)
default = vis.getViewport().getTransform()
print('x:', so3.apply(default[0], [1, 0, 0]))
print('y:', so3.apply(default[0], [0, 1, 0]))
print('z:', so3.apply(default[0], [0, 0, 1]))
print('offset:', default[1])
circle_points = []
npts = 50
times = []
for i in range(npts + 1):
angle = math.radians(360 * i / npts)
circle_points.append(
se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default))
times.append(i * 20 / npts)
def create(self, start_pc, goal_pc):
"""
This method cretes the simulation
:param start_pc: robot's initial position coordinate
:param goal_pc: goal position coordinate
:return:
"""
print "Creating the Simulator"
object_dir = "/home/jeet/PycharmProjects/DeepQMotionPlanning/"
self.start_pc = start_pc
self.goal_pc = goal_pc
coordinates.setWorldModel(self.world)
getDoubleRoomDoor(self.world, 8, 8, 1)
builder = Builder(object_dir)
# Create a goal cube
n_objects = 1
width = 0.1
depth = 0.1
height = 0.1
x = goal_pc[0]
y = goal_pc[1]
z = goal_pc[2] / 2
thickness = 0.005
color = (0.2, 0.6, 0.3, 1.0)
builder.make_objects(self.world, n_objects, "goal", width, depth,
height, thickness, self.terrain_limit, color,
self.goal_pc)
# Create a obstacle cube
n_objects = 4
width = 0.2
depth = 0.2
height = 0.2
x = 2.3
y = 0.8
z = goal_pc[2] / 2
thickness = 0.001
color = (0.8, 0.2, 0.2, 1.0)
builder.make_objects(self.world, n_objects, "rigid", width, depth,
height, thickness, self.terrain_limit, color)
self.vis = vis
vis.add("world", self.world)
# Create the view port
vp = vis.getViewport()
vp.w, vp.h = 1200, 900
vp.x, vp.y = 0, 0
vis.setViewport(vp)
# Create the robot
self.robot = sphero6DoF(self.world.robot(0), "", None)
self.robot.setConfig(start_pc)
# Create the axis representation
# vis.add("WCS", [so3.identity(), [0, 0, 0]])
# vis.setAttribute("WCS", "size", 24)
# Add text messages component for collision check and robot position
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
vis.addText("textStep", "Steps: ")
vis.setAttribute("textStep", "size", 24)
vis.addText("textGoalDistance", "Goal Distance: ")
vis.setAttribute("textGoalDistance", "size", 24)
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
self.collision_checker = collide.WorldCollider(self.world)
vis.setWindowTitle("Simulator")
vis.show()
return vis, self.robot, self.world
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")
mesh = w.terrain(0).geometry().getTriangleMesh()
terrain_pub = kros.object_publisher("terrain", mesh, latch=True, queue_size=10)
terrain_pub.publish(mesh)
pc = w.terrain(0).geometry().convert('PointCloud').getPointCloud()
pc_pub = kros.object_publisher("terrain_point_cloud",
pc,
latch=True,
queue_size=10)
pc_pub.publish(pc)
vis.add("world", w)
vis.show()
camera_pub = kros.object_publisher('camera_info',
vis.getViewport(),
latch=True,
queue_size=10)
camera_pub.publish(vis.getViewport())
sim = Simulator(w)
js_pub = kros.object_publisher("js",
sim.controller(0),
convert_kwargs={
'q': 'actual',
'dq': 'actual',
'effort': 'actual'
},
queue_size=10)
print("Simulating and publishing...")
def __init__(self, fn):
## Creates a world and loads all the items on the command line
self.world = WorldModel()
self.robotSystemName = 'O'
for f in fn:
print(f)
res = self.world.readFile(f)
if not res:
raise RuntimeError("Unable to load model " + fn)
self.showVis = False
coordinates.setWorldModel(self.world)
vis.lock()
bW.getDoubleRoomWindow(self.world, 8, 8, 1)
vis.unlock()
## Add the world to the visualizer
vis.add("world", self.world)
vp = vis.getViewport()
vp.w, vp.h = 1800, 800
vis.setViewport(vp)
self.robots = []
self.n = self.world.numRobots()
for i in range(self.n):
self.robots.append(
sphero6DoF(self.world.robot(i),
self.world.robot(i).getName()))
self.eps = 0.000001
self.sj = [[0, 0, 0], [0.2, 0, 0]]
self.sjStar = [[-0.070534, -0.097082, 0], [0, -0.06, 0],
[0.070534, -0.097082, 0], [0.057063, -0.018541, 0],
[0.11413, 0.037082, 0], [0.035267, 0.048541, 0],
[0, 0.12, 0], [-0.035267, 0.048541, 0],
[-0.11413, 0.037082, 0], [-0.057063, -0.018541, 0]]
self.sjL = [[0, 0, 0], [0, 0.2, 0], [0, 0.4, 0], [0, 0.6, 0],
[0, 0.8, 0], [0, 1, 0], [0.2, 0, 0], [0.4, 0, 0],
[0.6, 0, 0], [0.8, 0, 0]]
self.sj = self.sjL
self.xB = [-4, 4]
self.yB = [-4, 4]
self.zB = [0.02, 1]
self.rad = 0.04
self.currConfig = [0, 0, 1, 1, 0]
self.scMin = 1
self.scXMin = 1
self.scYMin = 2
self.sumDist = 0
if self.n > 1:
minSij = vectorops.norm(vectorops.sub(self.sj[0], self.sj[1]))
minSijX = math.fabs(self.sj[0][0] - self.sj[1][0])
minSijY = math.fabs(self.sj[0][1] - self.sj[1][1])
for i in range(self.n):
for j in range(self.n):
if i != j:
dist = vectorops.norm(
vectorops.sub(self.sj[i], self.sj[j]))
if dist < minSij:
minSij = dist
dist = math.fabs(self.sj[i][0] - self.sj[j][0])
if dist < minSijX:
minSijX = dist
dist = math.fabs(self.sj[i][1] - self.sj[j][1])
if dist < minSijY:
minSijY = dist
for i in range(self.n):
self.sumDist += vectorops.norm(self.sj[i])
if minSij > self.eps:
self.scMin = 2 * math.sqrt(2) * self.rad / minSij
if minSijX > self.eps:
self.scXMin = 2 * math.sqrt(2) * self.rad / minSijX
if minSijY > self.eps:
self.scYMin = 2 * math.sqrt(2) * self.rad / minSijY
self.scMax = max(2, self.scMin)
self.scB = [self.scMin, 2 * self.scMin]
print('Minimum scale')
print(self.scMin)
vis.add(self.robotSystemName, [so3.identity(), [10, 10, -10]])
vis.setAttribute(self.robotSystemName, "size", 12)
vis.edit(self.robotSystemName)
vis.add("WCS", [so3.identity(), [0, 0, 0]])
vis.setAttribute("WCS", "size", 32)
vis.edit("WCS")
self.collisionChecker = collide.WorldCollider(self.world)
if self.showVis:
## Display the world coordinate system
vis.addText("textCol", "No collision")
vis.setAttribute("textCol", "size", 24)
## On-screen text display
vis.addText("textConfig", "Robot configuration: ")
vis.setAttribute("textConfig", "size", 24)
vis.addText("textbottom",
"WCS: X-axis Red, Y-axis Green, Z-axis Blue",
(20, -30))
print "Starting visualization window#..."
## Run the visualizer, which runs in a separate thread
vis.setWindowTitle("Visualization for kinematic simulation")
vis.show()
simTime = 60
startTime = time.time()
oldTime = startTime
self.setConfig(0, 0, 1, 1, 0)
q = self.robots[0].getConfig()
if self.showVis:
q2f = ['{0:.2f}'.format(elem) for elem in q]
strng = "Robot configuration: " + str(q2f)
vis.addText("textConfig", strng)
colFlag = self.checkCollision()
print(colFlag)
if self.showVis:
time.sleep(10)
