def test_model_default_base(self):
model = iDynTree.Model()
link1 = iDynTree.Link()
link2 = iDynTree.Link()
model.add_link("link1", link1)
link2_idx = model.add_link("link2", link2)
model.default_base_link = link2_idx
self.assertEqual(model.default_base_link, link2_idx)
def test_model_one_link_model(self):
model = iDynTree.Model()
link1 = iDynTree.Link()
self.assertEqual(model.add_link("link1", link1), 0)
self.assertEqual(model.get_nr_of_links(), 1)
self.assertEqual(model.get_link_index("link1"), 0)
self.assertEqual(model.get_link_name(0), "link1")
self.assertIsNotNone(model.get_link(0))
def test_custom_options(self):
options = iDynTree.ModelExporterOptions()
options.robot_exported_name = "robot_name"
exporter = iDynTree.ModelExporter()
model = iDynTree.Model()
link = iDynTree.Link()
model.add_link("a_link", link)
exporter.init(model, options=options)
self.assertEqual(exporter.options.robot_exported_name, "robot_name")
def test_exporter(self):
exporter = iDynTree.ModelExporter()
model = iDynTree.Model()
link = iDynTree.Link()
model.add_link("a_link", link)
exporter.init(model)
self.assertTrue(exporter.export_model_to_string())
self.assertEqual(exporter.model.get_nr_of_links(),
model.get_nr_of_links())
def test_resize_with_model(self):
model_shapes = iDynTree.ModelSolidShapes()
model = iDynTree.Model()
link1 = iDynTree.Link()
link2 = iDynTree.Link()
model.add_link("link1", link1)
model.add_link("link2", link2)
model_shapes.resize(model)
self.assertEqual(len(model_shapes), model.get_nr_of_links())
def test_compute_traversal(self):
model = iDynTree.Model()
link1 = iDynTree.Link()
link2 = iDynTree.Link()
model.add_link("link1", link1)
model.add_link("link2", link2)
joint = iDynTree.RevoluteJoint()
joint.set_attached_links(0, 1)
model.add_joint("joint1", joint)
traversal = model.compute_full_tree_traversal(0)
self.assertIsNotNone(traversal)
self.assertEqual(traversal.get_nr_of_visited_links(), 2)
def test_custom_sensors(self):
exporter = iDynTree.ModelExporter()
model = iDynTree.Model()
link = iDynTree.Link()
model.add_link("a_link", link)
sensors = iDynTree.SensorsList()
sensor = iDynTree.AccelerometerSensor()
sensor.name = "a_name"
sensor.parent_link_index = 1
sensors.add_sensor(sensor)
exporter.init(model, sensors=sensors)
self.assertEqual(
exporter.sensors.get_nr_of_sensors(iDynTree.ACCELEROMETER), 1)
def test_model_one_joint(self):
model = iDynTree.Model()
link1 = iDynTree.Link()
link2 = iDynTree.Link()
model.add_link("link1", link1)
model.add_link("link2", link2)
joint = iDynTree.RevoluteJoint()
joint.set_attached_links(0, 1)
self.assertEqual(model.add_joint("joint1", joint), 0)
self.assertEqual(model.get_nr_of_joints(), 1)
self.assertEqual(model.get_joint_index("joint1"), 0)
self.assertEqual(model.get_joint_name(0), "joint1")
self.assertIsNotNone(model.get_joint(0))
def test_model_shapes(self):
model = iDynTree.Model()
link1 = iDynTree.Link()
link2 = iDynTree.Link()
model.add_link("link1", link1)
model.add_link("link2", link2)
self.assertEqual(len(model.visual_solid_shapes), model.get_nr_of_links())
self.assertEqual(len(model.collision_solid_shapes), model.get_nr_of_links())
# Append a shape to test that the objects are writable.
mesh = iDynTree.ExternalMesh()
mesh.name = "a_mesh"
old_len = len(model.visual_solid_shapes[0])
model.visual_solid_shapes[0].append(mesh)
self.assertEqual(len(model.visual_solid_shapes[0]), old_len + 1)
def test_model_frame(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
model = iDynTree.Model()
link1 = iDynTree.Link()
model.add_link("link1", link1)
num_of_frames = model.get_nr_of_frames()
self.assertTrue(
model.add_additional_frame_to_link("link1", "frame1", transform))
self.assertEqual(model.get_nr_of_frames(), num_of_frames + 1)
frame_index = model.get_frame_index("frame1")
self.assertGreaterEqual(frame_index, 0)
self.assertEqual(model.get_frame_name(frame_index), "frame1")
self.assertEqual(model.get_frame_link(frame_index), 0)
frame_transform = model.get_frame_transform(frame_index)
self.assertEqual(list(frame_transform.position), list(position))
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(frame_transform.rotation[r, c], rotation[r, c])
