Go to the documentation of this file.00001 import openravepy
00002 from openravepy.ikfast import *
00003 import sys
00004 import os
00005 import logging
00006 import yaml
00007 import subprocess
00008 import roslib
00009
00010 if __name__ == '__main__':
00011 settings = yaml.load(open(sys.argv[1]))
00012 print settings
00013 format = logging.Formatter('%(levelname)s: %(message)s')
00014 handler = logging.StreamHandler(sys.stdout)
00015 handler.setFormatter(format)
00016 log.addHandler(handler)
00017 log.setLevel(logging.INFO)
00018
00019 convpath = os.path.join(os.path.dirname(sys.argv[0]),'urdf_openrave')
00020 xml = subprocess.check_output(convpath+" --urdf %s %s %s %s "% (settings['urdf_path'],settings['name'],settings['base_link'],settings['tip_link']),shell=True)
00021 try:
00022 env=openravepy.Environment()
00023 kinbody=env.ReadRobotXMLData(xml)
00024 env.Add(kinbody)
00025 links = [str(l.GetName()) for l in kinbody.GetLinks()]
00026 print links
00027 joints = [str(j.GetName()) for j in kinbody.GetJoints()]
00028 print joints
00029 solver = IKFastSolver(kinbody,kinbody)
00030 chaintree = solver.generateIkSolver(links.index(settings['base_link']),links.index(settings['tip_link']),[joints.index(f) for f in settings['free_joints']])
00031 code=solver.writeIkSolver(chaintree,lang='cpp')
00032 finally:
00033 openravepy.RaveDestroy()
00034 open(sys.argv[2],'w').write(code)