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 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 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_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_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_api(self):
# TODO(eric.cousineau): Reduce these tests to only test API, and do
# simple sanity checks on the numbers.
tree = RigidBodyTree(
os.path.join(pydrake.getDrakePath(),
"examples/pendulum/Pendulum.urdf"))
num_q = 7
num_v = 7
self.assertEqual(tree.number_of_positions(), num_q)
self.assertEqual(tree.number_of_velocities(), num_v)
q = np.zeros(num_q)
v = np.zeros(num_v)
# Trivial kinematics.
kinsol = tree.doKinematics(q, v)
p = tree.transformPoints(kinsol, np.zeros(3), 0, 1)
self.assertTrue(np.allclose(p, np.zeros(3)))
# AutoDiff jacobians.
def do_transform(q):
kinsol = tree.doKinematics(q)
point = np.ones(3)
return tree.transformPoints(kinsol, point, 2, 0)
# - Position.
value = do_transform(q)
self.assertTrue(np.allclose(value, np.ones(3)))
# - Gradient.
g = jacobian(do_transform, q)
g_expected = np.array([[[1, 0, 0, 0, 1, -1, 1]],
[[0, 1, 0, -1, 0, 1, 0]],
[[0, 0, 1, 1, -1, 0, -1]]])
self.assertTrue(np.allclose(g, g_expected))
# Relative transform.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.relativeTransform(kinsol, 1, 2)
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
# Do FK and compare pose of 'arm' with expected pose.
q[:] = 0
q[6] = np.pi / 2
kinsol = tree.doKinematics(q)
T = tree.CalcBodyPoseInWorldFrame(kinsol, tree.FindBody("arm"))
T_expected = np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]])
self.assertTrue(np.allclose(T, T_expected))
def test_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_urdf(self):
"""Test that an instance of a URDF model can be loaded into a
RigidBodyTree by passing a complete set of arguments to Drake's URDF
parser.
"""
urdf_file = os.path.join(
getDrakePath(),
"examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf")
with open(urdf_file) as f:
urdf_string = f.read()
base_dir = os.path.dirname(urdf_file)
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot = RigidBodyTree()
AddModelInstanceFromUrdfStringSearchingInRosPackages(
urdf_string, package_map, base_dir, floating_base_type, weld_frame,
robot)
expected_num_bodies = 86
self.assertEqual(robot.get_num_bodies(),
expected_num_bodies,
msg='Incorrect number of bodies: {0} vs. {1}'.format(
robot.get_num_bodies(), expected_num_bodies))
# Check actuators.
actuator = robot.GetActuator("head_pan_motor")
self.assertIsInstance(actuator, RigidBodyActuator)
self.assertEqual(actuator.name, "head_pan_motor")
self.assertIs(actuator.body, robot.FindBody("head_pan_link"))
self.assertEqual(actuator.reduction, 6.0)
self.assertEqual(actuator.effort_limit_min, -2.645)
self.assertEqual(actuator.effort_limit_max, 2.645)
# Check full number of actuators.
self.assertEqual(len(robot.actuators), robot.get_num_actuators())
for actuator in robot.actuators:
self.assertIsInstance(actuator, RigidBodyActuator)
def test_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_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)
from pydrake.multibody.rigid_body_tree import RigidBodyTree, RigidBodyFrame
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# load rigid body tree
tree = RigidBodyTree("/home/dexai/catkin_ws/src/ice_cream_parlor_description/urdf/Dexai_parlor-drake.urdf")
# set robot configuration
q = np.zeros(7) #following comment was syler's:# arm position doesn't matter here so just use zeros
#prep for 3d graphing
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# look up indices of camera frames
cam_idx = tree.FindBody("kinect1_rgb_optical_frame").get_body_index()
depth_idx = tree.FindBody("kinect1_depth_optical_frame").get_body_index()
# do kinematics with iiwa joint configuration
kinsol = tree.doKinematics(q)
# find relative transform between rgb and depth optical frame
T = tree.relativeTransform(kinsol, cam_idx, depth_idx)
T_rgb_d = T
# print T
X = T[:3,3] # extract position from transformation matrix
X_rgb_d = X
print (X)
ax.plot(???)
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 QDotToVelocity and VelocityToQDot methods
q = tree.getZeroConfiguration()
v_real = np.zeros(num_v)
q_ad = np.array(map(AutoDiffXd, q))
v_real_ad = np.array(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 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 that drawKinematicTree runs
tree.drawKinematicTree(
join(os.environ["TEST_TMPDIR"], "test_graph.dot"))
