test_modelloader.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 PKG = 'openhrp3'
4 NAME = 'modelloader-test'
5 
6 try: # catkin does not requires load_manifest
7  import openhrp3
8 except:
9  import roslib; roslib.load_manifest("openhrp3")
10 
11 #import OpenRTM_aist.RTM_IDL # for catkin
12 
13 from omniORB import CORBA, any, cdrUnmarshal, cdrMarshal
14 import CosNaming
15 
16 import sys, os, socket, subprocess
17 
18 import OpenRTM_aist
19 from OpenHRP import *
20 
21 import rospkg
22 import unittest
23 import rostest
24 
25 rootrc = None
26 # set custom port for modelloader-test.launch
27 def initCORBA():
28  global rootnc
29  #nshost = socket.gethostname()
30  #nsport = 15005
31 
32  # initCORBA
33  #os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:{0}:{1}/NameService'.format(nshost,nsport)
34  orb = CORBA.ORB_init(sys.argv, CORBA.ORB_ID)
35 
36  nameserver = orb.resolve_initial_references("NameService");
37  rootnc = nameserver._narrow(CosNaming.NamingContext)
38 
40  global rootnc
41  try:
42  obj = rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
43  return obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
44  except:
45  print("Could not find ModelLoader", sys.exc_info()[0])
46  exit
47 
48 def equal(a, b, tol = 1e-4):
49  if type(a) == float and type(b) == float:
50  return abs(a - b) < tol
51  if type(a) == list and type(b) == list:
52  return all(equal(x,y) for (x,y) in zip(a,b))
53  else:
54  return True
55 
56 def norm(a):
57  r = 0
58  for e in a:
59  r = r + e*e
60  return r/len(a)
61 
62 class TestModelLoaderBase(unittest.TestCase):
63  ml = None
64  wrl_url = None
65  dae_url = None
66  wrl_binfo = None
67  dae_binfo = None
68  wrl_links = None
69  dae_links = None
70 
71  def setUp(self):
72  initCORBA()
73  self.ml = findModelLoader()
74 
75  def print_ok(self, fmt, ok):
76  s = '\033[0m' if ok else '\033[91m'
77  e = '\033[0m'
78  str = s+"{0:70} {1}".format(fmt, ok)+e
79  print(str)
80  self.assertTrue(ok,str)
81 
82  def loadFiles(self, wrl_file, dae_file):
83  """ Override this method for loading model files from another directory """
84  openhrp3_prefix=subprocess.check_output('pkg-config openhrp3.1 --variable=prefix', shell=True).rstrip()
85  self.wrl_url = openhrp3_prefix+"/share/OpenHRP-3.1/sample/model/"+wrl_file
86  self.dae_url = openhrp3_prefix+"/share/OpenHRP-3.1/sample/model/"+dae_file
87  self.wrl_binfo = self.ml.getBodyInfo(self.wrl_url)
88  self.dae_binfo = self.ml.getBodyInfo(self.dae_url)
89  self.wrl_links = self.wrl_binfo._get_links()
90  self.dae_links = self.dae_binfo._get_links()
91 
92  def checkModels(self, wrl_file, dae_file):
93  self.loadFiles(wrl_file, dae_file)
94  ret = True
95  print("%16s %16s"%(wrl_file, dae_file))
96  for (wrl_l, dae_l) in zip(self.wrl_links, self.dae_links) :
97  # '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'
98  print(";; %s %d,%d"%(dae_l.name, dae_l.jointId, dae_l.parentIndex));
99  name_ok = wrl_l.name == dae_l.name
100  translation_ok = equal(wrl_l.translation, dae_l.translation)
101  rotation_ok = equal(norm(wrl_l.rotation), norm(dae_l.rotation))
102  mass_ok = equal(wrl_l.mass, dae_l.mass)
103  centerOfMass_ok = equal(wrl_l.centerOfMass, dae_l.centerOfMass)
104  inertia_ok = equal(wrl_l.inertia, dae_l.inertia)
105  llimit_ok = equal(wrl_l.llimit, dae_l.llimit) if wrl_l.parentIndex > 0 else True
106  ulimit_ok = equal(wrl_l.ulimit, dae_l.ulimit) if wrl_l.parentIndex > 0 else True
107  lvlimit_ok = equal(wrl_l.lvlimit, dae_l.lvlimit) if wrl_l.parentIndex > 0 else True
108  uvlimit_ok = equal(wrl_l.uvlimit, dae_l.uvlimit) if wrl_l.parentIndex > 0 else True
109  climit_ok = equal(wrl_l.climit, dae_l.climit) if wrl_l.parentIndex > 0 else True
110  torqueConst_ok = equal(wrl_l.torqueConst, dae_l.torqueConst) if wrl_l.parentIndex > 0 else True
111  gearRatio_ok = equal(wrl_l.gearRatio, dae_l.gearRatio) if wrl_l.parentIndex > 0 else True
112  ret = all([ret, name_ok, translation_ok, rotation_ok, mass_ok, centerOfMass_ok])
113  self.print_ok("name {0:24s} {1:24s} ".format(wrl_l.name, dae_l.name), True)#) ## not fixed yet
114  self.print_ok(" tran {0:24} {1:24}".format(wrl_l.translation, dae_l.translation), translation_ok)
115  self.print_ok(" rot {0:24} {1:24}".format(wrl_l.rotation, dae_l.rotation), rotation_ok)
116  self.print_ok(" mass {0:<24} {1:<24}".format(wrl_l.mass, dae_l.mass), mass_ok)
117  self.print_ok(" CoM {0:24} {1:24}".format(wrl_l.centerOfMass, dae_l.centerOfMass), centerOfMass_ok)
118  self.print_ok(" iner {0:50}\n {1:50}".format(wrl_l.inertia, dae_l.inertia), inertia_ok)
119  self.print_ok(" llim {0:24} {1:24}".format(wrl_l.llimit, dae_l.llimit), llimit_ok)
120  self.print_ok(" ulim {0:24} {1:24}".format(wrl_l.ulimit, dae_l.ulimit), ulimit_ok)
121  self.print_ok(" lvlim {0:24} {1:24}".format(wrl_l.lvlimit, dae_l.lvlimit), lvlimit_ok)
122  self.print_ok(" uvlim {0:24} {1:24}".format(wrl_l.uvlimit, dae_l.uvlimit), uvlimit_ok)
123  self.print_ok(" clim {0:24} {1:24}".format(wrl_l.climit, dae_l.climit), climit_ok)
124  self.print_ok(" trqcnt {0:24} {1:24}".format(wrl_l.torqueConst, dae_l.torqueConst), torqueConst_ok)
125  self.print_ok(" gratio {0:24} {1:24}".format(wrl_l.gearRatio, dae_l.gearRatio), gearRatio_ok)
126  for (wrl_s, dae_s) in zip(wrl_l.segments, dae_l.segments):
127  # name, mass, centerOfMass, inertia, transformMatrix, shapeIndices
128  name_ok = wrl_s.name == dae_s.name
129  mass_ok = equal(wrl_s.mass, dae_s.mass)
130  centerOfMass_ok = equal(wrl_s.centerOfMass, dae_s.centerOfMass)
131  inertia_ok = equal(wrl_s.inertia, dae_s.inertia)
132  transformMatrix_ok = equal(wrl_s.transformMatrix, dae_s.transformMatrix)
133  shapeIndices_ok = equal(wrl_s.shapeIndices, dae_s.shapeIndices)
134  ret = all([ret, name_ok, mass_ok, centerOfMass_ok,inertia_ok, transformMatrix_ok, shapeIndices_ok])
135  self.print_ok(" name {0:24s} {1:24s} ".format(wrl_s.name, dae_s.name), name_ok)
136  self.print_ok(" mass {0:<24} {1:<24}".format(wrl_s.mass, dae_s.mass), mass_ok)
137  self.print_ok(" CoM {0:24} {1:24}".format(wrl_s.centerOfMass, dae_s.centerOfMass), centerOfMass_ok)
138  self.print_ok(" iner {0:50}\n {1:50}".format(wrl_s.inertia, dae_s.inertia), inertia_ok)
139  self.print_ok(" trans {0:50}\n {1:50}".format(wrl_s.transformMatrix, dae_s.transformMatrix), transformMatrix_ok)
140  self.print_ok(" shape {0:24} {1:24}".format(wrl_s.shapeIndices, dae_s.shapeIndices), shapeIndices_ok)
141 
142  if not ret:
143  print("===========\n== ERROR == {0} and {1} differs\n===========".format(wrl_file, dae_file))
144  return ret
145 
147  """ Make class for testing by inheriting TestModelLoaderBase """
149  self.checkModels("sample.wrl","sample.dae")
150 
151  def test_pa10(self):
152  self.checkModels("PA10/pa10.main.wrl","PA10/pa10.main.dae")
153 
154  #def test_3dof_arm(self):
155  # self.checkModels("sample3dof.wrl","sample3dof.dae")
156 
157 if __name__ == '__main__':
158  rostest.run(PKG, NAME, TestModelLoader, sys.argv)
png_infop png_charp png_int_32 png_int_32 int * type
Definition: png.h:2332
def checkModels(self, wrl_file, dae_file)
def loadFiles(self, wrl_file, dae_file)
def equal(a, b, tol=1e-4)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05