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 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()
return True
if __name__ == "__main__":
if len(sys.argv) <= 1:
print "USAGE: kinematicSim.py [world_file]"
exit()
## Creates a world and loads all the items on the command line
world = WorldModel()
for fn in sys.argv[1:]:
res = world.readFile(fn)
if not res:
raise RuntimeError("Unable to load model " + fn)
coordinates.setWorldModel(world)
## A rooms separated by a wall with a door
bW.getRoom(world, 8, 8, 1)
## Add the world to the visualizer
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 1200, 800
vis.setViewport(vp)
## Create robot object. Change the class to the desired robot.
## Also, make sure the robot class corresponds to the robot in simpleWorld.xml file
#robot = kobuki(world.robot(0), vis)
#robot.setAltitude(0.01)
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 __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)