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
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)
return rbt, pbrv
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"))
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", ""))
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"))
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", ""))
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"]
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"]
def setup_kuka(self, rbt):
iiwa_urdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
wsg50_sdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "wsg_50_description", "sdf",
"schunk_wsg_50.sdf")
table_sdf_path = os.path.join(
pydrake.getDrakePath(),
"examples", "kuka_iiwa_arm", "models", "table",
"extra_heavy_duty_table_surface_only_collision.sdf")
guillotine_path = "guillotine.sdf"
AddFlatTerrainToWorld(rbt)
table_frame_robot = RigidBodyFrame(
"table_frame_robot", rbt.world(),
[0.0, 0, 0], [0, 0, 0])
self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed,
table_frame_robot, rbt)
self.tabletop_indices.append(rbt.get_num_bodies()-1)
table_frame_fwd = RigidBodyFrame(
"table_frame_fwd", rbt.world(),
[0.7, 0, 0], [0, 0, 0])
self.add_model_wrapper(table_sdf_path, FloatingBaseType.kFixed,
table_frame_fwd, rbt)
self.tabletop_indices.append(rbt.get_num_bodies()-1)
robot_base_frame = RigidBodyFrame(
"robot_base_frame", rbt.world(),
[0.0, 0, self.table_top_z_in_world], [0, 0, 0])
self.add_model_wrapper(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
# Add gripper
gripper_frame = rbt.findFrame("iiwa_frame_ee")
self.add_model_wrapper(wsg50_sdf_path, FloatingBaseType.kFixed,
gripper_frame, rbt)
if self.with_knife:
# Add guillotine
guillotine_frame = RigidBodyFrame(
"guillotine_frame", rbt.world(),
[0.6, -0.41, self.table_top_z_in_world], [0, 0, 0])
self.add_model_wrapper(guillotine_path, FloatingBaseType.kFixed,
guillotine_frame, rbt)
self.guillotine_blade_index = \
rbt.FindBody("blade").get_body_index()
def setup_kuka(rbt):
iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
wsg50_sdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "wsg_50_description", "sdf",
"schunk_wsg_50.sdf")
table_sdf_path = os.path.join(
pydrake.getDrakePath(), "examples", "kuka_iiwa_arm", "models", "table",
"extra_heavy_duty_table_surface_only_collision.sdf")
object_urdf_path = os.path.join(pydrake.getDrakePath(), "examples",
"kuka_iiwa_arm", "models", "objects",
"block_for_pick_and_place.urdf")
AddFlatTerrainToWorld(rbt)
table_frame_robot = RigidBodyFrame("table_frame_robot", rbt.world(),
[0.0, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed,
table_frame_robot, rbt)
table_frame_fwd = RigidBodyFrame("table_frame_fwd", rbt.world(),
[0.8, 0, 0], [0, 0, 0])
AddModelInstancesFromSdfString(
open(table_sdf_path).read(), FloatingBaseType.kFixed, table_frame_fwd,
rbt)
table_top_z_in_world = 0.736 + 0.057 / 2
robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(),
[0.0, 0, table_top_z_in_world],
[0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
object_init_frame = RigidBodyFrame("object_init_frame", rbt.world(),
[0.8, 0, table_top_z_in_world + 0.1],
[0, 0, 0])
AddModelInstanceFromUrdfFile(object_urdf_path,
FloatingBaseType.kRollPitchYaw,
object_init_frame, rbt)
# Add gripper
gripper_frame = rbt.findFrame("iiwa_frame_ee")
AddModelInstancesFromSdfString(
open(wsg50_sdf_path).read(), FloatingBaseType.kFixed, gripper_frame,
rbt)
def test():
import os
import pydrake
from director import roboturdf
filename = os.path.join(pydrake.getDrakePath(), 'examples/PR2/pr2.urdf')
assert os.path.isfile(filename)
robotModel, jointController = roboturdf.loadRobotModel(urdfFile=filename,
view=view,
useConfigFile=False)
conf = {
'pr2LeftGripper': [0.07],
'pr2RightArm':
[-0.91698, 0.042600, -1.5, -2.01531, -1.57888, -1.65300, -2.04511],
'pr2Base': [0.0, 0.0, 0.0],
'pr2Torso': [0.3],
'pr2RightGripper': [0.07],
'pr2Head': [0.0, 0.0],
'pr2LeftArm': [2.1, 1.29, 0.0, -0.15, 0.0, -0.1, 0.0]
}
t = BHPNTranslator(jointController)
t.setRobotConf(conf)
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))
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))
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)
def test_flat_terrain(self):
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
# Test that AddFlatTerrainToWorld is spelled correctly.
AddFlatTerrainToWorld(tree)
def test_kinematics_com_api(self):
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
q = np.zeros(num_q)
v = np.zeros(num_v)
self.assertTrue(np.allclose(q, tree.getZeroConfiguration()))
kinsol = tree.doKinematics(q, v)
# Full center of mass.
c = tree.centerOfMass(kinsol)
self.assertTrue(np.allclose(c, [0.0, 0.0, -0.2425]))
# - Jacobian.
Jc = tree.centerOfMassJacobian(kinsol)
Jc_expected = np.array([[1., 0., 0., 0., -0.2425, 0., -0.25],
[0., 1., 0., 0.2425, 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0.]])
self.assertTrue(np.allclose(Jc, Jc_expected))
# Specific body.
arm_com = tree.FindBody("arm_com")
c = arm_com.get_center_of_mass()
self.assertTrue(np.allclose(c, [0.0, 0.0, -0.5]))
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)
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)
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))
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))
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))))
def add_robot(rbt): # type: (RigidBodyTree) -> None
# The iiwa link path
iiwa_urdf_path = os.path.join(pydrake.getDrakePath(), "manipulation",
"models", "iiwa_description", "urdf",
"iiwa14_primitive_collision.urdf")
robot_base_frame = RigidBodyFrame("robot_base_frame", rbt.world(),
[0.0, 0.0, 0.0], [0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
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))))
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 addDrakeConfigShortcuts(self, directorConfig):
import pydrake
drakePath = pydrake.getDrakePath()
directorConfig.add_argument(
'--iiwa-drake',
dest='directorConfigFile',
action='store_const',
const=os.path.join(drakePath,
'examples/kuka_iiwa_arm/director_config.json'),
help='Use KUKA IIWA from drake/examples')
def test_kinematics_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
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]])))
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
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
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]])))
def test_shapes_parsing(self):
# TODO(gizatt) This test ought to have its reliance on
# the Pendulum model specifics stripped (so that e.g. material changes
# don't break the test), and split pure API testing of
# VisualElement and Geometry over to shapes_test while keeping
# RigidBodyTree and RigidBody visual element extraction here.
urdf_path = os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
# "base_part2" should have a single visual element.
base_part2 = tree.FindBody("base_part2")
self.assertIsNotNone(base_part2)
visual_elements = base_part2.get_visual_elements()
self.assertEqual(len(visual_elements), 1)
self.assertIsInstance(visual_elements[0], shapes.VisualElement)
# It has green material by default
sphere_visual_element = visual_elements[0]
green_material = np.array([0.3, 0.6, 0.4, 1])
white_material = np.array([1., 1., 1., 1.])
self.assertTrue(
np.allclose(sphere_visual_element.getMaterial(), green_material))
sphere_visual_element.setMaterial(white_material)
self.assertTrue(
np.allclose(sphere_visual_element.getMaterial(), white_material))
# We expect this link TF to have positive z-component...
local_tf = sphere_visual_element.getLocalTransform()
self.assertAlmostEqual(local_tf[2, 3], 0.015)
# ... as well as sphere geometry.
self.assertTrue(sphere_visual_element.hasGeometry())
sphere_geometry = sphere_visual_element.getGeometry()
self.assertIsInstance(sphere_geometry, shapes.Sphere)
self.assertEqual(sphere_geometry.getShape(), shapes.Shape.SPHERE)
self.assertNotEqual(sphere_geometry.getShape(), shapes.Shape.BOX)
# For a sphere geometry, getPoints() should return
# one point at the center of the sphere.
sphere_geometry_pts = sphere_geometry.getPoints()
self.assertEqual(sphere_geometry_pts.shape, (3, 1))
sphere_geometry_bb = sphere_geometry.getBoundingBoxPoints()
self.assertEqual(sphere_geometry_bb.shape, (3, 8))
# Sphere's don't have faces supplied (yet?)
self.assertFalse(sphere_geometry.hasFaces())
with self.assertRaises(RuntimeError):
sphere_geometry.getFaces()
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)
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]])))
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)
def test_fk_pose_world_frame(self):
# load pendulum robot with fixed base
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"),
pydrake.rbtree.FloatingBaseType.kFixed)
# set test configuration (Pendulum robot has single joint 'theta')
q = r.getZeroConfiguration()
q[0] = np.pi / 2
# do FK and compare pose of 'arm' with expected pose
k = r.doKinematics(q)
pose_fk = r.CalcBodyPoseInWorldFrame(k, r.FindBody("arm"))
pose_true = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(pose_fk, pose_true))
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))
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)
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]])))
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))
def testAddModelInstanceFromUrdfStringSearchingInRosPackages(self):
urdf_file = os.path.join(pydrake.getDrakePath(),
"examples/pr2/pr2.urdf")
urdf_string = open(urdf_file).read()
base_dir = os.path.dirname(urdf_file)
package_map = pydrake.rbtree.PackageMap()
weld_frame = None
floating_base_type = pydrake.rbtree.kRollPitchYaw
robot = pydrake.rbtree.RigidBodyTree()
pydrake.rbtree.AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string, package_map, base_dir, floating_base_type, weld_frame,
robot)
expected_num_bodies = 83
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))
def construct_iiwa_simple():
import os
import pydrake
from pydrake.all import AddFlatTerrainToWorld, RigidBodyFrame, AddModelInstanceFromUrdfFile, FloatingBaseType
# The rigid body tree
rbt = RigidBodyTree()
AddFlatTerrainToWorld(rbt)
# The iiwa
iiwa_urdf_path = os.path.join(
pydrake.getDrakePath(),
"manipulation", "models", "iiwa_description", "urdf",
"iiwa14_polytope_collision.urdf")
robot_base_frame = RigidBodyFrame(
"robot_base_frame", rbt.world(),
[0.0, 0.0, 0.0], [0, 0, 0])
AddModelInstanceFromUrdfFile(iiwa_urdf_path, FloatingBaseType.kFixed,
robot_base_frame, rbt)
return rbt
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)
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))
# Check actuators.
actuator = robot.GetActuator("head_pan_motor")
self.assertIsInstance(actuator, RigidBodyActuator)
self.assertEqual(actuator.name, "head_pan_motor")
self.assertIs(actuator.body, robot.FindBody("head_pan_link"))
self.assertEqual(actuator.reduction, 6.0)
self.assertEqual(actuator.effort_limit_min, -2.645)
self.assertEqual(actuator.effort_limit_max, 2.645)
# Check full number of actuators.
self.assertEqual(len(robot.actuators), robot.get_num_actuators())
for actuator in robot.actuators:
self.assertIsInstance(actuator, RigidBodyActuator)
def test_dynamics_api(self):
urdf_path = os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kRollPitchYaw)
def assert_sane(x, nonzero=True):
self.assertTrue(np.all(np.isfinite(x)))
if nonzero:
self.assertTrue(np.any(x != 0))
num_q = num_v = 7
num_u = tree.get_num_actuators()
self.assertEquals(num_u, 1)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Update kinematics.
kinsol = tree.doKinematics(q, v)
# Sanity checks:
# - Actuator map.
self.assertEquals(tree.B.shape, (num_v, num_u))
B_expected = np.zeros((num_v, num_u))
B_expected[-1] = 1
self.assertTrue(np.allclose(tree.B, B_expected))
# - Mass matrix.
H = tree.massMatrix(kinsol)
self.assertEquals(H.shape, (num_v, num_v))
assert_sane(H)
self.assertTrue(np.allclose(H[-1, -1], 0.25))
# - Bias terms.
C = tree.dynamicsBiasTerm(kinsol, {})
self.assertEquals(C.shape, (num_v, ))
assert_sane(C)
# - Inverse dynamics.
vd = np.zeros(num_v)
tau = tree.inverseDynamics(kinsol, {}, vd)
assert_sane(tau)
# - Friction torques.
friction_torques = tree.frictionTorques(v)
self.assertTrue(friction_torques.shape, (num_v, ))
assert_sane(friction_torques, nonzero=False)
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))
# Check actuators.
actuator = robot.GetActuator("head_pan_motor")
self.assertIsInstance(actuator, RigidBodyActuator)
self.assertEqual(actuator.name, "head_pan_motor")
self.assertIs(actuator.body, robot.FindBody("head_pan_link"))
self.assertEqual(actuator.reduction, 6.0)
self.assertEqual(actuator.effort_limit_min, -2.645)
self.assertEqual(actuator.effort_limit_max, 2.645)
# Check full number of actuators.
self.assertEqual(len(robot.actuators), robot.get_num_actuators())
for actuator in robot.actuators:
self.assertIsInstance(actuator, RigidBodyActuator)
def test_gradient(self):
r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/Pendulum/Pendulum.urdf"),
pydrake.rbtree.FloatingBaseType.kRollPitchYaw)
def do_transform(q):
kinsol = r.doKinematics(q)
point = np.ones((3, 1))
return r.transformPoints(kinsol, point, 2, 0)
q = np.zeros(7)
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones((3, 1))))
g = fd.jacobian(do_transform, q)
self.assertTrue(np.allclose(g,
np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])))
def testAddModelInstanceFromUrdfStringSearchingInRosPackages(self):
urdf_file = os.path.join(pydrake.getDrakePath(),
"examples/PR2/pr2.urdf")
urdf_string = open(urdf_file).read()
base_dir = os.path.dirname(urdf_file)
package_map = pydrake.rbtree.PackageMap()
weld_frame = None
floating_base_type = pydrake.rbtree.kRollPitchYaw
robot = pydrake.rbtree.RigidBodyTree()
pydrake.rbtree.AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
expected_num_bodies = 83
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))
def test_api(self):
box_size = [1., 2., 3.]
radius = 0.1
length = 0.2
box = shapes.Box(size=box_size)
self.assertTrue(np.allclose(box.size, box_size))
self.assertEqual(box.getPoints().shape, (3, 8))
self.assertEqual(len(box.getFaces()), 12)
self.assertEqual(len(box.getFaces()[0]), 3)
sphere = shapes.Sphere(radius=radius)
self.assertEqual(sphere.radius, radius)
self.assertEqual(sphere.getPoints().shape, (3, 1))
with self.assertRaises(RuntimeError):
sphere.getFaces()
cylinder = shapes.Cylinder(radius=radius, length=length)
self.assertEqual(cylinder.radius, radius)
self.assertEqual(cylinder.length, length)
capsule = shapes.Capsule(radius=radius, length=length)
self.assertEqual(capsule.radius, radius)
self.assertEqual(capsule.length, length)
pts = np.tile(box_size, (10, 1)).T
mesh_points = shapes.MeshPoints(pts)
self.assertEqual(mesh_points.getPoints().shape, (3, 10))
obj_mesh_path = os.path.join(
pydrake.getDrakePath(), "examples/quadrotor/quadrotor_base.obj")
obj_mesh_uri = "box_obj"
mesh = shapes.Mesh(uri=obj_mesh_uri, resolved_filename=obj_mesh_path)
self.assertTrue(np.allclose(mesh.scale, [1., 1., 1.]))
self.assertEqual(mesh.uri, obj_mesh_uri)
self.assertEqual(mesh.resolved_filename, obj_mesh_path)
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 directory 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"))
# Load our model from URDF
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
return robot
urdf_file = os.path.join(
pydrake.getDrakePath(),
"examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
# Load our model from URDF
robot = load_robot_from_urdf(urdf_file)
# Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link
# along that link's x axis
robot.addFrame(RigidBodyFrame(
"r_hand_frame", robot.FindBody("r_gripper_palm_link"),
np.array([0.1, 0, 0]), np.array([0., 0, 0])))
# Make sure attribute access works on bodies
assert robot.world().get_name() == "world"
hand_frame_id = robot.findFrame("r_hand_frame").get_frame_index()
import os
import numpy as np
import pydrake
from pydrake.solvers import ik
robot = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf"))
# Make sure attribute access works on bodies
assert robot.world().name() == "world"
constraints = [ik.WorldPositionConstraint(robot, 1,
np.zeros((3,)),
np.array([0,0,1.0]),
np.array([0,0,1.0])),
ik.WorldPositionConstraint(robot, robot.findLink("r_gripper_palm_link").body_index,
np.zeros((3,)),
np.array([0.5,0,1.5]),
np.array([0.5,0,1.5])),
]
q_seed = robot.getZeroConfiguration()
options = ik.IKoptions(robot)
results = ik.inverseKinSimple(robot, q_seed, q_seed, constraints, options)
print repr(results.q_sol)
def setUp(self):
self.r = pydrake.rbtree.RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
floating_base_type = pydrake.rbtree.kRollPitchYaw
# Load our model from URDF
robot = pydrake.rbtree.RigidBodyTree()
pydrake.rbtree.AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
return robot
urdf_file = os.path.join(pydrake.getDrakePath(), "examples/PR2/pr2.urdf")
# Load our model from URDF
robot = load_robot_from_urdf(urdf_file)
# Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link
# along that link's x axis
robot.addFrame(
pydrake.rbtree.RigidBodyFrame("r_hand_frame",
robot.FindBody("r_gripper_palm_link"),
np.array([0.1, 0, 0]),
np.array([0., 0, 0])))
# Make sure attribute access works on bodies
assert robot.world().get_name() == "world"
import os
import numpy as np
import pydrake
from pydrake.solvers import ik
# Load our model from URDF
robot = pydrake.rbtree.RigidBodyTree(os.path.join(pydrake.getDrakePath(),
"examples/PR2/pr2.urdf"))
# Add a convenient frame, positioned 0.1m away from the r_gripper_palm_link
# along that link's x axis
robot.addFrame(
pydrake.rbtree.RigidBodyFrame("r_hand_frame",
robot.FindBody("r_gripper_palm_link"),
np.array([0.1, 0, 0]),
np.array([0., 0, 0])))
# Make sure attribute access works on bodies
assert robot.world().get_name() == "world"
hand_frame_id = robot.findFrame("r_hand_frame").frame_index
base_body_id = robot.FindBody('base_footprint').get_body_index()
constraints = [
# These three constraints ensure that the base of the robot is
# at z = 0 and has no pitch or roll. Instead of directly
# constraining orientation, we just require that the points at
# [0, 0, 0], [1, 0, 0], and [0, 1, 0] in the robot's base's
# frame must all be at z = 0 in world frame.
# We don't care about the x or y position of the robot's base,
# so we use NaN values to tell the IK solver not to apply a
def test_api(self):
urdf_path = os.path.join(
getDrakePath(), "examples/pendulum/Pendulum.urdf")
for is_discrete in [False, True]:
tree = RigidBodyTree(
urdf_path, floating_base_type=FloatingBaseType.kFixed)
if is_discrete:
timestep = 0.1
plant = mut.RigidBodyPlant(tree, timestep)
else:
timestep = 0.0
plant = mut.RigidBodyPlant(tree)
model_id = 0
# Tested in order in which they're declared in `multibody_py.cc`,
# when able.
plant.set_contact_model_parameters(
mut.CompliantContactModelParameters())
plant.set_default_compliant_material(mut.CompliantMaterial())
self.assertTrue(plant.get_rigid_body_tree() is tree)
self.assertEquals(plant.get_num_bodies(), tree.get_num_bodies())
self.assertEquals(
plant.get_num_positions(), tree.get_num_positions())
self.assertEquals(
plant.get_num_positions(model_id), tree.get_num_positions())
self.assertEquals(
plant.get_num_velocities(), tree.get_num_velocities())
self.assertEquals(
plant.get_num_velocities(model_id), tree.get_num_velocities())
num_states = plant.get_num_positions() + plant.get_num_velocities()
self.assertEquals(plant.get_num_states(), num_states)
self.assertEquals(plant.get_num_states(model_id), num_states)
num_actuators = 1
self.assertEquals(plant.get_num_actuators(), num_actuators)
self.assertEquals(plant.get_num_actuators(model_id), num_actuators)
self.assertEquals(
plant.get_num_model_instances(), 1)
self.assertEquals(
plant.get_input_size(), plant.get_num_actuators())
self.assertEquals(plant.get_output_size(), plant.get_num_states())
context = plant.CreateDefaultContext()
x = plant.GetStateVector(context)
plant.SetDefaultState(context, context.get_mutable_state())
plant.set_position(context, 0, 1.)
self.assertEquals(x[0], 1.)
plant.set_velocity(context, 0, 2.)
self.assertEquals(x[1], 2.)
plant.set_state_vector(context, [3., 3.])
self.assertTrue(np.allclose(x, [3., 3.]))
plant.set_state_vector(context.get_mutable_state(), [4., 4.])
self.assertTrue(np.allclose(x, [4., 4.]))
self.assertEquals(
plant.FindInstancePositionIndexFromWorldIndex(0, 0), 0)
# Existence checks.
self.assertTrue(plant.actuator_command_input_port() is not None)
self.assertTrue(plant.model_instance_has_actuators(model_id))
self.assertTrue(
plant.model_instance_actuator_command_input_port(model_id)
is not None)
self.assertTrue(
plant.state_output_port() is not None)
self.assertTrue(
plant.model_instance_state_output_port(model_id) is not None)
self.assertTrue(
plant.torque_output_port() is not None)
self.assertTrue(
plant.model_instance_torque_output_port(model_id) is not None)
self.assertTrue(
plant.kinematics_results_output_port() is not None)
self.assertTrue(
plant.contact_results_output_port() is not None)
# Basic status.
self.assertEquals(plant.is_state_discrete(), is_discrete)
self.assertEquals(plant.get_time_step(), timestep)
