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 MakeControlledKukaPlan():
kSdfPath = "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
num_q = 7
rigid_body_tree = RigidBodyTree()
AddModelInstancesFromSdfFile(FindResourceOrThrow(kSdfPath),
FloatingBaseType.kFixed, None,
rigid_body_tree)
zero_conf = rigid_body_tree.getZeroConfiguration()
joint_lb = zero_conf - np.ones(num_q) * 0.01
joint_ub = zero_conf + np.ones(num_q) * 0.01
pc1 = PostureConstraint(rigid_body_tree, np.array([0, 0.5]))
joint_idx = np.arange(num_q)
pc1.setJointLimits(joint_idx, joint_lb, joint_ub)
pos_end = np.array([0.6, 0, 0.325])
pos_lb = pos_end - np.ones(3) * 0.005
pos_ub = pos_end + np.ones(3) * 0.005
wpc1 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([1, 3]))
pc2 = PostureConstraint(rigid_body_tree, np.array([4, 5.9]))
pc2.setJointLimits(joint_idx, joint_lb, joint_ub)
wpc2 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([6, 9]))
joint_position_start_idx = rigid_body_tree.FindChildBodyOfJoint(
"iiwa_joint_2").get_position_start_index()
pc3 = PostureConstraint(rigid_body_tree, np.array([6, 8]))
pc3.setJointLimits(np.array([joint_position_start_idx]),
np.ones(1) * 0.7,
np.ones(1) * 0.8)
kTimes = np.array([0.0, 2.0, 5.0, 7.0, 9.0])
q_seed = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
q_norm = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
for i in range(kTimes.size):
q_seed[:, i] = zero_conf + 0.1 * np.ones(
rigid_body_tree.get_num_positions())
q_norm[:, i] = zero_conf
ikoptions = IKoptions(rigid_body_tree)
constraint_array = [pc1, wpc1, pc2, pc3, wpc2]
ik_results = InverseKinPointwise(rigid_body_tree, kTimes, q_seed, q_norm,
constraint_array, ikoptions)
q_sol = ik_results.q_sol
ik_info = ik_results.info
infeasible_constraint = ik_results.infeasible_constraints
info_good = True
for i in range(kTimes.size):
print('IK ik_info[{}] = {}'.format(i, ik_info[i]))
if ik_info[i] != 1:
info_good = False
if not info_good:
raise Exception(
"inverseKinPointwise failed to compute a valid solution.")
knots = np.array(q_sol).transpose()
traj_pp = PiecewisePolynomial.FirstOrderHold(kTimes, knots)
return traj_pp
