2 import pinocchio
as pin
6 @unittest.skipUnless(pin.WITH_URDFDOM,
"Needs URDFDOM")
9 self.
current_file = os.path.dirname(str(os.path.abspath(__file__)))
11 os.path.join(self.
current_file,
"../../models/example-robot-data/robots")
14 os.path.join(self.
model_dir,
"romeo_description/urdf/romeo.urdf")
19 pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer())
23 pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer(), model)
24 pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer())
28 file_content = model.read()
30 model_ref = pin.buildModelFromUrdf(self.
model_path, pin.JointModelFreeFlyer())
31 model = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer())
33 self.assertEqual(model, model_ref)
35 model_self = pin.Model()
36 pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), model_self)
37 self.assertEqual(model_self, model_ref)
42 model_dir = os.path.abspath(
43 os.path.join(self.
current_file,
"../../models/example-robot-data/robots")
45 model_path = os.path.abspath(
46 os.path.join(model_dir,
"ur_description/urdf/ur5_robot.urdf")
49 model = pin.buildModelFromUrdf(model_path)
50 filename =
"model.pickle"
51 with open(filename,
"wb")
as f:
54 with open(filename,
"rb")
as f:
55 model_copy = pickle.load(f)
57 self.assertTrue(model == model_copy)
60 if __name__ ==
"__main__":