def __init__(self, world, object):
GLWidgetPlugin.__init__(self)
from klampt.model.collide import WorldCollider
from klampt.plan.rigidobjectcspace import RigidObjectCSpace
from klampt import ObjectPoser
self.world = world
self.object = object
self.space = RigidObjectCSpace(object, collider=WorldCollider(world))
self.costFunction = None
self.start = None
self.goal = None
self.startWidget = ObjectPoser(self.object)
self.goalWidget = ObjectPoser(self.object)
self.addWidget(self.startWidget)
self.addWidget(self.goalWidget)
self.optimizingPlanner = True
self.path = []
self.G = None
def setup_robot_and_light(
self,
robotfile='./data/idealbot.rob',
mesh_file='./data/environment_meshes/full_detail_hospital_cad_meters.obj',
float_height=0.08):
world = WorldModel()
res1 = world.loadElement(robotfile)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
qmin = np.nan_to_num(qmin, posinf=1000, neginf=-100)
qmax = np.nan_to_num(qmax, posinf=1000, neginf=-100)
robot.setJointLimits(qmin, qmax)
#world.loadElement(robotfile)
#a = Geometry3D()
# res = a.loadFile(mesh_file)
res = world.loadElement(mesh_file)
print(res)
collider = WorldCollider(world)
#we then dilate the base nd ignore collisions between it and the 2 subsequent links:
# collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3)))
# collider.ignoreCollision((robot.link(self.base_height_link),robot.link(3)))
# collider.ignoreCollision((robot.link(8),robot.link(6)))
# we now
cfig = robot.getConfig()
terrain = world.terrain(0)
lamp = robot.link(self.lamp_linknum)
print('\n\n\nbase height link = {}, lamp linknum = {}\n\n\n'.format(
self.base_height_link, self.lamp_linknum))
cfig[self.base_height_link] = float_height
robot.setConfig(cfig)
robot.link(self.lamp_linknum).appearance().setColor(
210 / 255, 128 / 255, 240 / 255, 1)
world.saveFile('disinfection.xml')
return world, robot, lamp, collider
print("#########################################")
print("Showing the simplified robot")
print("#########################################")
vis.setWindowTitle("Simplified robot")
vis.dialog()
#Automatic construction of space
if not CLOSED_LOOP_TEST:
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-3,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-3
space.setup()
else:
#TESTING: closed loop robot cspace
collider = WorldCollider(world)
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
vis.add("IK goal", obj)
vis.dialog()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-3
space.setup()
def __init__(self, world):
self.world=world
self.robot=world.robot(0)
self.collider=WorldCollider(world)