def _create_model():
joints = [
Joint('joint_x', 'prismatic', parent='rfl', child='bridge', limit=Limit()),
Joint('joint_y', 'prismatic', parent='bridge', child='vertical', limit=Limit()),
Joint('joint_z', 'prismatic', parent='vertical', child='base_link', limit=Limit()),
Joint('joint_1', 'revolute', parent='base_link', child='link_1', limit=Limit()),
Joint('joint_2', 'revolute', parent='link_1', child='link_2', limit=Limit()),
Joint('joint_3', 'revolute', parent='link_2', child='link_3', limit=Limit()),
Joint('joint_4', 'revolute', parent='link_3', child='link_4', limit=Limit()),
Joint('joint_5', 'revolute', parent='link_4', child='link_5', limit=Limit()),
Joint('joint_6', 'revolute', parent='link_5', child='link_6', limit=Limit()),
]
links = [
Link('rfl'), Link('bridge'), Link('vertical'), Link('base_link'),
Link('link_1'), Link('link_2'), Link('link_3'), Link('link_4'), Link('link_5'), Link('link_6'),
]
return joints, links
def test_prismatic_calculate_transformation():
limit = Limit(lower=0, upper=1000)
j1 = Joint('j1', 'prismatic', None, None, axis=Axis('1 0 0'), limit=limit)
t = j1.calculate_transformation(550)
assert t == Translation([550, 0, 0])
def test_revolute_calculate_transformation():
limit = Limit(lower=-2 * pi, upper=2 * pi)
j1 = Joint('j1', 'revolute', None, None, limit=limit)
transformation = j1.calculate_transformation(2*pi)
assert transformation == Transformation()