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_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_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_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_inertia(self): mass = 3.14 position = iDynTree.Position(1, 2, 3) rotational_inertia = iDynTree.RotationalInertia() rotational_inertia[1, 1] = 1 inertia = iDynTree.SpatialInertia(mass, position, rotational_inertia) link = iDynTree.Link() link.inertia = inertia self.assertEqual(link.inertia.get_mass(), mass)
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_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])
def test_index(self): link = iDynTree.Link() link.set_index(5) self.assertEqual(link.get_index(), 5)