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 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 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 = mut.RbtInverseDynamicsController(
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 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 = attic_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 = attic_mut.RgbdCameraDiscrete.kDefaultPeriod
discrete = attic_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_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 = mut.RbtInverseDynamics(
tree=tree,
mode=mut.RbtInverseDynamics.InverseDynamicsMode.
kGravityCompensation) # noqa
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 = mut.RbtInverseDynamics(
tree=tree,
mode=mut.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 = mut.RbtInverseDynamics(
tree=tree,
mode=mut.RbtInverseDynamics.InverseDynamicsMode.kGravityCompensation) # noqa
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 = mut.RbtInverseDynamics(
tree=tree,
mode=mut.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_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_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 = attic_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 = attic_mut.RgbdCameraDiscrete.kDefaultPeriod
discrete = attic_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 setupPendulumExample():
rbt = RigidBodyTree(FindResource("pendulum/pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-1.2, 1.2], [-1.2, 1.2])
return rbt, pbrv
def setupDoublePendulumExample():
rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
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 setupDoublePendulumExample():
rbt = RigidBodyTree(FindResource("double_pendulum/double_pendulum.urdf"),
floating_base_type=FloatingBaseType.kFixed) # noqa
Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]],
dtype=np.float64)
fig, ax = plt.subplots(1, 1)
pbrv = PlanarRigidBodyVisualizer(rbt, Tview, [-2.5, 2.5], [-2.5, 2.5],
use_random_colors=True, ax=ax)
return rbt, pbrv
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_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_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 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_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_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 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.assertEqual(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(list(map(AutoDiffXd, q)))
v_ad = np.array(list(map(AutoDiffXd, v)))
kinsol_ad = tree.doKinematics(q_ad, v_ad)
# Sanity checks:
# - Actuator map.
self.assertEqual(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.assertEqual(H.shape, (num_v, num_v))
self.assertEqual(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.assertEqual(C.shape, (num_v,))
self.assertEqual(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.assertEqual(tau.shape, (num_v,))
self.assertEqual(tau_ad.shape, (num_v,))
assert_sane(tau)
# - Friction torques.
friction_torques = tree.frictionTorques(v)
self.assertEqual(friction_torques.shape, (num_v,))
assert_sane(friction_torques, nonzero=False)
# - Centroidal Momentum
id = set([0])
Ag = tree.centroidalMomentumMatrix(
cache=kinsol, model_instance_id_set=id, in_terms_of_qdot=False)
self.assertEqual(Ag.shape, (6, num_q))
AgDotV = tree.centroidalMomentumMatrixDotTimesV(
cache=kinsol, model_instance_id_set=id)
self.assertEqual(AgDotV.shape, (6,))
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_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 = mut.RbtInverseDynamicsController(
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 test_flat_terrain(self):
tree = RigidBodyTree(FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf"))
# Test that AddFlatTerrainToWorld is spelled correctly.
AddFlatTerrainToWorld(tree)
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_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 setUp(self):
self.r = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
def _make_tree(self):
urdf_path = FindResourceOrThrow(
"drake/examples/pendulum/Pendulum.urdf")
tree = RigidBodyTree(urdf_path,
floating_base_type=FloatingBaseType.kFixed)
return tree
class TestRBTIK(unittest.TestCase):
def setUp(self):
self.r = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
def testSingleTimeKinematicConstraint(self):
# Demonstrate that a subclass of SingleTimeKinematicConstraint
# inherits functional `eval` and `bounds` methods.
q = np.zeros(self.r.get_num_positions())
kinsol = self.r.doKinematics(q)
ub = np.array([0.0, 0.0, -0.45])
lb = np.array([0.0, 0.0, -0.55])
# This point, at the zero configuration, is
# at [0, 0, -1] in world frame as well.
body_pt = np.array([0., 0., -1])
constraint = ik.WorldPositionConstraint(self.r,
self.r.FindBodyIndex("arm"),
body_pt, lb, ub)
self.assertIsInstance(constraint, ik.SingleTimeKinematicConstraint)
lb_inspect, ub_inspect = constraint.bounds(t=0.)
self.assertTrue(np.allclose(lb_inspect, lb))
self.assertTrue(np.allclose(ub_inspect, ub))
c, dc_dq = constraint.eval(0., kinsol)
# Because the floating base position is all 0's and the arm
# origin is not offset from the floating base, the world
# point should equal the body point.
c_expected = body_pt
dc_dq_expected = np.array([[1., 0., 0., 0., -1., 0., -1.],
[0., 1., 0., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0.]])
self.assertTrue(np.allclose(c, c_expected))
self.assertTrue(np.allclose(dc_dq, dc_dq_expected))
def testPostureConstraint(self):
q = -0.9
posture_constraint = ik.PostureConstraint(self.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(self.r)
results = ik.InverseKin(self.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(self.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(self.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 testWorldGazeTargetConstraint(self):
model = self.r
body = 2
axis = np.ones([3, 1])
target = np.ones([3, 1])
gaze_origin = np.zeros([3, 1])
cone_threshold = 1e-3
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold)
# Test that construction doesn't fail with a given timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold, tspan)
def testRelativePositionConstraint(self):
model = self.r
pts = np.zeros([3, 1])
lb = -np.ones([3, 1])
ub = np.ones([3, 1])
bodyA_idx = 1
bodyB_idx = 2
translation = np.zeros(3)
quaternion = np.array([1, 0, 0, 0])
bTbp = np.concatenate([translation, quaternion]).reshape(7, 1)
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp)
# Test that construction doesn't fail with a given timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp, tspan)
def testRelativeGazeDirConstraint(self):
model = self.r
bodyA_idx = 1
bodyB_idx = 2
axis = np.ones([3, 1])
dir = np.ones([3, 1])
conethreshold = 1e-3
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir,
conethreshold)
# Test that construction doesn't fail with a given timespan.
ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir,
conethreshold, tspan)
def testMinDistanceConstraint(self):
model = self.r
min_distance = 1e-2
active_bodies_idx = list()
active_group_name = set()
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name)
# Test that construction doesn't fail with a given timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name, tspan)
from pydrake.common import FindResourceOrThrow
from pydrake.attic.multibody.rigid_body_tree import (
AddModelInstanceFromUrdfFile,
FloatingBaseType,
RigidBodyTree,
RigidBodyFrame,
)
from pydrake.systems.framework import BasicVector
from pydrake.systems.sensors import CameraInfo
from pydrake.attic.systems.sensors import 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)
def setUp(self):
self.r = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
def test_rigid_body_tree_collision_api(self):
# This test creates a simple RBT with two spheres in simple
# collision and verifies collision-detection APIs.
rbt = RigidBodyTree()
world_body = rbt.world()
# Both bodies are unit-diameter spheres
# spaced 0.5m from each other along the x axis.
for k in range(2):
body = RigidBody()
body.set_name("body_{}".format(k))
joint = PrismaticJoint("x", np.eye(4),
np.array([1.0, 0., 0.]))
body.add_joint(world_body, joint)
body.set_spatial_inertia(np.eye(6))
rbt.add_rigid_body(body)
sphere_element = shapes.Sphere(0.5)
sphere_collision_element = CollisionElement(
sphere_element, np.eye(4))
sphere_collision_element.set_body(body)
rbt.addCollisionElement(sphere_collision_element, body, "default")
rbt.compile()
self.assertTrue(rbt.initialized())
self.assertEqual(rbt.get_num_positions(), 2)
q0 = np.array([-0.25, 0.25])
kinsol = rbt.doKinematics(q0)
# One point in each region of overlap:
# Sphere one is (), sphere two is []
# p0 ( p2 [ p3 ) p4 ] p5
points = np.array([[-1., -0.6, 0., 0.6, 1.],
[0., 0., 0., 0., 0],
[0., 0., 0., 0., 0]])
n_pts = points.shape[1]
phi, normal, x, body_x, body_idx = rbt.collisionDetectFromPoints(
cache=kinsol, points=points, use_margins=False)
# Check phi, but leave the other fields as API checks.
self.assertTrue(
np.allclose(phi, np.array([0.25, -0.15, -0.25, -0.15, 0.25])))
self.assertEqual(phi.shape[0], n_pts)
self.assertEqual(normal.shape, (3, n_pts))
self.assertEqual(x.shape, (3, n_pts))
self.assertEqual(normal.shape, (3, n_pts))
point_pairs = rbt.ComputeMaximumDepthCollisionPoints(
cache=kinsol, use_margins=False,
throw_if_missing_gradient=True)
self.assertEqual(len(point_pairs), 1)
pp = point_pairs[0]
self.assertEqual(rbt.FindCollisionElement(pp.idA), pp.elementA)
self.assertEqual(rbt.FindCollisionElement(pp.idB), pp.elementB)
possible_points = [np.array([0.5, 0., 0.]), np.array([-0.5, 0.0, 0.0])]
for pt in [pp.ptA, pp.ptB]:
self.assertTrue(
np.allclose(pt, np.array([0.5, 0., 0.])) or
np.allclose(pt, np.array([-0.5, 0., 0.])))
self.assertTrue(
np.allclose(np.abs(pp.normal), np.array([1., 0., 0.])))
self.assertEqual(pp.distance, -0.5)
class TestRBTIK(unittest.TestCase):
def setUp(self):
self.r = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
def testSingleTimeKinematicConstraint(self):
# Demonstrate that a subclass of SingleTimeKinematicConstraint
# inherits functional `eval` and `bounds` methods.
q = np.zeros(self.r.get_num_positions())
kinsol = self.r.doKinematics(q)
ub = np.array([0.0, 0.0, -0.45])
lb = np.array([0.0, 0.0, -0.55])
# This point, at the zero configuration, is
# at [0, 0, -1] in world frame as well.
body_pt = np.array([0., 0., -1])
constraint = ik.WorldPositionConstraint(
self.r, self.r.FindBodyIndex("arm"),
body_pt, lb, ub)
self.assertIsInstance(constraint, ik.SingleTimeKinematicConstraint)
lb_inspect, ub_inspect = constraint.bounds(t=0.)
self.assertTrue(np.allclose(lb_inspect, lb))
self.assertTrue(np.allclose(ub_inspect, ub))
c, dc_dq = constraint.eval(0., kinsol)
# Because the floating base position is all 0's and the arm
# origin is not offset from the floating base, the world
# point should equal the body point.
c_expected = body_pt
dc_dq_expected = np.array([[1., 0., 0., 0., -1., 0., -1.],
[0., 1., 0., 1., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0.]])
self.assertTrue(np.allclose(c, c_expected))
self.assertTrue(np.allclose(dc_dq, dc_dq_expected))
def testPostureConstraint(self):
q = -0.9
posture_constraint = ik.PostureConstraint(self.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(self.r)
results = ik.InverseKin(
self.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(self.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(self.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 testWorldGazeTargetConstraint(self):
model = self.r
body = 2
axis = np.ones([3, 1])
target = np.ones([3, 1])
gaze_origin = np.zeros([3, 1])
cone_threshold = 1e-3
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold)
# Test that construction doesn't fail with a given timespan.
ik.WorldGazeTargetConstraint(model, body, axis, target, gaze_origin,
cone_threshold, tspan)
def testRelativePositionConstraint(self):
model = self.r
pts = np.zeros([3, 1])
lb = -np.ones([3, 1])
ub = np.ones([3, 1])
bodyA_idx = 1
bodyB_idx = 2
translation = np.zeros(3)
quaternion = np.array([1, 0, 0, 0])
bTbp = np.concatenate([translation, quaternion]).reshape(7, 1)
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp)
# Test that construction doesn't fail with a given timespan.
ik.RelativePositionConstraint(model, pts, lb, ub, bodyA_idx, bodyB_idx,
bTbp, tspan)
def testRelativeGazeDirConstraint(self):
model = self.r
bodyA_idx = 1
bodyB_idx = 2
axis = np.ones([3, 1])
dir = np.ones([3, 1])
conethreshold = 1e-3
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir,
conethreshold)
# Test that construction doesn't fail with a given timespan.
ik.RelativeGazeDirConstraint(model, bodyA_idx, bodyB_idx, axis, dir,
conethreshold, tspan)
def testMinDistanceConstraint(self):
model = self.r
min_distance = 1e-2
active_bodies_idx = list()
active_group_name = set()
tspan = np.array([0., 1.])
# Test that construction doesn't fail with the default timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name)
# Test that construction doesn't fail with a given timespan.
ik.MinDistanceConstraint(model, min_distance, active_bodies_idx,
active_group_name, tspan)
