def test_atlas_parsing(self):
# Sanity check on parsing.
pm = PackageMap()
model = os.path.join(pydrake.getDrakePath(), "examples", "atlas",
"urdf", "atlas_minimal_contact.urdf")
pm.PopulateUpstreamToDrake(model)
tree = RigidBodyTree(model,
package_map=pm,
floating_base_type=FloatingBaseType.kRollPitchYaw)
self.assertEqual(tree.get_num_actuators(), 30)
def test_atlas_parsing(self):
# Sanity check on parsing.
pm = PackageMap()
model = FindResourceOrThrow(
"drake/examples/atlas/urdf/atlas_minimal_contact.urdf")
pm.PopulateUpstreamToDrake(model)
tree = RigidBodyTree(model,
package_map=pm,
floating_base_type=FloatingBaseType.kRollPitchYaw)
self.assertEqual(tree.get_num_actuators(), 30)
# Sanity checks joint limits
# - Check sizes.
self.assertEqual(tree.joint_limit_min.size, tree.number_of_positions())
self.assertEqual(tree.joint_limit_max.size, tree.number_of_positions())
# - Check extremal values against values taken from URDF-file. Ignore
# the floating-base joints, as they are not present in the URDF.
self.assertAlmostEqual(np.min(tree.joint_limit_min[6:]), -3.011)
self.assertAlmostEqual(np.max(tree.joint_limit_max[6:]), 3.14159)
def test_package_map(self):
pm = PackageMap()
self.assertFalse(pm.Contains("foo"))
self.assertEqual(pm.size(), 0)
pm.Add("foo", os.path.abspath(os.curdir))
self.assertEqual(pm.size(), 1)
self.assertTrue(pm.Contains("foo"))
self.assertEqual(pm.GetPath("foo"), os.path.abspath(os.curdir))
# Populate from folder.
# TODO(eric.cousineau): This mismatch between casing is confusing, with
# `Atlas` being the package name, but `atlas` being the dirctory name.
pm = PackageMap()
self.assertEqual(pm.size(), 0)
pm.PopulateFromFolder(os.path.join(getDrakePath(), "examples",
"atlas"))
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas", ""))
# Populate from environment.
pm = PackageMap()
os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"] = os.path.join(
getDrakePath(), "examples")
pm.PopulateFromEnvironment("PYDRAKE_TEST_ROS_PACKAGE_PATH")
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas", ""))
del os.environ["PYDRAKE_TEST_ROS_PACKAGE_PATH"]
# Populate upstream.
pm = PackageMap()
pm.PopulateUpstreamToDrake(
os.path.join(getDrakePath(), "examples", "atlas", "urdf",
"atlas_minimal_contact.urdf"))
self.assertTrue(pm.Contains("Atlas"))
self.assertEqual(pm.GetPath("Atlas"),
os.path.join(getDrakePath(), "examples", "atlas"))