bindings_geometry_model_urdf.py
Go to the documentation of this file.
1 import unittest
2 import pinocchio as pin
3 import os
4 
5 
6 def checkGeom(geom1, geom2):
7  return geom1.ngeoms == geom2.ngeoms
8 
9 
10 @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM")
11 class TestGeometryObjectUrdfBindings(unittest.TestCase):
12  def setUp(self):
13  self.current_file = os.path.dirname(str(os.path.abspath(__file__)))
14  self.mesh_path = os.path.abspath(
15  os.path.join(self.current_file, "../../models")
16  )
17  self.model_dir = os.path.abspath(
18  os.path.join(self.current_file, "../../models/example-robot-data/robots")
19  )
20  self.model_path = os.path.abspath(
21  os.path.join(self.model_dir, "romeo_description/urdf/romeo.urdf")
22  )
23 
24  def test_load(self):
25  hint_list = [self.mesh_path, "wrong/hint"]
26  expected_mesh_path = os.path.join(
27  self.model_dir, "romeo_description/meshes/V1/collision/LHipPitch.dae"
28  )
29 
30  model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
31  collision_model = pin.buildGeomFromUrdf(
32  model, self.model_path, pin.GeometryType.COLLISION, hint_list
33  )
34 
35  col = collision_model.geometryObjects[1]
36  self.assertEqual(
37  os.path.normpath(col.meshPath), os.path.normpath(expected_mesh_path)
38  )
39 
40  def test_self_load(self):
41  hint_list = [self.mesh_path]
42 
43  model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
44  collision_model_ref = pin.buildGeomFromUrdf(
45  model, self.model_path, pin.GeometryType.COLLISION, hint_list
46  )
47 
48  collision_model_self = pin.GeometryModel()
49  pin.buildGeomFromUrdf(
50  model,
51  self.model_path,
52  pin.GeometryType.COLLISION,
53  collision_model_self,
54  hint_list,
55  )
56  self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
57 
58  collision_model_self = pin.GeometryModel()
59  pin.buildGeomFromUrdf(
60  model,
61  self.model_path,
62  pin.GeometryType.COLLISION,
63  collision_model_self,
64  self.mesh_path,
65  )
66  self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
67 
68  hint_vec = pin.StdVec_StdString()
69  hint_vec.append(self.mesh_path)
70 
71  collision_model_self = pin.GeometryModel()
72  pin.buildGeomFromUrdf(
73  model,
74  self.model_path,
75  pin.GeometryType.COLLISION,
76  collision_model_self,
77  hint_vec,
78  )
79  self.assertTrue(checkGeom(collision_model_ref, collision_model_self))
80 
81  def test_multi_load(self):
82  hint_list = [self.mesh_path, "wrong/hint"]
83  expected_collision_path = os.path.join(
84  self.model_dir, "romeo_description/meshes/V1/collision/LHipPitch.dae"
85  )
86  expected_visual_path = os.path.join(
87  self.model_dir, "romeo_description/meshes/V1/visual/LHipPitch.dae"
88  )
89 
90  model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer())
91 
92  collision_model = pin.buildGeomFromUrdf(
93  model, self.model_path, pin.GeometryType.COLLISION, hint_list
94  )
95  col = collision_model.geometryObjects[1]
96  self.assertEqual(
97  os.path.normpath(col.meshPath), os.path.normpath(expected_collision_path)
98  )
99 
100  visual_model = pin.buildGeomFromUrdf(
101  model, self.model_path, pin.GeometryType.VISUAL, hint_list
102  )
103  vis = visual_model.geometryObjects[1]
104  self.assertEqual(
105  os.path.normpath(vis.meshPath), os.path.normpath(expected_visual_path)
106  )
107 
108  model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf(
109  self.model_path, hint_list, pin.JointModelFreeFlyer()
110  )
111 
112  self.assertEqual(model, model_2)
113 
114  col_2 = collision_model_2.geometryObjects[1]
115  self.assertEqual(
116  os.path.normpath(col_2.meshPath), os.path.normpath(expected_collision_path)
117  )
118 
119  vis_2 = visual_model_2.geometryObjects[1]
120  self.assertEqual(
121  os.path.normpath(vis_2.meshPath), os.path.normpath(expected_visual_path)
122  )
123 
124  model_c, collision_model_c = pin.buildModelsFromUrdf(
125  self.model_path,
126  hint_list,
127  pin.JointModelFreeFlyer(),
128  geometry_types=pin.GeometryType.COLLISION,
129  )
130 
131  self.assertEqual(model, model_c)
132 
133  col_c = collision_model_c.geometryObjects[1]
134  self.assertEqual(
135  os.path.normpath(col_c.meshPath), os.path.normpath(expected_collision_path)
136  )
137 
138  model_v, visual_model_v = pin.buildModelsFromUrdf(
139  self.model_path,
140  hint_list,
141  pin.JointModelFreeFlyer(),
142  geometry_types=pin.GeometryType.VISUAL,
143  )
144 
145  self.assertEqual(model, model_v)
146 
147  vis_v = visual_model_v.geometryObjects[1]
148  self.assertEqual(
149  os.path.normpath(vis_v.meshPath), os.path.normpath(expected_visual_path)
150  )
151 
152 
153 if __name__ == "__main__":
154  unittest.main()
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.setUp
def setUp(self)
Definition: bindings_geometry_model_urdf.py:12
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.current_file
current_file
Definition: bindings_geometry_model_urdf.py:13
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.model_dir
model_dir
Definition: bindings_geometry_model_urdf.py:17
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.test_load
def test_load(self)
Definition: bindings_geometry_model_urdf.py:24
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.mesh_path
mesh_path
Definition: bindings_geometry_model_urdf.py:14
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.model_path
model_path
Definition: bindings_geometry_model_urdf.py:20
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.test_self_load
def test_self_load(self)
Definition: bindings_geometry_model_urdf.py:40
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings
Definition: bindings_geometry_model_urdf.py:11
bindings_geometry_model_urdf.checkGeom
def checkGeom(geom1, geom2)
Definition: bindings_geometry_model_urdf.py:6
bindings_geometry_model_urdf.TestGeometryObjectUrdfBindings.test_multi_load
def test_multi_load(self)
Definition: bindings_geometry_model_urdf.py:81


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:06