00001
00002
00003 PKG = 'openhrp3'
00004 NAME = 'modelloader-test'
00005
00006 try:
00007 import openhrp3
00008 except:
00009 import roslib; roslib.load_manifest("openhrp3")
00010
00011
00012
00013 from omniORB import CORBA, any, cdrUnmarshal, cdrMarshal
00014 import CosNaming
00015
00016 import sys, os, socket, subprocess
00017
00018 import OpenRTM_aist
00019 from OpenHRP import *
00020
00021 import rospkg
00022 import unittest
00023 import rostest
00024
00025 rootrc = None
00026
00027 def initCORBA():
00028 global rootnc
00029
00030
00031
00032
00033
00034 orb = CORBA.ORB_init(sys.argv, CORBA.ORB_ID)
00035
00036 nameserver = orb.resolve_initial_references("NameService");
00037 rootnc = nameserver._narrow(CosNaming.NamingContext)
00038
00039 def findModelLoader():
00040 global rootnc
00041 try:
00042 obj = rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
00043 return obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
00044 except:
00045 print "Could not find ModelLoader", sys.exc_info()[0]
00046 exit
00047
00048 def equal(a, b, tol = 1e-4):
00049 if type(a) == float and type(b) == float:
00050 return abs(a - b) < tol
00051 if type(a) == list and type(b) == list:
00052 return all(equal(x,y) for (x,y) in zip(a,b))
00053 else:
00054 return True
00055
00056 def norm(a):
00057 r = 0
00058 for e in a:
00059 r = r + e*e
00060 return r/len(a)
00061
00062 class TestModelLoaderBase(unittest.TestCase):
00063 ml = None
00064 wrl_url = None
00065 dae_url = None
00066 wrl_binfo = None
00067 dae_binfo = None
00068 wrl_links = None
00069 dae_links = None
00070
00071 def setUp(self):
00072 initCORBA()
00073 self.ml = findModelLoader()
00074
00075 def print_ok(self, fmt, ok):
00076 s = '\033[0m' if ok else '\033[91m'
00077 e = '\033[0m'
00078 str = s+"{0:70} {1}".format(fmt, ok)+e
00079 print str
00080 self.assertTrue(ok,str)
00081
00082 def loadFiles(self, wrl_file, dae_file):
00083 """ Override this method for loading model files from another directory """
00084 openhrp3_prefix=subprocess.check_output('pkg-config openhrp3.1 --variable=prefix', shell=True).rstrip()
00085 self.wrl_url = openhrp3_prefix+"/share/OpenHRP-3.1/sample/model/"+wrl_file
00086 self.dae_url = openhrp3_prefix+"/share/OpenHRP-3.1/sample/model/"+dae_file
00087 self.wrl_binfo = self.ml.getBodyInfo(self.wrl_url)
00088 self.dae_binfo = self.ml.getBodyInfo(self.dae_url)
00089 self.wrl_links = self.wrl_binfo._get_links()
00090 self.dae_links = self.dae_binfo._get_links()
00091
00092 def checkModels(self, wrl_file, dae_file):
00093 self.loadFiles(wrl_file, dae_file)
00094 ret = True
00095 print "%16s %16s"%(wrl_file, dae_file)
00096 for (wrl_l, dae_l) in zip(self.wrl_links, self.dae_links) :
00097
00098 print ";; %s %d,%d"%(dae_l.name, dae_l.jointId, dae_l.parentIndex);
00099 name_ok = wrl_l.name == dae_l.name
00100 translation_ok = equal(wrl_l.translation, dae_l.translation)
00101 rotation_ok = equal(norm(wrl_l.rotation), norm(dae_l.rotation))
00102 mass_ok = equal(wrl_l.mass, dae_l.mass)
00103 centerOfMass_ok = equal(wrl_l.centerOfMass, dae_l.centerOfMass)
00104 inertia_ok = equal(wrl_l.inertia, dae_l.inertia)
00105 llimit_ok = equal(wrl_l.llimit, dae_l.llimit) if wrl_l.parentIndex > 0 else True
00106 ulimit_ok = equal(wrl_l.ulimit, dae_l.ulimit) if wrl_l.parentIndex > 0 else True
00107 lvlimit_ok = equal(wrl_l.lvlimit, dae_l.lvlimit) if wrl_l.parentIndex > 0 else True
00108 uvlimit_ok = equal(wrl_l.uvlimit, dae_l.uvlimit) if wrl_l.parentIndex > 0 else True
00109 climit_ok = equal(wrl_l.climit, dae_l.climit) if wrl_l.parentIndex > 0 else True
00110 torqueConst_ok = equal(wrl_l.torqueConst, dae_l.torqueConst) if wrl_l.parentIndex > 0 else True
00111 gearRatio_ok = equal(wrl_l.gearRatio, dae_l.gearRatio) if wrl_l.parentIndex > 0 else True
00112 ret = all([ret, name_ok, translation_ok, rotation_ok, mass_ok, centerOfMass_ok])
00113 self.print_ok("name {0:24s} {1:24s} ".format(wrl_l.name, dae_l.name), True)
00114 self.print_ok(" tran {0:24} {1:24}".format(wrl_l.translation, dae_l.translation), translation_ok)
00115 self.print_ok(" rot {0:24} {1:24}".format(wrl_l.rotation, dae_l.rotation), rotation_ok)
00116 self.print_ok(" mass {0:<24} {1:<24}".format(wrl_l.mass, dae_l.mass), mass_ok)
00117 self.print_ok(" CoM {0:24} {1:24}".format(wrl_l.centerOfMass, dae_l.centerOfMass), centerOfMass_ok)
00118 self.print_ok(" iner {0:50}\n {1:50}".format(wrl_l.inertia, dae_l.inertia), inertia_ok)
00119 self.print_ok(" llim {0:24} {1:24}".format(wrl_l.llimit, dae_l.llimit), llimit_ok)
00120 self.print_ok(" ulim {0:24} {1:24}".format(wrl_l.ulimit, dae_l.ulimit), ulimit_ok)
00121 self.print_ok(" lvlim {0:24} {1:24}".format(wrl_l.lvlimit, dae_l.lvlimit), lvlimit_ok)
00122 self.print_ok(" uvlim {0:24} {1:24}".format(wrl_l.uvlimit, dae_l.uvlimit), uvlimit_ok)
00123 self.print_ok(" clim {0:24} {1:24}".format(wrl_l.climit, dae_l.climit), climit_ok)
00124 self.print_ok(" trqcnt {0:24} {1:24}".format(wrl_l.torqueConst, dae_l.torqueConst), torqueConst_ok)
00125 self.print_ok(" gratio {0:24} {1:24}".format(wrl_l.gearRatio, dae_l.gearRatio), gearRatio_ok)
00126 for (wrl_s, dae_s) in zip(wrl_l.segments, dae_l.segments):
00127
00128 name_ok = wrl_s.name == dae_s.name
00129 mass_ok = equal(wrl_s.mass, dae_s.mass)
00130 centerOfMass_ok = equal(wrl_s.centerOfMass, dae_s.centerOfMass)
00131 inertia_ok = equal(wrl_s.inertia, dae_s.inertia)
00132 transformMatrix_ok = equal(wrl_s.transformMatrix, dae_s.transformMatrix)
00133 shapeIndices_ok = equal(wrl_s.shapeIndices, dae_s.shapeIndices)
00134 ret = all([ret, name_ok, mass_ok, centerOfMass_ok,inertia_ok, transformMatrix_ok, shapeIndices_ok])
00135 self.print_ok(" name {0:24s} {1:24s} ".format(wrl_s.name, dae_s.name), name_ok)
00136 self.print_ok(" mass {0:<24} {1:<24}".format(wrl_s.mass, dae_s.mass), mass_ok)
00137 self.print_ok(" CoM {0:24} {1:24}".format(wrl_s.centerOfMass, dae_s.centerOfMass), centerOfMass_ok)
00138 self.print_ok(" iner {0:50}\n {1:50}".format(wrl_s.inertia, dae_s.inertia), inertia_ok)
00139 self.print_ok(" trans {0:50}\n {1:50}".format(wrl_s.transformMatrix, dae_s.transformMatrix), transformMatrix_ok)
00140 self.print_ok(" shape {0:24} {1:24}".format(wrl_s.shapeIndices, dae_s.shapeIndices), shapeIndices_ok)
00141
00142 if not ret:
00143 print "===========\n== ERROR == {0} and {1} differs\n===========".format(wrl_file, dae_file)
00144 return ret
00145
00146 class TestModelLoader(TestModelLoaderBase):
00147 """ Make class for testing by inheriting TestModelLoaderBase """
00148 def test_sample_models(self):
00149 self.checkModels("sample.wrl","sample.dae")
00150
00151 def test_pa10(self):
00152 self.checkModels("PA10/pa10.main.wrl","PA10/pa10.main.dae")
00153
00154
00155
00156
00157 if __name__ == '__main__':
00158 rostest.run(PKG, NAME, TestModelLoader, sys.argv)