def test_inverse_dynamics_controller(self):
urdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/urdf/iiwa14_primitive_collision.urdf")
tree = RigidBodyTree(
urdf_path, floating_base_type=FloatingBaseType.kFixed)
kp = np.array([1., 2., 3., 4., 5., 6., 7.])
ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
controller = InverseDynamicsController(robot=tree,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=True)
context = controller.CreateDefaultContext()
output = controller.AllocateOutput()
estimated_state_port = 0
desired_state_port = 1
desired_acceleration_port = 2
control_port = 0
self.assertEqual(
controller.get_input_port(desired_acceleration_port).size(), 7)
self.assertEqual(
controller.get_input_port(estimated_state_port).size(), 14)
self.assertEqual(
controller.get_input_port(desired_state_port).size(), 14)
self.assertEqual(
controller.get_output_port(control_port).size(), 7)
# current state
q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
x = np.concatenate([q, v])
# reference state and acceleration
q_r = q + 0.1*np.ones_like(q)
v_r = v + 0.1*np.ones_like(v)
x_r = np.concatenate([q_r, v_r])
vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])
integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])
vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term
context.FixInputPort(estimated_state_port, BasicVector(x))
context.FixInputPort(desired_state_port, BasicVector(x_r))
context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
controller.set_integral_value(context, integral_term)
# compute the expected torque
cache = tree.doKinematics(q, v)
expected_torque = tree.massMatrix(cache).dot(vd_d) + \
tree.dynamicsBiasTerm(cache, {})
controller.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(),
expected_torque))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("filename")
args = parser.parse_args()
# Load the model file.
tree = RigidBodyTree()
ext = os.path.splitext(args.filename)[1]
if ext == ".sdf":
AddModelInstancesFromSdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
elif ext == ".urdf":
AddModelInstanceFromUrdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
else:
parser.error("Unknown extension " + ext)
# Send drake-visualizer messages to load the model and then position it in
# its default configuration.
q = tree.getZeroConfiguration()
v = np.zeros(tree.get_num_velocities())
lcm = DrakeLcm()
visualizer = DrakeVisualizer(tree=tree, lcm=lcm)
visualizer.PublishLoadRobot()
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, BasicVector(np.concatenate([q, v])))
visualizer.Publish(context)
def test_kinematics_com_api(self):
tree = RigidBodyTree(FindResourceOrThrow(
"drake/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 __init__(self, file_name , Is_fixed = True ):
rb_tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
AddFlatTerrainToWorld(rb_tree, 1000, 10)
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])
if Is_fixed is True:
self.fixed_type = FloatingBaseType.kFixed
else:
self.fixed_type = FloatingBaseType.kRollPitchYaw
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(file_name))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(file_name, 'r').read(),
pmap,
os.path.dirname(file_name),
self.fixed_type, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb_tree)
self.rb_tree = rb_tree
self.joint_limit_max = self.rb_tree.joint_limit_max
self.joint_limit_min = self.rb_tree.joint_limit_min
print(self.joint_limit_max)
print(self.joint_limit_min)
def test_inverse_dynamics_controller(self):
urdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/urdf/iiwa14_primitive_collision.urdf")
tree = RigidBodyTree(
urdf_path, floating_base_type=FloatingBaseType.kFixed)
kp = np.array([1., 2., 3., 4., 5., 6., 7.])
ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
controller = InverseDynamicsController(robot=tree,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=True)
context = controller.CreateDefaultContext()
output = controller.AllocateOutput()
estimated_state_port = 0
desired_state_port = 1
desired_acceleration_port = 2
control_port = 0
self.assertEqual(
controller.get_input_port(desired_acceleration_port).size(), 7)
self.assertEqual(
controller.get_input_port(estimated_state_port).size(), 14)
self.assertEqual(
controller.get_input_port(desired_state_port).size(), 14)
self.assertEqual(
controller.get_output_port(control_port).size(), 7)
# current state
q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
x = np.concatenate([q, v])
# reference state and acceleration
q_r = q + 0.1*np.ones_like(q)
v_r = v + 0.1*np.ones_like(v)
x_r = np.concatenate([q_r, v_r])
vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])
integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])
vd_d = vd_r + kp*(q_r-q) + kd*(v_r-v) + ki*integral_term
context.FixInputPort(estimated_state_port, BasicVector(x))
context.FixInputPort(desired_state_port, BasicVector(x_r))
context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
controller.set_integral_value(context, integral_term)
# compute the expected torque
cache = tree.doKinematics(q, v)
expected_torque = tree.massMatrix(cache).dot(vd_d) + \
tree.dynamicsBiasTerm(cache, {})
controller.CalcOutput(context, output)
self.assertTrue(np.allclose(output.get_vector_data(0).CopyToVector(),
expected_torque))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("filename")
args = parser.parse_args()
# Load the model file.
tree = RigidBodyTree()
ext = os.path.splitext(args.filename)[1]
if ext == ".sdf":
AddModelInstancesFromSdfFile(
args.filename, FloatingBaseType.kFixed, None, tree)
elif ext == ".urdf":
AddModelInstanceFromUrdfFile(
args.filename, FloatingBaseType.kFixed, None, tree)
else:
parser.error("Unknown extension " + ext)
# Send drake-visualizer messages to load the model and then position it in
# its default configuration.
q = tree.getZeroConfiguration()
v = np.zeros(tree.get_num_velocities())
lcm = DrakeLcm()
visualizer = DrakeVisualizer(tree=tree, lcm=lcm)
visualizer.PublishLoadRobot()
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, np.concatenate([q, v]))
visualizer.Publish(context)
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_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 = FindResourceOrThrow(
"drake/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()
# Add a visual element just to test the spelling of AddVisualElement.
tree.world().AddVisualElement(sphere_visual_element)
# Test that I can call compile.
tree.compile()
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_rgbd_camera(self):
sdf_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/nothing.sdf")
tree = RigidBodyTree()
with open(sdf_path) as f:
sdf_string = f.read()
AddModelInstancesFromSdfString(sdf_string, FloatingBaseType.kFixed,
None, tree)
frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link"))
tree.addFrame(frame)
# Use HDTV size.
width = 1280
height = 720
camera = mut.RgbdCamera(name="camera",
tree=tree,
frame=frame,
z_near=0.5,
z_far=5.0,
fov_y=np.pi / 4,
show_window=False,
width=width,
height=height)
def check_info(camera_info):
self.assertIsInstance(camera_info, mut.CameraInfo)
self.assertEqual(camera_info.width(), width)
self.assertEqual(camera_info.height(), height)
check_info(camera.color_camera_info())
check_info(camera.depth_camera_info())
self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3)
self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3)
self.assertTrue(camera.tree() is tree)
# N.B. `RgbdCamera` copies the input frame.
self.assertEqual(camera.frame().get_name(), frame.get_name())
self._check_ports(camera)
# Test discrete camera.
period = mut.RgbdCameraDiscrete.kDefaultPeriod
discrete = mut.RgbdCameraDiscrete(camera=camera,
period=period,
render_label_image=True)
self.assertTrue(discrete.camera() is camera)
self.assertTrue(discrete.mutable_camera() is camera)
self.assertEqual(discrete.period(), period)
self._check_ports(discrete)
# That we can access the state as images.
context = discrete.CreateDefaultContext()
values = context.get_abstract_state()
self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U])
self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F])
self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
def test_accessor_api(self):
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/simple_four_bar/FourBar.urdf"))
bodies = tree.get_bodies()
self.assertGreater(len(bodies), 0)
for body in bodies:
self.assertIsInstance(body, RigidBody)
frames = tree.get_frames()
self.assertGreater(len(frames), 0)
for frame in frames:
self.assertIsInstance(frame, RigidBodyFrame)
def test_accessor_api(self):
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/simple_four_bar/FourBar.urdf"))
bodies = tree.get_bodies()
self.assertGreater(len(bodies), 0)
for body in bodies:
self.assertIsInstance(body, RigidBody)
frames = tree.get_frames()
self.assertGreater(len(frames), 0)
for frame in frames:
self.assertIsInstance(frame, RigidBodyFrame)
def test_inverse_dynamics(self):
urdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/urdf/iiwa14_primitive_collision.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
num_v = tree.get_num_velocities()
def compute_torque(tree, q, v, v_dot):
cache = tree.doKinematics(q, v)
return tree.massMatrix(cache).dot(v_dot) + \
tree.dynamicsBiasTerm(cache, {})
estimated_state_port = 0
desired_acceleration_port = 1
def check_torque_example(controller, q, v, v_dot_desired=None):
if controller.is_pure_gravity_compensation():
v_dot_desired = np.zeros(num_v)
context = controller.CreateDefaultContext()
x = np.concatenate([q, v])
context.FixInputPort(estimated_state_port, BasicVector(x))
if not controller.is_pure_gravity_compensation():
context.FixInputPort(desired_acceleration_port,
BasicVector(v_dot_desired))
output = controller.AllocateOutput()
controller.CalcOutput(context, output)
expected_torque = compute_torque(tree, q, v, v_dot_desired)
self.assertTrue(
np.allclose(
output.get_vector_data(0).CopyToVector(), expected_torque))
# Test with pure gravity compensation.
controller = RbtInverseDynamics(
tree=tree,
mode=RbtInverseDynamics.InverseDynamicsMode.kGravityCompensation)
q = np.array([.1, .2, .3, .4, .5, .6, .7])
v = np.zeros(num_v)
check_torque_example(controller, q, v)
# Test with desired acceleration.
controller = RbtInverseDynamics(
tree=tree,
mode=RbtInverseDynamics.InverseDynamicsMode.kInverseDynamics)
q = np.array([.7, .6, .5, .4, .3, .2, .1])
v = np.array([-.1, -.2, -.3, -.4, -.5, -.6, -.7])
v_dot_desired = np.array([-.1, .1, -.1, .1, -.1, .1, -.1])
check_torque_example(controller, q, v, v_dot_desired)
def test_inverse_dynamics(self):
urdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/urdf/iiwa14_primitive_collision.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
num_v = tree.get_num_velocities()
def compute_torque(tree, q, v, v_dot):
cache = tree.doKinematics(q, v)
return tree.massMatrix(cache).dot(v_dot) + \
tree.dynamicsBiasTerm(cache, {})
estimated_state_port = 0
desired_acceleration_port = 1
def check_torque_example(controller, q, v, v_dot_desired=None):
if controller.is_pure_gravity_compensation():
v_dot_desired = np.zeros(num_v)
context = controller.CreateDefaultContext()
x = np.concatenate([q, v])
context.FixInputPort(estimated_state_port, BasicVector(x))
if not controller.is_pure_gravity_compensation():
context.FixInputPort(desired_acceleration_port,
BasicVector(v_dot_desired))
output = controller.AllocateOutput()
controller.CalcOutput(context, output)
expected_torque = compute_torque(tree, q, v, v_dot_desired)
self.assertTrue(
np.allclose(
output.get_vector_data(0).CopyToVector(), expected_torque))
# Test with pure gravity compensation.
controller = InverseDynamics(
tree=tree,
mode=InverseDynamics.InverseDynamicsMode.kGravityCompensation)
q = np.array([.1, .2, .3, .4, .5, .6, .7])
v = np.zeros(num_v)
check_torque_example(controller, q, v)
# Test with desired acceleration.
controller = InverseDynamics(
tree=tree,
mode=InverseDynamics.InverseDynamicsMode.kInverseDynamics)
q = np.array([.7, .6, .5, .4, .3, .2, .1])
v = np.array([-.1, -.2, -.3, -.4, -.5, -.6, -.7])
v_dot_desired = np.array([-.1, .1, -.1, .1, -.1, .1, -.1])
check_torque_example(controller, q, v, v_dot_desired)
def run_pendulum_static_visualization():
import os
from pydrake.all import FloatingBaseType
vis_path = os.path.dirname(__file__)
pendulum_path = os.path.join(vis_path, 'data/pendulum.urdf')
rbt = RigidBodyTree(pendulum_path, floating_base_type=FloatingBaseType.kFixed)
# The zero config
nq = rbt.get_num_positions()
q = np.zeros(shape=nq)
# Draw it
MeshCatVisualizer.draw_rigidbody_tree(rbt, q)
def test_id_table(self):
robot = RigidBodyTree()
id_table = AddModelInstancesFromSdfFile(
FindResourceOrThrow("drake/examples/acrobot/Acrobot.sdf"),
FloatingBaseType.kRollPitchYaw, None, robot)
# Check IDs.
(name, id), = id_table.items()
self.assertEqual(name, "Acrobot")
self.assertEqual(id, 0)
# Ensure that we have our desired base body.
base_body_id, = robot.FindBaseBodies(id)
expected_body_id = robot.FindBody("base_link").get_body_index()
self.assertEqual(base_body_id, expected_body_id)
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 test_rgbd_camera(self):
sdf_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/nothing.sdf")
tree = RigidBodyTree()
with open(sdf_path) as f:
sdf_string = f.read()
AddModelInstancesFromSdfString(
sdf_string, FloatingBaseType.kFixed, None, tree)
frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link"))
tree.addFrame(frame)
# Use HDTV size.
width = 1280
height = 720
camera = mut.RgbdCamera(
name="camera", tree=tree, frame=frame,
z_near=0.5, z_far=5.0,
fov_y=np.pi / 4, show_window=False,
width=width, height=height)
def check_info(camera_info):
self.assertIsInstance(camera_info, mut.CameraInfo)
self.assertEqual(camera_info.width(), width)
self.assertEqual(camera_info.height(), height)
check_info(camera.color_camera_info())
check_info(camera.depth_camera_info())
self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3)
self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3)
self.assertTrue(camera.tree() is tree)
# N.B. `RgbdCamera` copies the input frame.
self.assertEqual(camera.frame().get_name(), frame.get_name())
self._check_ports(camera)
# Test discrete camera.
period = mut.RgbdCameraDiscrete.kDefaultPeriod
discrete = mut.RgbdCameraDiscrete(
camera=camera, period=period, render_label_image=True)
self.assertTrue(discrete.camera() is camera)
self.assertTrue(discrete.mutable_camera() is camera)
self.assertEqual(discrete.period(), period)
self._check_ports(discrete)
# That we can access the state as images.
context = discrete.CreateDefaultContext()
values = context.get_abstract_state()
self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U])
self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F])
self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
def test_kinematics_com_api(self):
tree = RigidBodyTree(
FindResourceOrThrow("drake/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))
# - JacobianDotTimesV
JcDotV = tree.centerOfMassJacobianDotTimesV(kinsol)
self.assertEqual(JcDotV.shape, (3, ))
# 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 load_robot_from_urdf(urdf_file):
"""
This function demonstrates how to pass a complete
set of arguments to Drake's URDF parser. It is also
possible to load a robot with a much simpler syntax
that uses default values, such as:
robot = RigidBodyTree(urdf_file)
"""
urdf_string = open(urdf_file).read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
# Load our model from URDF
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string,
package_map,
base_dir,
floating_base_type,
weld_frame,
robot)
return robot
def urdfViz(plant):
"""
Visualize the position/orientation of the vehicle along a trajectory using the PlanarRigidBodyVisualizer and
a basic .urdf file representing the vehicle, start, goal, and state constraints.
"""
tree = RigidBodyTree(FindResource("/notebooks/6832-code/ben_uav.urdf"),
FloatingBaseType.kRollPitchYaw)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-1, 15], ylim=[-0.5, 2.5])
tf = plant.ttraj[-1]
times = np.linspace(0, tf, 200)
# organize the relevant states for the visualizer
posn = np.zeros((13, len(times)))
for i, t in enumerate(times):
x = plant.xdtraj_poly.value(t)
u = plant.udtraj_poly.value(t)
posn[0:6, i] = 0
posn[6, i] = (x[0] - plant.xdtraj[:,0].min()) / 14 # x
posn[7, i] = 0 # z
posn[8, i] = x[1] # y
posn[9, i] = 0 # yz plane
posn[10, i] = np.pi / 2 - x[4] # pitch down
posn[11, i] = 0 # yaw
posn[12, i] = -u[1] # tail angle
test_poly = PiecewisePolynomial.FirstOrderHold(times, posn)
return vis.animate(test_poly, repeat=False)
def test_usage_all(self):
from pydrake.all import (
FindResourceOrThrow, RigidBodyPlant, RigidBodyTree, Simulator)
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
simulator = Simulator(RigidBodyPlant(tree))
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)
for robot in robot_1, robot_2:
expected_num_bodies = 4
self.assertEqual(robot.get_num_bodies(), expected_num_bodies)
def SetupLittleDog():
# Build up your Robot World
rb_tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0])
AddFlatTerrainToWorld(rb_tree, 1000, 10)
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0.2], [0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, #FloatingBaseType.kRollPitchYaw,
robot_frame,
rb_tree)
return rb_tree
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_usage_no_all(self):
from pydrake.common import FindResourceOrThrow
from pydrake.multibody.rigid_body_plant import RigidBodyPlant
from pydrake.multibody.rigid_body_tree import RigidBodyTree
from pydrake.systems.analysis import Simulator
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf"))
simulator = Simulator(RigidBodyPlant(tree))
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 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 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 load_robot(filename, **kwargs):
robot = RigidBodyTree()
if filename.endswith(".sdf"):
AddModelInstancesFromSdfFile(FindResourceOrThrow(filename),
FloatingBaseType.kRollPitchYaw,
None, robot, **kwargs)
else:
assert filename.endswith(".urdf")
AddModelInstanceFromUrdfFile(FindResourceOrThrow(filename),
FloatingBaseType.kRollPitchYaw,
None, robot, **kwargs)
return robot
def loadRigidBodyTree(self, urdfFile, packagePaths):
assert os.path.isfile(urdfFile)
packageMap = rbt.makePackageMap(packagePaths)
urdfString = open(urdfFile, 'r').read()
baseDir = str(os.path.dirname(urdfFile))
rigidBodyTree = RigidBodyTree()
rbt.addModelInstanceFromUrdfString(rigidBodyTree, urdfString,
packageMap, baseDir)
return rigidBodyTree
def test_frame_api(self):
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
# xyz + rpy
frame = RigidBodyFrame(
name="frame_1", body=tree.world(),
xyz=[0, 0, 0], rpy=[0, 0, 0])
self.assertEqual(frame.get_name(), "frame_1")
tree.addFrame(frame)
self.assertTrue(frame.get_frame_index() < 0, frame.get_frame_index())
self.assertTrue(frame.get_rigid_body() is tree.world())
self.assertIsInstance(frame.get_transform_to_body(), Isometry3)
self.assertTrue(tree.findFrame(frame_name="frame_1") is frame)
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 get_container_origin_xyz(urdf_path, container_dir):
tree = RigidBodyTree(urdf_path)
# set robot configuration
q = np.zeros(7) # arm position doesn't matter here so just use zeros
# look up indices of frames
base_idx = tree.FindBody('base_link').get_body_index()
container_idx = tree.FindBody(
os.path.basename(container_dir)).get_body_index()
# do kinematics with oint configuration
kinsol = tree.doKinematics(q)
# find relative transform between frames
T = tree.relativeTransform(kinsol, base_idx, container_idx)
return T[:3, 3]
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 SetupKinova():
# Build up your Robot World
rb_tree = RigidBodyTree()
world_frame = RigidBodyFrame("world_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
AddFlatTerrainToWorld(rb_tree, 1000, 10)
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, #FloatingBaseType.kRollPitchYaw,
robot_frame,
rb_tree)
print("Num of Joints : {}, \n Num of Actuators: {}".format(
rb_tree.get_num_positions(), rb_tree.get_num_actuators()))
return rb_tree
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(FindResourceOrThrow(
"drake/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))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
kinsol = tree.doKinematics(q)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
def test_constraint_api(self):
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/simple_four_bar/FourBar.urdf"))
num_q = 9
num_con = 7
bodyA = 1
bodyB = 2
point = np.ones(3)
distance = 1
q = tree.getZeroConfiguration()
v = np.zeros(num_q)
q_ad = np.array(list(map(AutoDiffXd, q)))
v_ad = np.array(list(map(AutoDiffXd, v)))
kinsol = tree.doKinematics(q, v)
kinsol_ad = tree.doKinematics(q_ad, v_ad)
self.assertEqual(tree.getNumPositionConstraints(), num_con - 1)
tree.addDistanceConstraint(bodyA, point, bodyB, point, distance)
self.assertEqual(tree.getNumPositionConstraints(), num_con)
constraint = tree.positionConstraints(kinsol)
J = tree.positionConstraintsJacobian(kinsol)
JdotV = tree.positionConstraintsJacDotTimesV(kinsol)
constraint_ad = tree.positionConstraints(kinsol_ad)
J_ad = tree.positionConstraintsJacobian(kinsol_ad)
JdotV_ad = tree.positionConstraintsJacDotTimesV(kinsol_ad)
self.assertEqual(constraint.shape, (num_con,))
self.assertEqual(J.shape, (num_con, num_q))
self.assertEqual(JdotV.shape, (num_con,))
self.assertEqual(constraint_ad.shape, (num_con,))
self.assertEqual(J_ad.shape, (num_con, num_q))
self.assertEqual(JdotV_ad.shape, (num_con,))
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)
def test_rigid_body_tree_programmatic_construction(self):
# Tests RBT programmatic construction methods by assembling
# a simple RBT with a prismatic and revolute joint, with
# both visual and collision geometry on the last joint.
rbt = RigidBodyTree()
world_body = rbt.world()
# body_1 is connected to the world via a prismatic joint along
# the +z axis.
body_1 = RigidBody()
body_1.set_name("body_1")
body_1_joint = PrismaticJoint("z", np.eye(4),
np.array([0., 0., 1.]))
body_1.add_joint(world_body, body_1_joint)
rbt.add_rigid_body(body_1)
# body_2 is connected to body_1 via a revolute joint around the z-axis.
body_2 = RigidBody()
body_2.set_name("body_2")
body_2_joint = RevoluteJoint("theta", np.eye(4),
np.array([0., 0., 1.]))
body_2.add_joint(body_1, body_2_joint)
box_element = shapes.Box([1.0, 1.0, 1.0])
box_visual_element = shapes.VisualElement(
box_element, np.eye(4), [1., 0., 0., 1.])
body_2.AddVisualElement(box_visual_element)
body_2_visual_elements = body_2.get_visual_elements()
rbt.add_rigid_body(body_2)
box_collision_element = CollisionElement(box_element, np.eye(4))
box_collision_element.set_body(body_2)
rbt.addCollisionElement(box_collision_element, body_2, "default")
# Define a collision filter group containing bodies 1 and 2 and make
# that group ignore itself.
rbt.DefineCollisionFilterGroup(name="test_group")
rbt.AddCollisionFilterGroupMember(
group_name="test_group", body_name="body_1", model_id=0)
rbt.AddCollisionFilterGroupMember(
group_name="test_group", body_name="body_2", model_id=0)
rbt.AddCollisionFilterIgnoreTarget("test_group", "test_group")
self.assertFalse(rbt.initialized())
rbt.compile()
self.assertTrue(rbt.initialized())
# The RBT's position vector should now be [z, theta].
self.assertEqual(body_1.get_position_start_index(), 0)
self.assertEqual(body_2.get_position_start_index(), 1)
self.assertIsNotNone(
rbt.FindCollisionElement(
body_2.get_collision_element_ids()[0]))
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(FindResourceOrThrow(
"drake/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)))
# Ensure mismatched sizes throw an error.
q_bad = np.zeros(num_q + 1)
v_bad = np.zeros(num_v + 1)
bad_args_list = (
(q_bad,),
(q_bad, v),
(q, v_bad),
)
for bad_args in bad_args_list:
with self.assertRaises(SystemExit):
tree.doKinematics(*bad_args)
# 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))
# Relative RPY, checking autodiff (#9886).
q[:] = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]
q_ad = np.array([AutoDiffXd(x) for x in q])
world = tree.findFrame("world")
frame = tree.findFrame("arm_com")
kinsol = tree.doKinematics(q)
rpy = tree.relativeRollPitchYaw(
cache=kinsol, from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
kinsol_ad = tree.doKinematics(q_ad)
rpy_ad = tree.relativeRollPitchYaw(
cache=kinsol_ad, from_body_or_frame_ind=world.get_frame_index(),
to_body_or_frame_ind=frame.get_frame_index())
for x, x_ad in zip(rpy, rpy_ad):
self.assertEqual(x, x_ad.value())
# 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))
# Geometric Jacobian.
# Construct a new tree with a quaternion floating base.
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"),
floating_base_type=FloatingBaseType.kQuaternion)
num_q = 8
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
# - Sanity check sizes.
J_default, v_indices_default = tree.geometricJacobian(kinsol, 0, 2, 0)
J_eeDotTimesV = tree.geometricJacobianDotTimesV(kinsol, 0, 2, 0)
self.assertEqual(J_default.shape[0], 6)
self.assertEqual(J_default.shape[1], num_v)
self.assertEqual(len(v_indices_default), num_v)
self.assertEqual(J_eeDotTimesV.shape[0], 6)
# - Check QDotToVelocity and VelocityToQDot methods
q = tree.getZeroConfiguration()
v_real = np.zeros(num_v)
q_ad = np.array(list(map(AutoDiffXd, q)))
v_real_ad = np.array(list(map(AutoDiffXd, v_real)))
kinsol = tree.doKinematics(q)
kinsol_ad = tree.doKinematics(q_ad)
qd = tree.transformVelocityToQDot(kinsol, v_real)
v = tree.transformQDotToVelocity(kinsol, qd)
qd_ad = tree.transformVelocityToQDot(kinsol_ad, v_real_ad)
v_ad = tree.transformQDotToVelocity(kinsol_ad, qd_ad)
self.assertEqual(qd.shape, (num_q,))
self.assertEqual(v.shape, (num_v,))
self.assertEqual(qd_ad.shape, (num_q,))
self.assertEqual(v_ad.shape, (num_v,))
v_to_qdot = tree.GetVelocityToQDotMapping(kinsol)
qdot_to_v = tree.GetQDotToVelocityMapping(kinsol)
v_to_qdot_ad = tree.GetVelocityToQDotMapping(kinsol_ad)
qdot_to_v_ad = tree.GetQDotToVelocityMapping(kinsol_ad)
self.assertEqual(v_to_qdot.shape, (num_q, num_v))
self.assertEqual(qdot_to_v.shape, (num_v, num_q))
self.assertEqual(v_to_qdot_ad.shape, (num_q, num_v))
self.assertEqual(qdot_to_v_ad.shape, (num_v, num_q))
v_map = tree.transformVelocityMappingToQDotMapping(kinsol,
np.eye(num_v))
qd_map = tree.transformQDotMappingToVelocityMapping(kinsol,
np.eye(num_q))
v_map_ad = tree.transformVelocityMappingToQDotMapping(kinsol_ad,
np.eye(num_v))
qd_map_ad = tree.transformQDotMappingToVelocityMapping(kinsol_ad,
np.eye(num_q))
self.assertEqual(v_map.shape, (num_v, num_q))
self.assertEqual(qd_map.shape, (num_q, num_v))
self.assertEqual(v_map_ad.shape, (num_v, num_q))
self.assertEqual(qd_map_ad.shape, (num_q, num_v))
# - Check FindBody and FindBodyIndex methods
body_name = "arm"
body = tree.FindBody(body_name)
self.assertEqual(body.get_body_index(), tree.FindBodyIndex(body_name))
# - Check ChildOfJoint methods
body = tree.FindChildBodyOfJoint("theta")
self.assertIsInstance(body, RigidBody)
self.assertEqual(body.get_name(), "arm")
self.assertEqual(tree.FindIndexOfChildBodyOfJoint("theta"), 2)
# - Check that default value for in_terms_of_qdot is false.
J_not_in_terms_of_q_dot, v_indices_not_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, False)
self.assertTrue((J_default == J_not_in_terms_of_q_dot).all())
self.assertEqual(v_indices_default, v_indices_not_in_terms_of_qdot)
# - Check with in_terms_of_qdot set to True.
J_in_terms_of_q_dot, v_indices_in_terms_of_qdot = \
tree.geometricJacobian(kinsol, 0, 2, 0, True)
self.assertEqual(J_in_terms_of_q_dot.shape[0], 6)
self.assertEqual(J_in_terms_of_q_dot.shape[1], num_q)
self.assertEqual(len(v_indices_in_terms_of_qdot), num_q)
# - Check transformPointsJacobian methods
q = tree.getZeroConfiguration()
v = np.zeros(num_v)
kinsol = tree.doKinematics(q, v)
Jv = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=False)
Jq = tree.transformPointsJacobian(cache=kinsol, points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1,
in_terms_of_qdot=True)
JdotV = tree.transformPointsJacobianDotTimesV(cache=kinsol,
points=np.zeros(3),
from_body_or_frame_ind=0,
to_body_or_frame_ind=1)
self.assertEqual(Jv.shape, (3, num_v))
self.assertEqual(Jq.shape, (3, num_q))
self.assertEqual(JdotV.shape, (3,))
# - Check that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
# - Check relative twist method
twist = tree.relativeTwist(cache=kinsol,
base_or_frame_ind=0,
body_or_frame_ind=1,
expressed_in_body_or_frame_ind=0)
self.assertEqual(twist.shape[0], 6)
def test_dynamics_api(self):
urdf_path = FindResourceOrThrow(
"drake/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)
# AutoDiff
q_ad = np.array(map(AutoDiffXd, q))
v_ad = np.array(map(AutoDiffXd, v))
kinsol_ad = tree.doKinematics(q_ad, v_ad)
# 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)
H_ad = tree.massMatrix(kinsol_ad)
self.assertEquals(H.shape, (num_v, num_v))
self.assertEquals(H_ad.shape, (num_v, num_v))
assert_sane(H)
self.assertTrue(np.allclose(H[-1, -1], 0.25))
# - Bias terms.
C = tree.dynamicsBiasTerm(kinsol, {})
C_ad = tree.dynamicsBiasTerm(kinsol_ad, {})
self.assertEquals(C.shape, (num_v,))
self.assertEquals(C_ad.shape, (num_v,))
assert_sane(C)
# - Inverse dynamics.
vd = np.zeros(num_v)
tau = tree.inverseDynamics(kinsol, {}, vd)
tau_ad = tree.inverseDynamics(kinsol_ad, {}, vd)
self.assertEquals(tau.shape, (num_v,))
self.assertEquals(tau_ad.shape, (num_v,))
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_rigid_body_tree_programmatic_construction(self):
# Tests RBT programmatic construction methods by assembling
# a simple RBT with a prismatic and revolute joint, with
# both visual and collision geometry on the last joint.
rbt = RigidBodyTree()
world_body = rbt.world()
# body_1 is connected to the world via a prismatic joint along
# the +z axis.
body_1 = RigidBody()
body_1.set_name("body_1")
body_1_joint = PrismaticJoint("z", np.eye(4), np.array([0., 0., 1.]))
body_1.add_joint(world_body, body_1_joint)
rbt.add_rigid_body(body_1)
# body_2 is connected to body_1 via a revolute joint around the z-axis.
body_2 = RigidBody()
body_2.set_name("body_2")
body_2_joint = RevoluteJoint("theta", np.eye(4), np.array([0., 0.,
1.]))
body_2.add_joint(body_1, body_2_joint)
box_element = shapes.Box([1.0, 1.0, 1.0])
box_visual_element = shapes.VisualElement(box_element, np.eye(4),
[1., 0., 0., 1.])
body_2.AddVisualElement(box_visual_element)
body_2_visual_elements = body_2.get_visual_elements()
rbt.add_rigid_body(body_2)
box_collision_element = CollisionElement(box_element, np.eye(4))
box_collision_element.set_body(body_2)
rbt.addCollisionElement(box_collision_element, body_2, "default")
# Define a collision filter group containing bodies 1 and 2 and make
# that group ignore itself.
rbt.DefineCollisionFilterGroup(name="test_group")
rbt.AddCollisionFilterGroupMember(group_name="test_group",
body_name="body_1",
model_id=0)
rbt.AddCollisionFilterGroupMember(group_name="test_group",
body_name="body_2",
model_id=0)
rbt.AddCollisionFilterIgnoreTarget("test_group", "test_group")
self.assertFalse(rbt.initialized())
rbt.compile()
self.assertTrue(rbt.initialized())
# The RBT's position vector should now be [z, theta].
self.assertEqual(body_1.get_position_start_index(), 0)
self.assertEqual(body_2.get_position_start_index(), 1)
self.assertIsNotNone(
rbt.FindCollisionElement(body_2.get_collision_element_ids()[0]))
FloatingBaseType,
RigidBodyTree,
RigidBodyFrame,
)
from pydrake.systems.framework import (
BasicVector,
)
from pydrake.systems.sensors import (
CameraInfo,
RgbdCamera,
)
# Create tree describing scene.
urdf_path = FindResourceOrThrow(
"drake/multibody/models/box.urdf")
tree = RigidBodyTree()
AddModelInstanceFromUrdfFile(
urdf_path, FloatingBaseType.kFixed, None, tree)
# - Add frame for camera fixture.
frame = RigidBodyFrame(
name="rgbd camera frame",
body=tree.world(),
xyz=[-2, 0, 2], # Ensure that the box is within range.
rpy=[0, np.pi / 4, 0])
tree.addFrame(frame)
# Create camera.
camera = RgbdCamera(
name="camera", tree=tree, frame=frame,
z_near=0.5, z_far=5.0,
fov_y=np.pi / 4, show_window=True)
