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)
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)
