def loadCalibrateConfigs(fn):
"""Loads a .configs.file into a list of configurations"""
f = open(fn, 'r')
res = []
for line in f.readlines():
res.append(readVector(line))
return res
def main():
"""The main loop that loads the models and starts the OpenGL visualizer"""
world = WorldModel()
#uncomment these lines and comment out the next 2 if you want to use the
#full Baxter model
#print "Loading full Baxter model (be patient, this will take a minute)..."
#world.loadElement(os.path.join(model_dir,"baxter.rob"))
print "Loading simplified Baxter model..."
world.loadElement(os.path.join(model_dir,"baxter_col.rob"))
print "Loading Kiva pod model..."
world.loadElement(os.path.join(model_dir,"kiva_pod/model.obj"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
#shift the Baxter up a bit (95cm)
Rbase,tbase = world.robot(0).getLink(0).getParentTransform()
world.robot(0).getLink(0).setParentTransform(Rbase,(0,0,0.95))
world.robot(0).setConfig(world.robot(0).getConfig())
#translate pod to be in front of the robot, and rotate the pod by 90 degrees
Trel = (so3.rotation((0,0,1),-math.pi/2),[1.2,0,0])
T = world.rigidObject(0).getTransform()
world.rigidObject(0).setTransform(*se3.mul(Trel,T))
#load the resting configuration from klampt_models/baxter_rest.config
global baxter_rest_config
f = open(model_dir+'baxter_rest.config','r')
baxter_rest_config = loader.readVector(f.readline())
f.close()
world.robot(0).setConfig(baxter_rest_config)
#run the visualizer
visualizer = MyGLViewer(world)
visualizer.run()
def main():
"""The main loop that loads the planning / simulation models and
starts the OpenGL visualizer."""
world = load_apc_world()
init_ground_truth()
if NO_SIMULATION_COLLISIONS:
simworld = load_baxter_only_world()
else:
simworld = load_apc_world()
spawn_objects_from_ground_truth(simworld)
#load the resting configuration from klampt_models/baxter_rest.config
global baxter_rest_config
f = open(model_dir+'baxter_with_parallel_gripper_rest.config','r')
baxter_rest_config = loader.readVector(f.readline())
f.close()
simworld.robot(0).setConfig(baxter_rest_config)
#run the visualizer
visualizer = MyGLViewer(simworld,world)
visualizer.run()
def loadCalibrateConfigs(fn):
"""Loads a .configs.file into a list of configurations"""
f = open(fn,'r')
res = []
for line in f.readlines():
res.append(readVector(line))
return res
def main():
"""The main loop that loads the planning / simulation models and
starts the OpenGL visualizer."""
world = load_apc_world()
init_ground_truth()
#Question 1,2,3: below line should be uncommented
simworld = load_baxter_only_world()
#Question 4: comment out line above, uncomment lines below
#simworld = load_apc_world()
#spawn_objects_from_ground_truth(simworld)
#load the resting configuration from klampt_models/baxter_rest.config
global baxter_rest_config
f = open(model_dir+'baxter_with_parallel_gripper_rest.config','r')
baxter_rest_config = loader.readVector(f.readline())
f.close()
simworld.robot(0).setConfig(baxter_rest_config)
#run the visualizer
visualizer = MyGLViewer(simworld,world)
visualizer.run()
starts the OpenGL visualizer."""
world = load_apc_world()
init_ground_truth()
# TODO
if NO_SIMULATION_COLLISIONS:
# simworld = load_baxter_only_world()
simworld = load_apc_world()
else:
simworld = load_apc_world()
spawn_objects_from_ground_truth(simworld)
#load the resting configuration from klampt_models/baxter_rest.config
# global baxter_rest_config
f = open(model_dir+'baxter_with_parallel_gripper_rest.config','r')
baxter_rest_config = loader.readVector(f.readline())
f.close()
simworld.robot(0).setConfig(baxter_rest_config)
#run the visualizer
visualizer = MyGLViewer(simworld,world)
visualizer.run()
###############################################################
# WRITTEN ANSWERS / COMMENTS:
# Q1.
#
#
# Q2.
#
import sys
if len(sys.argv) != 4:
print "Usage: robot_to_mesh robot config mesh"
exit(0)
robotfn = sys.argv[1]
configfn = sys.argv[2]
meshfn = sys.argv[3]
#load the robot file
world = klampt.WorldModel()
robot = world.loadRobot(robotfn)
if robot.getName()=='':
print "Unable to load robot file",robotfn
exit(1)
fconfig = open(configfn,'r')
for line in fconfig:
q = loader.readVector(line)
robot.setConfig(q)
break
fconfig.close()
unionPoints = []
unionTris = []
for i in range(robot.numLinks()):
T = robot.getLink(i).getTransform()
geom = robot.getLink(i).getGeometry()
t = geom.type()
if t=="TriangleMesh":
trimesh = geom.getTriangleMesh()
tris = []
for index in xrange(0,len(trimesh.indices),3):
tris.append(trimesh.indices[index:index+3])
