test_modelloader.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 PKG = 'openhrp3'
00004 NAME = 'modelloader-test'
00005 
00006 try: # catkin does not requires load_manifest
00007     import openhrp3
00008 except:
00009     import roslib; roslib.load_manifest("openhrp3")
00010 
00011 #import OpenRTM_aist.RTM_IDL # for catkin
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 # set custom port for modelloader-test.launch
00027 def initCORBA():
00028     global rootnc
00029     #nshost = socket.gethostname()
00030     #nsport = 15005
00031 
00032     # initCORBA
00033     #os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:{0}:{1}/NameService'.format(nshost,nsport)
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             # 'centerOfMass', 'childIndices', 'climit', 'encoderPulse', 'gearRatio', 'hwcs', 'inertia', 'inlinedShapeTransformMatrices', 'jointAxis', 'jointId', 'jointType', 'jointValue', 'lights', 'llimit', 'lvlimit', 'mass', 'name', 'parentIndex', 'rotation', 'rotorInertia', 'rotorResistor', 'segments', 'sensors', 'shapeIndices', 'specFiles', 'torqueConst', 'translation', 'ulimit', 'uvlimit'
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)#) ## not fixed yet
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                 # name, mass, centerOfMass, inertia, transformMatrix, shapeIndices
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     #def test_3dof_arm(self):
00155     #    self.checkModels("sample3dof.wrl","sample3dof.dae")
00156 
00157 if __name__ == '__main__':
00158     rostest.run(PKG, NAME, TestModelLoader, sys.argv)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19