def fromJson(self, object):
"""Sets this IK problem to a JSON object representing it. A ValueError
is raised if it is not the correct type."""
if object['type'] != 'IKProblem':
raise ValueError("Object must have type IKProblem")
self.objectives = []
for obj in object['objectives']:
self.objectives.append(loader.fromJson(obj))
if 'softObjectives' in object:
self.softObjectives = object['softObjectives']
else:
self.softObjectives = False
self.activeDofs = object.get('activeDofs', None)
self.jointLimits = object.get('jointLimits', None)
self.costFunctionDescription = object.get('costFunction', None)
if self.costFunctionDescription is None:
self.costFunction = None
else:
self.costFunction = functionfactory.makeFunction(
self.costFunctionDescription['type'],
self.costFunctionDescription['args'])
self.feasibilityTestDescription = object.get('feasibilityTest', None)
if self.feasibilityTestDescription is None:
self.feasibilityTest = None
else:
if isinstance(self.feasibilityTestDescription, dict):
self.feasibilityTest = functionfactory.makeFunction(
self.feasibilityTestDescription['type'],
self.feasibilityTestDescription['args'])
else:
self.feasibilityTest = functionfactory.andFunction(*[
functionfactory.makeFunction(test['type'], test['args'])
for test in self.feasibilityTestDescription
])
return
def run_tests_main(ikobjectivejson,ikfeatures):
"""A basic tester that runs a given ik feature as configured by the command line arguments. Runs a given
IK objective on one or more links"""
#make the IK template from the json spec
objectives = []
if len(ns.link)==0:
print "No link specified, using the last link in the robot file"
ns.link = [robot.numLinks()-1]
for i,link in enumerate(ns.link):
try:
link = int(link)
except:
pass
#set up the IK problem templates
link = robot.link(link)
ikobjectivejson['link'] = link.index
ikobjectivejson['type'] = 'IKObjective'
ikobjectivejson['localPosition'] = ns.localpoint
obj = loader.fromJson(ikobjectivejson)
obj.robot = robot
objectives.append(obj)
#generate some IK problem
res = ikdb.solve(objectives,feasibilityCheck=ns.feasible,costFunction=ns.cost)
print "res:",res
def fromJson(self,object):
"""Sets this IK problem to a JSON object representing it. A ValueError
is raised if it is not the correct type."""
if object['type'] != 'IKProblem':
raise ValueError("Object must have type IKProblem")
self.objectives = []
for obj in object['objectives']:
self.objectives.append(loader.fromJson(obj))
if 'softObjectives' in object:
self.softObjectives = object['softObjectives']
else:
self.softObjectives = False
self.activeDofs = object.get('activeDofs',None)
self.jointLimits = object.get('jointLimits',None)
self.costFunctionDescription = object.get('costFunction',None)
if self.costFunctionDescription is None:
self.costFunction = None
else:
self.costFunction = functionfactory.makeFunction(self.costFunctionDescription['type'],self.costFunctionDescription['args'])
self.feasibilityTestDescription = object.get('feasibilityTest',None)
if self.feasibilityTestDescription is None:
self.feasibilityTest = None
else:
if isinstance(self.feasibilityTestDescription,dict):
self.feasibilityTest = functionfactory.makeFunction(self.feasibilityTestDescription['type'],self.feasibilityTestDescription['args'])
else:
self.feasibilityTest = functionfactory.andFunction(*[functionfactory.makeFunction(test['type'],test['args']) for test in self.feasibilityTestDescription])
return
def featuresToIkObjective(ikgoal0, featureList, values):
"""Given an IKObjective "template", a list of features, and a list of values,
returns a new IKObjective whose features are set to the list of values"""
jsonObj = loader.toJson(ikgoal0)
features.inject(jsonObj, featureList, values)
obj = loader.fromJson(jsonObj, type='IKObjective')
if hasattr(ikgoal0, 'robot'):
obj.robot = ikgoal0.robot
return obj
def featuresToIkObjective(ikgoal0,featureList,values):
"""Given an IKObjective "template", a list of features, and a list of values,
returns a new IKObjective whose features are set to the list of values"""
jsonObj = loader.toJson(ikgoal0)
features.inject(jsonObj,featureList,values)
obj = loader.fromJson(jsonObj,type='IKObjective')
if hasattr(ikgoal0,'robot'):
obj.robot = ikgoal0.robot
return obj
def add(self, fn, openDir=True, warn=True):
#assert fn not in self.active
if fn in self.active:
print("add(): Warning, file", fn, "is already active")
return
for i, (cfn, citem) in enumerate(self.visCache):
if cfn == fn:
print()
print("klampt_browser: PULLED", fn, "FROM CACHE")
print()
self.active[fn] = citem
return True
if len(self.active) >= MAX_VIS_ITEMS:
return
if os.path.isdir(fn):
if openDir:
failures = []
successes = []
for f in os.listdir(fn):
if f not in ['.', '..'] and os.path.splitext(f)[1] != '':
if not self.add(os.path.join(fn, f),
openDir=False,
warn=False):
failures.append(f)
else:
successes.append(f)
if len(failures) != 0 and len(successes) != 0:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid items",
"Could not load files " + ', '.join(failures) +
" as Klamp't elements")
return True
else:
return False
path, ext = os.path.splitext(fn)
#print "Extension is",ext
if ext in world_item_extensions:
try:
worldid = self.tempWorld.loadElement(fn)
except Exception:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid item",
"Could not load " + fn + " as a Klamp't world element")
return False
if worldid < 0:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid item",
"Could not load " + fn + " as a Klamp't world element")
return False
obj = None
for i in range(self.tempWorld.numRobots()):
if self.tempWorld.robot(i).getID() == worldid:
obj = self.tempWorld.robot(i)
break
for i in range(self.tempWorld.numRigidObjects()):
if self.tempWorld.rigidObject(i).getID() == worldid:
obj = self.tempWorld.rigidObject(i)
break
for i in range(self.tempWorld.numTerrains()):
if self.tempWorld.terrain(i).getID() == worldid:
obj = self.tempWorld.terrain(i)
break
assert obj is not None, "Hmm... couldn't find world id %d in world?" % (
worldid, )
self.loadedItem(fn, obj)
return True
try:
type = loader.filenameToType(fn)
except RuntimeError:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid item",
"Could not load file " + fn + " as a known Klamp't type")
return False
if type == 'xml':
#try loading a world
try:
world = WorldModel()
res = world.readFile(fn)
if not res:
try:
obj = loader.load('MultiPath', fn)
except Exception as e:
if warn:
print(
"klampt_browser: Trying MultiPath load, got exception",
e)
import traceback
traceback.print_exc()
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid WorldModel",
"Could not load " + fn +
" as a world XML file")
return False
self.loadedItem(fn, obj)
return True
except IOError:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid WorldModel",
"Could not load " + fn + " as a world XML file")
return False
self.loadedItem(fn, world)
return
elif type == 'json':
import json
f = open(fn, 'r')
jsonobj = json.load(f)
try:
obj = loader.fromJson(jsonobj)
except Exception:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid JSON", "Could not recognize " +
fn + " as a known Klamp't type")
return False
else:
try:
obj = loader.load(type, fn)
except Exception as e:
if warn:
QtWidgets.QMessageBox.warning(
self.splitter, "Invalid item",
"Error while loading file " + fn + ": " + str(e))
return False
self.loadedItem(fn, obj)
return True
def run_tests_main(ikobjectivejson,ikfeatures):
"""A basic tester that runs a given ik feature as configured by the command line arguments. Runs a given
IK objective on one or more links"""
import argparse
default_parser = argparse.ArgumentParser(description='Generate an IK database for a point-constrained IK problem.')
default_parser.add_argument('-t','--train',metavar='N',type=int,help="# of training examples",default=1000)
default_parser.add_argument('-T','--test',metavar='N',type=int,help="# of testing examples",default=1000)
default_parser.add_argument('-r','--robot',metavar='ROBOT',type=str,help="Robot or world file",default="../../Klampt/data/robots/tx90ball.rob")
default_parser.add_argument('-f','--file',metavar='FILE',type=str,help="DB file name",default="auto")
default_parser.add_argument('-g','--testfile',metavar='FILE',type=str,help="Test file name",default="auto")
default_parser.add_argument('--redotrain',action='store_const',const=1,help="Re-train the database",default=0)
default_parser.add_argument('--redotest',action='store_const',const=1,help="Re-generate the test set",default=0)
default_parser.add_argument('--metriclearn',metavar='N',type=int,help="Number of iterations of metric learning",default=0)
default_parser.add_argument('-k','--neighbors',metavar='K',action='append',type=int,help="Number of nearest neighbors for prediction",default=[1,5,10])
default_parser.add_argument('--RR',metavar='K',action='append',type=int,help="Number of restarts to test",default=[])
default_parser.add_argument('--cost',metavar='NAME',type=str,help="Name of objective function",default='jointRangeCost')
default_parser.add_argument('--feasible',metavar='NAME',type=str,help="Name of feasibility test function",default='collisionFree')
default_parser.add_argument('-p','--localpoint',metavar='V',action='append',type=float,nargs=3,help="Local point of IK constraint",default=[0,0,0])
default_parser.add_argument('-l','--link',metavar='N',action='append',help="Index or name of robot link(s)",default=[])
ns = default_parser.parse_args(sys.argv[1:])
world = WorldModel()
world.readFile(ns.robot)
robot = world.robot(0)
#setup the set of possible functions
functionfactory.registerDefaultFunctions()
functionfactory.registerCollisionFunction(world)
functionfactory.registerJointRangeCostFunction(robot)
#make the IK template, and its ranges and features
objectives = []
features = []
ranges = []
if len(ns.link)==0:
print "No link specified, using the last link in the robot file"
ns.link = [robot.numLinks()-1]
for i,link in enumerate(ns.link):
try:
link = int(link)
except:
pass
#set up the IK problem templates
link = robot.link(link)
ikobjectivejson['link'] = link.index
ikobjectivejson['type'] = 'IKObjective'
ikobjectivejson['localPosition'] = ns.localpoint
obj = loader.fromJson(ikobjectivejson)
obj.robot = robot
objectives.append(obj)
if isinstance(ikfeatures,(list,tuple)):
for f in ikfeatures:
features.append(('objectives',i,f))
else:
features.append(('objectives',i,ikfeatures))
#auto-detect position range via random sampling
linkrange = default_position_range(link,ns.localpoint)
linkrange = linkrange[:obj.numPosDims()]
linkrange += [(-math.pi,math.pi)]*obj.numRotDims()
ranges += linkrange
template = IKProblem(*objectives)
#initialize cost function and feasibility test
template.setFeasibilityTest(ns.feasible,None)
if ns.cost == 'jointRangeCost':
qmin,qmax = robot.getJointLimits()
template.setCostFunction('jointRangeCost_dynamic',[qmin,qmax])
else:
template.setCostFunction(ns.cost,None)
tester = make_ikdb_tester(robot,template,features,ranges,
ns.train,ns.test,
ns.file,ns.testfile,
ns.redotrain,ns.redotest)
if ns.metriclearn > 0:
for i in range(10):
tester.db.metricLearn(ns.metriclearn/10)
print "Round",i,"matrix:",tester.db.metricMatrix
#run the testing
for N in ns.RR:
tester.testRaw(N)
for k in ns.neighbors:
tester.testDB(k)
def run_tests_main(ikobjectivejson, ikfeatures):
"""A basic tester that runs a given ik feature as configured by the command line arguments. Runs a given
IK objective on one or more links"""
import argparse
default_parser = argparse.ArgumentParser(
description=
'Generate an IK database for a point-constrained IK problem.')
default_parser.add_argument('-t',
'--train',
metavar='N',
type=int,
help="# of training examples",
default=1000)
default_parser.add_argument('-T',
'--test',
metavar='N',
type=int,
help="# of testing examples",
default=1000)
default_parser.add_argument(
'-r',
'--robot',
metavar='ROBOT',
type=str,
help="Robot or world file",
default="../../Klampt/data/robots/tx90ball.rob")
default_parser.add_argument('-f',
'--file',
metavar='FILE',
type=str,
help="DB file name",
default="auto")
default_parser.add_argument('-g',
'--testfile',
metavar='FILE',
type=str,
help="Test file name",
default="auto")
default_parser.add_argument('--redotrain',
action='store_const',
const=1,
help="Re-train the database",
default=0)
default_parser.add_argument('--redotest',
action='store_const',
const=1,
help="Re-generate the test set",
default=0)
default_parser.add_argument('--metriclearn',
metavar='N',
type=int,
help="Number of iterations of metric learning",
default=0)
default_parser.add_argument(
'-k',
'--neighbors',
metavar='K',
action='append',
type=int,
help="Number of nearest neighbors for prediction",
default=[1, 5, 10])
default_parser.add_argument('--RR',
metavar='K',
action='append',
type=int,
help="Number of restarts to test",
default=[])
default_parser.add_argument('--cost',
metavar='NAME',
type=str,
help="Name of objective function",
default='jointRangeCost')
default_parser.add_argument('--feasible',
metavar='NAME',
type=str,
help="Name of feasibility test function",
default='collisionFree')
default_parser.add_argument('-p',
'--localpoint',
metavar='V',
action='append',
type=float,
nargs=3,
help="Local point of IK constraint",
default=[0, 0, 0])
default_parser.add_argument('-l',
'--link',
metavar='N',
action='append',
help="Index or name of robot link(s)",
default=[])
ns = default_parser.parse_args(sys.argv[1:])
world = WorldModel()
world.readFile(ns.robot)
robot = world.robot(0)
#setup the set of possible functions
functionfactory.registerDefaultFunctions()
functionfactory.registerCollisionFunction(world)
functionfactory.registerJointRangeCostFunction(robot)
#make the IK template, and its ranges and features
objectives = []
features = []
ranges = []
if len(ns.link) == 0:
print "No link specified, using the last link in the robot file"
ns.link = [robot.numLinks() - 1]
for i, link in enumerate(ns.link):
try:
link = int(link)
except:
pass
#set up the IK problem templates
link = robot.link(link)
ikobjectivejson['link'] = link.index
ikobjectivejson['type'] = 'IKObjective'
ikobjectivejson['localPosition'] = ns.localpoint
obj = loader.fromJson(ikobjectivejson)
obj.robot = robot
objectives.append(obj)
if isinstance(ikfeatures, (list, tuple)):
for f in ikfeatures:
features.append(('objectives', i, f))
else:
features.append(('objectives', i, ikfeatures))
#auto-detect position range via random sampling
linkrange = default_position_range(link, ns.localpoint)
linkrange = linkrange[:obj.numPosDims()]
linkrange += [(-math.pi, math.pi)] * obj.numRotDims()
ranges += linkrange
template = IKProblem(*objectives)
#initialize cost function and feasibility test
template.setFeasibilityTest(ns.feasible, None)
if ns.cost == 'jointRangeCost':
qmin, qmax = robot.getJointLimits()
template.setCostFunction('jointRangeCost_dynamic', [qmin, qmax])
else:
template.setCostFunction(ns.cost, None)
tester = make_ikdb_tester(robot, template, features, ranges, ns.train,
ns.test, ns.file, ns.testfile, ns.redotrain,
ns.redotest)
if ns.metriclearn > 0:
for i in range(10):
tester.db.metricLearn(ns.metriclearn / 10)
print "Round", i, "matrix:", tester.db.metricMatrix
#run the testing
for N in ns.RR:
tester.testRaw(N)
for k in ns.neighbors:
tester.testDB(k)