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_rest_transform(self):
position = iDynTree.Position(1, 2, 3)
rotation = iDynTree.Rotation(0, 0, 1, 1, 0, 0, 0, 1, 0)
transform = iDynTree.Transform(rotation, position)
joint = iDynTree.RevoluteJoint()
joint.set_attached_links(1, 2)
joint.set_rest_transform(transform)
joint_transform = joint.get_rest_transform(1, 2)
self.assertEqual(list(joint_transform.position), list(position))
for r, c in itertools.product(range(3), range(3)):
self.assertEqual(joint_transform.rotation[r, c], rotation[r, c])
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_attached_links(self): joint = iDynTree.RevoluteJoint() joint.set_attached_links(1, 2) self.assertEqual(joint.get_first_attached_link(), 1) self.assertEqual(joint.get_second_attached_link(), 2)