本文整理汇总了Python中pydrake.getDrakePath函数的典型用法代码示例。如果您正苦于以下问题：Python getDrakePath函数的具体用法？Python getDrakePath怎么用？Python getDrakePath使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了getDrakePath函数的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: setupValkyrieExample
def setupValkyrieExample():
# Valkyrie Example
rbt = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rbt.world(),
[0, 0, 0], [0, 0, 0])
from pydrake.multibody.parsers import PackageMap
pmap = PackageMap()
# Note: Val model is currently not installed in drake binary distribution.
pmap.PopulateFromFolder(os.path.join(pydrake. getDrakePath(), "examples"))
# TODO(russt): remove plane.urdf and call AddFlatTerrainTOWorld instead
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(FindResource(os.path.join("underactuated", "plane.urdf")), 'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kFixed,
world_frame,
rbt)
val_start_frame = RigidBodyFrame("val_start_frame", rbt.world(),
[0, 0, 1.5], [0, 0, 0])
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(pydrake.getDrakePath() + "/examples/valkyrie/urdf/urdf/valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf", 'r').read(), # noqa
pmap,
pydrake.getDrakePath() + "/examples/",
FloatingBaseType.kRollPitchYaw,
val_start_frame,
rbt)
Tview = np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]],
dtype=np.float64)
pbrv = MeshcatRigidBodyVisualizer(rbt, draw_collision=True)
return rbt, pbrv示例2: test_populate_upstream
def test_populate_upstream(self):
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"))示例3: test_populate_from_folder
def test_populate_from_folder(self):
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", ""))示例4: test_populate_from_environment
def test_populate_from_environment(self):
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"]示例5: test_sdf
def test_sdf(self):
sdf_file = os.path.join(
getDrakePath(), "examples/acrobot/Acrobot.sdf")
with open(sdf_file) as f:
sdf_string = f.read()
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot_1 = RigidBodyTree()
AddModelInstancesFromSdfStringSearchingInRosPackages(
sdf_string,
package_map,
floating_base_type,
weld_frame,
robot_1)
robot_2 = RigidBodyTree()
AddModelInstancesFromSdfString(
sdf_string,
floating_base_type,
weld_frame,
robot_2)
robot_3 = RigidBodyTree()
AddModelInstancesFromSdfFile(
sdf_file,
floating_base_type,
weld_frame,
robot_3)
for robot in robot_1, robot_2, robot_3:
expected_num_bodies = 4
self.assertEqual(robot.get_num_bodies(), expected_num_bodies)示例6: test_urdf
def test_urdf(self):
"""Test that an instance of a URDF model can be loaded into a
RigidBodyTree by passing a complete set of arguments to Drake's URDF
parser.
"""
urdf_file = os.path.join(
getDrakePath(),
"examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
with open(urdf_file) as f:
urdf_string = f.read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
expected_num_bodies = 86
self.assertEqual(robot.get_num_bodies(), expected_num_bodies,
msg='Incorrect number of bodies: {0} vs. {1}'.format(
robot.get_num_bodies(), expected_num_bodies))示例7: test_constructor
def test_constructor(self):
pm = PackageMap()
model = os.path.join(getDrakePath(), "examples", "Atlas", "urdf",
"atlas_minimal_contact.urdf")
pm.PopulateUpstreamToDrake(model)
robot = rbtree.RigidBodyTree(
model, package_map=pm,
floating_base_type=rbtree.FloatingBaseType.kRollPitchYaw)示例8: testCoM0
def testCoM0(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
kinsol = r.doKinematics(np.zeros((7,1)), np.zeros((7,1)))
c = r.centerOfMass(kinsol)
self.assertTrue(np.allclose(c.flat, [0.0, 0.0, -0.2425], atol=1e-4))示例9: test_value
def test_value(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
self.assertEqual(r.number_of_positions(), 7)
self.assertEqual(r.number_of_velocities(), 7)
kinsol = r.doKinematics(np.zeros((7,1)), np.zeros((7,1)))
p = r.transformPoints(kinsol, np.zeros((3,1)), 0, 1)
self.assertTrue(np.allclose(p, np.zeros((3,1))))示例10: test_relative_transform
def test_relative_transform(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"))
q = np.zeros(7)
q[6] = np.pi / 2
kinsol = r.doKinematics(q)
T = r.relativeTransform(kinsol, 1, 2)
self.assertTrue(np.allclose(T,
np.array([[0, 0, 1, 0],
[0, 1, 0, 0],
[-1, 0, 0, 0],
[0, 0, 0, 1]])))示例11: resolvePackageFilename
def resolvePackageFilename(filename):
if not packagepath.PackageMap.isPackageUrl(filename):
return filename
if Geometry.PackageMap is None:
import pydrake
m = packagepath.PackageMap()
m.populateFromSearchPaths([pydrake.getDrakePath()])
m.populateFromEnvironment(['DRAKE_PACKAGE_PATH', 'ROS_PACKAGE_PATH'])
Geometry.PackageMap = m
return Geometry.PackageMap.resolveFilename(filename) or filename示例12: test_big_gradient
def test_big_gradient(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
q = toAutoDiff(np.zeros((7,1)), np.eye(7, 100))
v = toAutoDiff(np.zeros((7,1)), np.zeros((7, 100)))
kinsol = r.doKinematics(q, v)
point = np.ones((3,1))
p = r.transformPoints(kinsol, point, 2, 0)
self.assertTrue(np.allclose(p.value(), np.ones((3,1))))
self.assertTrue(np.allclose(p.derivatives()[:3,:7],
np.array([[1, 0, 0, 0, 1, -1, 1],
[0, 1, 0, -1, 0, 1, 0],
[0, 0, 1, 1, -1, 0, -1]])))示例13: testPostureConstraint
def testPostureConstraint(self):
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"))
q = -0.9
posture_constraint = ik.PostureConstraint(r)
posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32),
np.array([[q]]),
np.array([[q]]))
# Choose a seed configuration (randomly) and a nominal configuration
# (at 0)
q_seed = np.vstack((np.zeros((6, 1)),
0.8147))
q_nom = np.vstack((np.zeros((6, 1)),
0.))
options = ik.IKoptions(r)
results = ik.InverseKin(
r, q_seed, q_nom, [posture_constraint], options)
self.assertEqual(results.info[0], 1)
self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)
# Run the tests again both pointwise and as a trajectory to
# validate the interfaces.
t = np.array([0., 1.])
q_seed_array = np.transpose(np.array([q_seed, q_seed]))[0]
q_nom_array = np.transpose(np.array([q_nom, q_nom]))[0]
results = ik.InverseKinPointwise(r, t, q_seed_array, q_nom_array,
[posture_constraint], options)
self.assertEqual(len(results.info), 2)
self.assertEqual(len(results.q_sol), 2)
self.assertEqual(results.info[0], 1)
# The pointwise result will go directly to the constrained
# value.
self.assertAlmostEqual(results.q_sol[0][6], q, 1e-9)
self.assertEqual(results.info[1], 1)
self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)
results = ik.InverseKinTraj(r, t, q_seed_array, q_nom_array,
[posture_constraint], options)
self.assertEqual(len(results.info), 1)
self.assertEqual(len(results.q_sol), 2)
self.assertEqual(results.info[0], 1)
# The trajectory result starts at the initial value and moves
# to the constrained value.
self.assertAlmostEqual(results.q_sol[0][6], q_seed[6], 1e-9)
self.assertAlmostEqual(results.q_sol[1][6], q, 1e-9)示例14: testCoMJacobian
def testCoMJacobian(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"))
q = r.getRandomConfiguration()
kinsol = r.doKinematics(q, np.zeros((7, 1)))
J = r.centerOfMassJacobian(kinsol)
self.assertTrue(np.shape(J) == (3, 7))
q = r.getZeroConfiguration()
kinsol = r.doKinematics(q, np.zeros((7, 1)))
J = r.centerOfMassJacobian(kinsol)
self.assertTrue(np.allclose(J.flat, [1., 0., 0., 0., -0.2425, 0., -0.25,
0., 1., 0., 0.2425, 0., 0., 0.,
0., 0., 1., 0., 0., 0., 0.], atol=1e-4))示例15: testPostureConstraint
def testPostureConstraint(self):
r = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/Pendulum/Pendulum.urdf"))
q = -0.9
posture_constraint = ik.PostureConstraint(r)
posture_constraint.setJointLimits(np.array([[6]], dtype=np.int32),
np.array([[q]]),
np.array([[q]]))
# Choose a seed configuration (randomly) and a nominal configuration (at 0)
q_seed = np.vstack((np.zeros((6,1)),
0.8147))
q_nom = np.vstack((np.zeros((6,1)),
0.))
options = ik.IKoptions(r)
results = ik.inverseKinSimple(r,
q_seed,
q_nom,
[posture_constraint],
options)
self.assertAlmostEqual(results.q_sol[6], q, 1e-9)