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])