2 import pinocchio
as pin
7 return geom1.ngeoms == geom2.ngeoms
10 @unittest.skipUnless(pin.WITH_URDFDOM,
"Needs URDFDOM")
13 self.
current_file = os.path.dirname(str(os.path.abspath(__file__)))
18 os.path.join(self.
current_file,
"../../models/example-robot-data/robots")
21 os.path.join(self.
model_dir,
"romeo_description/urdf/romeo.urdf")
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"
30 model = pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer())
31 collision_model = pin.buildGeomFromUrdf(
32 model, self.
model_path, pin.GeometryType.COLLISION, hint_list
35 col = collision_model.geometryObjects[1]
37 os.path.normpath(col.meshPath), os.path.normpath(expected_mesh_path)
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
48 collision_model_self = pin.GeometryModel()
49 pin.buildGeomFromUrdf(
52 pin.GeometryType.COLLISION,
56 self.assertTrue(
checkGeom(collision_model_ref, collision_model_self))
58 collision_model_self = pin.GeometryModel()
59 pin.buildGeomFromUrdf(
62 pin.GeometryType.COLLISION,
66 self.assertTrue(
checkGeom(collision_model_ref, collision_model_self))
68 hint_vec = pin.StdVec_StdString()
71 collision_model_self = pin.GeometryModel()
72 pin.buildGeomFromUrdf(
75 pin.GeometryType.COLLISION,
79 self.assertTrue(
checkGeom(collision_model_ref, collision_model_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"
86 expected_visual_path = os.path.join(
87 self.
model_dir,
"romeo_description/meshes/V1/visual/LHipPitch.dae"
90 model = pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer())
92 collision_model = pin.buildGeomFromUrdf(
93 model, self.
model_path, pin.GeometryType.COLLISION, hint_list
95 col = collision_model.geometryObjects[1]
97 os.path.normpath(col.meshPath), os.path.normpath(expected_collision_path)
100 visual_model = pin.buildGeomFromUrdf(
101 model, self.
model_path, pin.GeometryType.VISUAL, hint_list
103 vis = visual_model.geometryObjects[1]
105 os.path.normpath(vis.meshPath), os.path.normpath(expected_visual_path)
108 model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf(
109 self.
model_path, hint_list, pin.JointModelFreeFlyer()
112 self.assertEqual(model, model_2)
114 col_2 = collision_model_2.geometryObjects[1]
116 os.path.normpath(col_2.meshPath), os.path.normpath(expected_collision_path)
119 vis_2 = visual_model_2.geometryObjects[1]
121 os.path.normpath(vis_2.meshPath), os.path.normpath(expected_visual_path)
124 model_c, collision_model_c = pin.buildModelsFromUrdf(
127 pin.JointModelFreeFlyer(),
128 geometry_types=pin.GeometryType.COLLISION,
131 self.assertEqual(model, model_c)
133 col_c = collision_model_c.geometryObjects[1]
135 os.path.normpath(col_c.meshPath), os.path.normpath(expected_collision_path)
138 model_v, visual_model_v = pin.buildModelsFromUrdf(
141 pin.JointModelFreeFlyer(),
142 geometry_types=pin.GeometryType.VISUAL,
145 self.assertEqual(model, model_v)
147 vis_v = visual_model_v.geometryObjects[1]
149 os.path.normpath(vis_v.meshPath), os.path.normpath(expected_visual_path)
153 if __name__ ==
"__main__":