Skip to content

Commit

Permalink
Use new Joint APIs for Parent/Child name (#92)
Browse files Browse the repository at this point in the history
Follow-up to gazebosim/sdformat#1053.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
scpeters authored Jun 21, 2022
1 parent 59a00a7 commit 3b08d03
Show file tree
Hide file tree
Showing 4 changed files with 20 additions and 20 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,12 @@ def add_fixed_joint(parent_name, child_name):
joint_sdf = sdf.Joint()
joint_sdf.set_type(sdf.JointType.FIXED)
if parent_name is None:
joint_sdf.set_parent_link_name("world")
joint_sdf.set_parent_name("world")
else:
joint_sdf.set_parent_link_name(parent_name)
joint_sdf.set_child_link_name(child_name)
joint_sdf.set_parent_name(parent_name)
joint_sdf.set_child_name(child_name)
joint_sdf.set_name(
"fixed_" + joint_sdf.parent_link_name() + "_" + child_name + "_joint")
"fixed_" + joint_sdf.parent_name() + "_" + child_name + "_joint")
return joint_sdf


Expand Down Expand Up @@ -100,14 +100,14 @@ def mjcf_joint_to_sdf(joint, parent_name, child_name, default_classes=None):
joint_sdf.set_name("joint_" + joint.name)
else:
joint_sdf.set_name(
"joint_" + joint_sdf.parent_link_name() + "_" + child_name)
"joint_" + joint_sdf.parent_name() + "_" + child_name)

if parent_name is None:
joint_sdf.set_parent_link_name("world")
joint_sdf.set_parent_name("world")
else:
joint_sdf.set_parent_link_name(parent_name)
joint_sdf.set_parent_name(parent_name)

joint_sdf.set_child_link_name(child_name)
joint_sdf.set_child_name(child_name)

if joint.axis is not None:
joint_axis_sdf.set_xyz(Vector3d(joint.axis[0],
Expand Down
4 changes: 2 additions & 2 deletions sdformat_mjcf/tests/helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,13 +43,13 @@ def resolve_parent_link_name(self, joint):
try:
return super().resolve_parent_link_name(joint)
except RuntimeError:
return joint.parent_link_name()
return joint.parent_name()

def resolve_child_link_name(self, joint):
try:
return super().resolve_child_link_name(joint)
except RuntimeError:
return joint.child_link_name()
return joint.child_name()


def setup_test_graph_resolver():
Expand Down
4 changes: 2 additions & 2 deletions sdformat_mjcf/tests/sdformat_to_mjcf/test_add_model.py
Original file line number Diff line number Diff line change
Expand Up @@ -110,8 +110,8 @@ def test_model_multiple_links_with_joint(self):
joint = sdf.Joint()
joint.set_name("joint")
joint.set_type(sdf.JointType.FIXED)
joint.set_parent_link_name("base_link")
joint.set_child_link_name("upper_link")
joint.set_parent_name("base_link")
joint.set_child_name("upper_link")
model.add_joint(joint)

mj_root = add_model(self.mujoco, model)
Expand Down
16 changes: 8 additions & 8 deletions sdformat_mjcf/tests/sdformat_to_mjcf/test_joints.py
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,8 @@ def test_defaults(self):
joint = model.joint_by_index(0)
self.assertNotEqual(None, joint)
self.assertEqual(sdf.JointType.REVOLUTE, joint.type())
self.assertEqual("world", joint.parent_link_name())
self.assertEqual("body1", joint.child_link_name())
self.assertEqual("world", joint.parent_name())
self.assertEqual("body1", joint.child_name())
self.assertNotEqual(None, joint.axis(0))
self.assertEqual(Vector3d(0, 0, 1), joint.axis(0).xyz())
self.assertEqual(0, joint.axis(0).damping())
Expand All @@ -71,8 +71,8 @@ def test_defaults(self):
joint = model.joint_by_index(1)
self.assertNotEqual(None, joint)
self.assertEqual(sdf.JointType.REVOLUTE, joint.type())
self.assertEqual("world", joint.parent_link_name())
self.assertEqual("body1", joint.child_link_name())
self.assertEqual("world", joint.parent_name())
self.assertEqual("body1", joint.child_name())
self.assertNotEqual(None, joint.axis(0))
self.assertEqual(Vector3d(0, 1, 0), joint.axis(0).xyz())
self.assertEqual(0, joint.axis(0).damping())
Expand All @@ -84,8 +84,8 @@ def test_defaults(self):
joint = model.joint_by_index(2)
self.assertNotEqual(None, joint)
self.assertEqual(sdf.JointType.PRISMATIC, joint.type())
self.assertEqual("body1", joint.parent_link_name())
self.assertEqual("body2", joint.child_link_name())
self.assertEqual("body1", joint.parent_name())
self.assertEqual("body2", joint.child_name())
self.assertNotEqual(None, joint.axis(0))
self.assertEqual(Vector3d(0, 0, 1), joint.axis(0).xyz())
self.assertEqual(0, joint.axis(0).damping())
Expand All @@ -97,8 +97,8 @@ def test_defaults(self):
joint = model.joint_by_index(3)
self.assertNotEqual(None, joint)
self.assertEqual(sdf.JointType.PRISMATIC, joint.type())
self.assertEqual("body2", joint.parent_link_name())
self.assertEqual("body3", joint.child_link_name())
self.assertEqual("body2", joint.parent_name())
self.assertEqual("body3", joint.child_name())
self.assertNotEqual(None, joint.axis(0))
self.assertEqual(Vector3d(1, 0, 0), joint.axis(0).xyz())
self.assertEqual(0.01, joint.axis(0).damping())
Expand Down

0 comments on commit 3b08d03

Please sign in to comment.