def test_linear_algebra(self):
a_scalar = AD(1, [1., 0])
b_scalar = AD(2, [0, 1.])
A = np.array([[a_scalar, a_scalar]])
B = np.array([[b_scalar, b_scalar]]).T
C = np.dot(A, B)
npc.assert_equal(C, [[AD(4, [4., 2])]])
# `matmul` not supported for `dtype=object` (#11332). `np.dot` should
# be used instead.
with self.assertRaises(TypeError):
C2 = np.matmul(A, B)
# Type mixing
Bf = np.array([[2., 2]]).T
C2 = np.dot(A, Bf) # Leverages implicit casting.
npc.assert_equal(C2, [[AD(4, [4., 0])]])
# Other methods.
X = np.array([[a_scalar, b_scalar], [b_scalar, a_scalar]])
npc.assert_equal(np.trace(X), AD(2, [2., 0]))
# `inv` is a ufunc that we must implement, if possible. However, given
# that this is currently `dtype=object`, it would be extremely unwise
# to do so. See #8116 for alternative.
with self.assertRaises(TypeError):
Y = np.linalg.inv(X)
# Use workaround for inverse. For now, just check values.
X_float = npc.to_float(X)
Xinv_float = np.linalg.inv(X_float)
Xinv = drake_math.inv(X)
np.testing.assert_equal(npc.to_float(Xinv), Xinv_float)
def test_to_float(self):
# Scalar.
xi = 1
xf = npc.to_float(xi)
self.assertEqual(xf.dtype, float)
self.assertEqual(xi, xf)
# Array.
Xi = np.array([1, 2, 3], np.int)
Xf = npc.to_float(Xi)
self.assertEqual(Xf.dtype, float)
np.testing.assert_equal(Xi, Xf)
# Custom.
a = Custom("1.")
b = Custom("2.")
self.assertEqual(npc.to_float(a), 1.)
A = np.array([a, b])
np.testing.assert_equal(npc.to_float(A), [1., 2.])
# - Convenience float comparators.
npc.assert_float_equal(a, 1.)
with self.assertRaises(AssertionError):
npc.assert_float_equal(a, 2.)
npc.assert_float_not_equal(a, 2.)
with self.assertRaises(AssertionError):
npc.assert_float_not_equal(a, 1.)
npc.assert_float_equal(A, [1., 2.])
# Check nearness.
Af_delta = npc.to_float(A) + 5e-16
npc.assert_float_not_equal(A, Af_delta)
npc.assert_float_allclose(A, Af_delta)
def test_linear_algebra(self):
a_scalar = AD(1, [1., 0])
b_scalar = AD(2, [0, 1.])
A = np.array([[a_scalar, a_scalar]])
B = np.array([[b_scalar, b_scalar]]).T
C = np.dot(A, B)
npc.assert_equal(C, [[AD(4, [4., 2])]])
# `matmul` not supported for `dtype=object` (#11332). `np.dot` should
# be used instead.
with self.assertRaises(TypeError):
C2 = np.matmul(A, B)
# Type mixing
Bf = np.array([[2., 2]]).T
C2 = np.dot(A, Bf) # Leverages implicit casting.
npc.assert_equal(C2, [[AD(4, [4., 0])]])
# Other methods.
X = np.array([[a_scalar, b_scalar], [b_scalar, a_scalar]])
npc.assert_equal(np.trace(X), AD(2, [2., 0]))
# `inv` is a ufunc that we must implement, if possible. However, given
# that this is currently `dtype=object`, it would be extremely unwise
# to do so. See #8116 for alternative.
with self.assertRaises(TypeError):
Y = np.linalg.inv(X)
# Use workaround for inverse. For now, just check values.
X_float = npc.to_float(X)
Xinv_float = np.linalg.inv(X_float)
Xinv = drake_math.inv(X)
np.testing.assert_equal(npc.to_float(Xinv), Xinv_float)
def test_to_float(self):
# Scalar.
xi = 1
xf = numpy_compare.to_float(xi)
self.assertEqual(xf.dtype, float)
self.assertEqual(xi, xf)
# Array.
Xi = np.array([1, 2, 3], np.int)
Xf = numpy_compare.to_float(Xi)
self.assertEqual(Xf.dtype, float)
np.testing.assert_equal(Xi, Xf)
# Custom.
a = Custom("1.")
b = Custom("2.")
self.assertEqual(numpy_compare.to_float(a), 1.)
A = np.array([a, b])
np.testing.assert_equal(numpy_compare.to_float(A), [1., 2.])
# - Convenience float comparators.
numpy_compare.assert_float_equal(a, 1.)
with self.assertRaises(AssertionError):
numpy_compare.assert_float_equal(a, 2.)
numpy_compare.assert_float_not_equal(a, 2.)
with self.assertRaises(AssertionError):
numpy_compare.assert_float_not_equal(a, 1.)
numpy_compare.assert_float_equal(A, [1., 2.])
# Check nearness.
Af_delta = numpy_compare.to_float(A) + 5e-16
numpy_compare.assert_float_not_equal(A, Af_delta)
numpy_compare.assert_float_allclose(A, Af_delta)
def check_logical(func, a, b, expected):
# Checks logical operations, with broadcasting, checking that `a` and `b`
# (of type `T`) have compatible logical operators when the left or right
# operands are `float`s. Specifically, tests:
# - f(T, T)
# - f(T, float)
# - f(float, T)
numpy_compare.assert_equal(func(a, b), expected)
af = numpy_compare.to_float(a)
bf = numpy_compare.to_float(b)
numpy_compare.assert_equal(func(a, bf), expected)
numpy_compare.assert_equal(func(af, b), expected)
def check_logical(func, a, b, expected):
# Checks logical operations, with broadcasting, checking that `a` and `b`
# (of type `T`) have compatible logical operators when the left or right
# operands are `float`s. Specifically, tests:
# - f(T, T)
# - f(T, float)
# - f(float, T)
npc.assert_equal(func(a, b), expected)
af = npc.to_float(a)
bf = npc.to_float(b)
npc.assert_equal(func(a, bf), expected)
npc.assert_equal(func(af, b), expected)
def test_angle_axis(self, T):
AngleAxis = mut.AngleAxis_[T]
value_identity = AngleAxis.Identity()
self.assertEqual(numpy_compare.resolve_type(value_identity.angle()), T)
numpy_compare.assert_float_equal(value_identity.angle(), 0.)
numpy_compare.assert_float_equal(value_identity.axis(), [1., 0, 0])
# Construct with rotation matrix.
R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
value = AngleAxis(rotation=R)
numpy_compare.assert_float_allclose(value.rotation(), R)
numpy_compare.assert_float_allclose(copy.copy(value).rotation(), R)
numpy_compare.assert_float_allclose(value.inverse().rotation(), R.T)
numpy_compare.assert_float_allclose(
value.multiply(value.inverse()).rotation(), np.eye(3))
if six.PY3:
numpy_compare.assert_float_allclose(
eval("value @ value.inverse()").rotation(), np.eye(3))
value.set_rotation(np.eye(3))
numpy_compare.assert_float_equal(value.rotation(), np.eye(3))
# Construct with quaternion.
Quaternion = mut.Quaternion_[T]
q = Quaternion(R)
value = AngleAxis(quaternion=q)
numpy_compare.assert_float_allclose(value.quaternion().wxyz(),
numpy_compare.to_float(q.wxyz()))
value.set_quaternion(Quaternion.Identity())
numpy_compare.assert_float_equal(value.quaternion().wxyz(),
[1., 0, 0, 0])
# Test setters.
value = AngleAxis(value_identity)
value.set_angle(np.pi / 4)
v = normalize(np.array([0.1, 0.2, 0.3]))
if T != Expression:
with self.assertRaises(RuntimeError):
value.set_axis([0.1, 0.2, 0.3])
value.set_axis(v)
numpy_compare.assert_float_equal(value.angle(), np.pi / 4)
numpy_compare.assert_float_equal(value.axis(), v)
# Cast.
self.check_cast(mut.AngleAxis_, T)
# Test symmetry based on accessors.
# N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal
# angles and axes to a half-plane.
angle = np.pi / 6
axis = normalize([0.1, 0.2, 0.3])
value = AngleAxis(angle=angle, axis=axis)
value_sym = AngleAxis(angle=-angle, axis=-axis)
numpy_compare.assert_equal(value.rotation(), value_sym.rotation())
numpy_compare.assert_equal(value.angle(), -value_sym.angle())
numpy_compare.assert_equal(value.axis(), -value_sym.axis())
def get_vector(value):
return np.hstack((value.angle(), value.axis()))
assert_pickle(self, value, get_vector, T=T)
def check_spatial_vector(self, cls, coeffs_name, rotation_name,
translation_name):
vec = cls()
# - Accessors.
self.assertTrue(isinstance(vec.rotational(), np.ndarray))
self.assertTrue(isinstance(vec.translational(), np.ndarray))
self.assertEqual(vec.rotational().shape, (3, ))
self.assertEqual(vec.translational().shape, (3, ))
# - Fully-parameterized constructor.
rotation_expected = [0.1, 0.3, 0.5]
translation_expected = [0., 1., 2.]
vec_args = {
rotation_name: rotation_expected,
translation_name: translation_expected,
}
vec1 = cls(**vec_args)
numpy_compare.assert_float_equal(vec1.rotational(), rotation_expected)
numpy_compare.assert_float_equal(vec1.translational(),
translation_expected)
vec_zero = cls()
vec_zero.SetZero()
vec_zero_to_float = numpy_compare.to_float(vec_zero.get_coeffs())
numpy_compare.assert_float_equal(cls.Zero().get_coeffs(),
vec_zero_to_float)
coeffs_expected = np.hstack((rotation_expected, translation_expected))
coeffs_args = {coeffs_name: coeffs_expected}
numpy_compare.assert_float_equal(
cls(**coeffs_args).get_coeffs(), coeffs_expected)
def test_rotation_matrix(self, T):
# - Constructors.
RotationMatrix = mut.RotationMatrix_[T]
AngleAxis = AngleAxis_[T]
Quaternion = Quaternion_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
R = RotationMatrix()
numpy_compare.assert_float_equal(
RotationMatrix(other=R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
numpy_compare.assert_float_equal(copy.copy(R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(
RotationMatrix.Identity().matrix(), np.eye(3))
R = RotationMatrix(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(quaternion=Quaternion.Identity())
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(theta_lambda=AngleAxis(angle=0, axis=[0, 0, 1]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(rpy=RollPitchYaw(rpy=[0, 0, 0]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# One axis RotationMatrices
R = RotationMatrix.MakeXRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeYRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeZRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# TODO(eric.cousineau): #11575, remove the conditional.
if T == float:
numpy_compare.assert_float_equal(R.row(index=0), [1., 0., 0.])
numpy_compare.assert_float_equal(R.col(index=0), [1., 0., 0.])
R.set(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# - Cast.
self.check_cast(mut.RotationMatrix_, T)
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
numpy_compare.assert_float_equal(
q.wxyz(), numpy_compare.to_float(q_R.wxyz()))
# - Inverse, transpose, projection
R_I = R.inverse().multiply(R)
numpy_compare.assert_float_equal(R_I.matrix(), np.eye(3))
R_T = R.transpose().multiply(R)
numpy_compare.assert_float_equal(R_T.matrix(), np.eye(3))
R_P = RotationMatrix.ProjectToRotationMatrix(M=2*np.eye(3))
numpy_compare.assert_float_equal(R_P.matrix(), np.eye(3))
# Matrix checks
numpy_compare.assert_equal(R.IsValid(), True)
R = RotationMatrix()
numpy_compare.assert_equal(R.IsExactlyIdentity(), True)
numpy_compare.assert_equal(R.IsIdentityToInternalTolerance(), True)
if six.PY3:
numpy_compare.assert_float_equal(
eval("R.inverse() @ R").matrix(), np.eye(3))
def check_angle_axis(self, T):
AngleAxis = mut.AngleAxis_[T]
value_identity = AngleAxis.Identity()
self.assertEqual(npc.resolve_type(value_identity.angle()), T)
npc.assert_float_equal(value_identity.angle(), 0.)
npc.assert_float_equal(value_identity.axis(), [1., 0, 0])
# Construct with rotation matrix.
R = np.array([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
value = AngleAxis(rotation=R)
npc.assert_float_allclose(value.rotation(), R)
npc.assert_float_allclose(copy.copy(value).rotation(), R)
npc.assert_float_allclose(value.inverse().rotation(), R.T)
npc.assert_float_allclose(
value.multiply(value.inverse()).rotation(), np.eye(3))
if six.PY3:
npc.assert_float_allclose(
eval("value @ value.inverse()").rotation(), np.eye(3))
value.set_rotation(np.eye(3))
npc.assert_float_equal(value.rotation(), np.eye(3))
# Construct with quaternion.
Quaternion = mut.Quaternion_[T]
q = Quaternion(R)
value = AngleAxis(quaternion=q)
npc.assert_float_allclose(
value.quaternion().wxyz(), npc.to_float(q.wxyz()))
value.set_quaternion(Quaternion.Identity())
npc.assert_float_equal(value.quaternion().wxyz(), [1., 0, 0, 0])
# Test setters.
value = AngleAxis(value_identity)
value.set_angle(np.pi / 4)
v = normalize(np.array([0.1, 0.2, 0.3]))
if T != Expression:
with self.assertRaises(RuntimeError):
value.set_axis([0.1, 0.2, 0.3])
value.set_axis(v)
npc.assert_float_equal(value.angle(), np.pi / 4)
npc.assert_float_equal(value.axis(), v)
# Test symmetry based on accessors.
# N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal
# angles and axes to a half-plane.
angle = np.pi / 6
axis = normalize([0.1, 0.2, 0.3])
value = AngleAxis(angle=angle, axis=axis)
value_sym = AngleAxis(angle=-angle, axis=-axis)
npc.assert_equal(value.rotation(), value_sym.rotation())
npc.assert_equal(value.angle(), -value_sym.angle())
npc.assert_equal(value.axis(), -value_sym.axis())
def test_roll_pitch_yaw(self, T):
# - Constructors.
RollPitchYaw = mut.RollPitchYaw_[T]
RotationMatrix = mut.RotationMatrix_[T]
Quaternion = Quaternion_[T]
rpy = RollPitchYaw(rpy=[0, 0, 0])
numpy_compare.assert_float_equal(
RollPitchYaw(other=rpy).vector(), [0., 0., 0.])
numpy_compare.assert_float_equal(rpy.vector(), [0., 0., 0.])
rpy = RollPitchYaw(roll=0, pitch=0, yaw=0)
numpy_compare.assert_float_equal(
[rpy.roll_angle(),
rpy.pitch_angle(),
rpy.yaw_angle()], [0., 0., 0.])
rpy = RollPitchYaw(R=RotationMatrix())
numpy_compare.assert_float_equal(rpy.vector(), [0., 0., 0.])
rpy = RollPitchYaw(matrix=np.eye(3))
numpy_compare.assert_float_equal(rpy.vector(), [0., 0., 0.])
q_I = Quaternion()
rpy_q_I = RollPitchYaw(quaternion=q_I)
numpy_compare.assert_float_equal(rpy_q_I.vector(), [0., 0., 0.])
# - Additional properties.
numpy_compare.assert_float_equal(rpy.ToQuaternion().wxyz(),
numpy_compare.to_float(q_I.wxyz()))
R = rpy.ToRotationMatrix().matrix()
numpy_compare.assert_float_equal(R, np.eye(3))
# - Converting changes in orientation
numpy_compare.assert_float_equal(
rpy.CalcRotationMatrixDt(rpyDt=[0, 0, 0]), np.zeros((3, 3)))
numpy_compare.assert_float_equal(
rpy.CalcAngularVelocityInParentFromRpyDt(rpyDt=[0, 0, 0]),
[0., 0., 0.])
numpy_compare.assert_float_equal(
rpy.CalcAngularVelocityInChildFromRpyDt(rpyDt=[0, 0, 0]),
[0., 0., 0.])
numpy_compare.assert_float_equal(
rpy.CalcRpyDtFromAngularVelocityInParent(w_AD_A=[0, 0, 0]),
[0., 0., 0.])
numpy_compare.assert_float_equal(
rpy.CalcRpyDDtFromRpyDtAndAngularAccelInParent(
rpyDt=[0, 0, 0], alpha_AD_A=[0, 0, 0]), [0., 0., 0.])
numpy_compare.assert_float_equal(
rpy.CalcRpyDDtFromAngularAccelInChild(rpyDt=[0, 0, 0],
alpha_AD_D=[0, 0, 0]),
[0., 0., 0.])
# Test pickling.
assert_pickle(self, rpy, RollPitchYaw.vector, T=T)
def test_rotation_matrix(self, T):
# - Constructors.
RotationMatrix = mut.RotationMatrix_[T]
AngleAxis = AngleAxis_[T]
Quaternion = Quaternion_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
R = RotationMatrix()
numpy_compare.assert_float_equal(
RotationMatrix(other=R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
numpy_compare.assert_float_equal(copy.copy(R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(RotationMatrix.Identity().matrix(),
np.eye(3))
R = RotationMatrix(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(quaternion=Quaternion.Identity())
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(theta_lambda=AngleAxis(angle=0, axis=[0, 0, 1]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(rpy=RollPitchYaw(rpy=[0, 0, 0]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# One axis RotationMatrices
R = RotationMatrix.MakeXRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeYRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeZRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# TODO(eric.cousineau): #11575, remove the conditional.
if T == float:
numpy_compare.assert_float_equal(R.row(index=0), [1., 0., 0.])
numpy_compare.assert_float_equal(R.col(index=0), [1., 0., 0.])
R.set(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# - Cast.
self.check_cast(mut.RotationMatrix_, T)
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
numpy_compare.assert_float_equal(q.wxyz(),
numpy_compare.to_float(q_R.wxyz()))
# - Conversion to AngleAxis
angle_axis = R.ToAngleAxis()
self.assertIsInstance(angle_axis, AngleAxis)
R_AngleAxis = RotationMatrix(angle_axis)
R_I = R.inverse().multiply(R_AngleAxis)
numpy_compare.assert_equal(R_I.IsIdentityToInternalTolerance(), True)
# - Inverse, transpose, projection
R_I = R.inverse().multiply(R)
numpy_compare.assert_float_equal(R_I.matrix(), np.eye(3))
numpy_compare.assert_float_equal((R.inverse() @ R).matrix(), np.eye(3))
R_T = R.transpose().multiply(R)
numpy_compare.assert_float_equal(R_T.matrix(), np.eye(3))
R_P = RotationMatrix.ProjectToRotationMatrix(M=2 * np.eye(3))
numpy_compare.assert_float_equal(R_P.matrix(), np.eye(3))
# - Multiplication.
R_AB = RotationMatrix([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
v_B = [10, 20, 30]
v_A = [20., -10., 30]
numpy_compare.assert_float_equal(R_AB.multiply(v_B=v_B), v_A)
# N.B. Remember that this takes ndarray[3, n], NOT ndarray[n, 3]!
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(R_AB.multiply(v_B=vlist_B), vlist_A)
# Matrix checks
numpy_compare.assert_equal(R.IsValid(), True)
R = RotationMatrix()
numpy_compare.assert_equal(R.IsExactlyIdentity(), True)
numpy_compare.assert_equal(R.IsIdentityToInternalTolerance(), True)
# Test pickling.
assert_pickle(self, R_AB, RotationMatrix.matrix, T=T)
def check_spatial_vector(self, T, cls, coeffs_name, rotation_name,
translation_name):
vec = cls()
# - Accessors.
if T == Expression:
# TODO(eric.cousineau): Teach `numpy_compare` to handle NaN in
# symbolic expressions, without having to evaluate the expressions.
self.assertEqual(vec.rotational().shape, (3, ))
self.assertEqual(vec.translational().shape, (3, ))
else:
numpy_compare.assert_float_equal(vec.rotational(),
np.full(3, np.nan))
numpy_compare.assert_float_equal(vec.translational(),
np.full(3, np.nan))
# - Fully-parameterized constructor.
rotation_expected = [0.1, 0.3, 0.5]
translation_expected = [0., 1., 2.]
vec_args = {
rotation_name: rotation_expected,
translation_name: translation_expected,
}
vec1 = cls(**vec_args)
numpy_compare.assert_float_equal(vec1.rotational(), rotation_expected)
numpy_compare.assert_float_equal(vec1.translational(),
translation_expected)
vec_zero = cls()
vec_zero.SetZero()
vec_zero_to_float = numpy_compare.to_float(vec_zero.get_coeffs())
numpy_compare.assert_float_equal(cls.Zero().get_coeffs(),
vec_zero_to_float)
coeffs_expected = np.hstack((rotation_expected, translation_expected))
coeffs_args = {coeffs_name: coeffs_expected}
numpy_compare.assert_float_equal(
cls(**coeffs_args).get_coeffs(), coeffs_expected)
# Test operators.
numpy_compare.assert_float_equal((-vec1).get_coeffs(),
-coeffs_expected)
new = copy.copy(vec1)
# - Ensure in-place ops do not return a new object.
pre_inplace = new
new += vec1
self.assertIs(pre_inplace, new)
numpy_compare.assert_float_equal(new.get_coeffs(), 2 * coeffs_expected)
numpy_compare.assert_float_equal((vec1 + vec1).get_coeffs(),
2 * coeffs_expected)
new = copy.copy(vec1)
pre_inplace = new
new -= vec1
self.assertIs(pre_inplace, new)
numpy_compare.assert_float_equal(new.get_coeffs(), np.zeros(6))
numpy_compare.assert_float_equal((vec1 - vec1).get_coeffs(),
np.zeros(6))
new = copy.copy(vec1)
pre_inplace = new
new *= T(2)
self.assertIs(pre_inplace, new)
numpy_compare.assert_float_equal(new.get_coeffs(), 2 * coeffs_expected)
numpy_compare.assert_float_equal((vec1 * T(2)).get_coeffs(),
2 * coeffs_expected)
numpy_compare.assert_float_equal((T(2) * vec1).get_coeffs(),
2 * coeffs_expected)
R = RotationMatrix_[T]()
numpy_compare.assert_float_equal((vec1.Rotate(R_FE=R)).get_coeffs(),
coeffs_expected)
# Test pickling.
assert_pickle(self, vec1, cls.get_coeffs, T=T)
def test_rotation_matrix(self, T):
# - Constructors.
RotationMatrix = mut.RotationMatrix_[T]
AngleAxis = AngleAxis_[T]
Quaternion = Quaternion_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
R = RotationMatrix()
numpy_compare.assert_float_equal(
RotationMatrix(other=R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
numpy_compare.assert_float_equal(copy.copy(R).matrix(), np.eye(3))
numpy_compare.assert_float_equal(
RotationMatrix.Identity().matrix(), np.eye(3))
R = RotationMatrix(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(quaternion=Quaternion.Identity())
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(theta_lambda=AngleAxis(angle=0, axis=[0, 0, 1]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix(rpy=RollPitchYaw(rpy=[0, 0, 0]))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# One axis RotationMatrices
R = RotationMatrix.MakeXRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeYRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
R = RotationMatrix.MakeZRotation(theta=0)
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# TODO(eric.cousineau): #11575, remove the conditional.
if T == float:
numpy_compare.assert_float_equal(R.row(index=0), [1., 0., 0.])
numpy_compare.assert_float_equal(R.col(index=0), [1., 0., 0.])
R = RotationMatrix.MakeFromOneVector(b_A=[1, 0, 0], axis_index=0)
numpy_compare.assert_equal(R.IsValid(), True)
R.set(R=np.eye(3))
numpy_compare.assert_float_equal(R.matrix(), np.eye(3))
# - Cast.
self.check_cast(mut.RotationMatrix_, T)
# - Nontrivial quaternion.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
R = RotationMatrix(quaternion=q)
q_R = R.ToQuaternion()
numpy_compare.assert_float_equal(
q.wxyz(), numpy_compare.to_float(q_R.wxyz()))
# - Conversion to AngleAxis
angle_axis = R.ToAngleAxis()
self.assertIsInstance(angle_axis, AngleAxis)
R_AngleAxis = RotationMatrix(angle_axis)
R_I = R.inverse().multiply(R_AngleAxis)
numpy_compare.assert_equal(R_I.IsNearlyIdentity(), True)
numpy_compare.assert_equal(R_I.IsNearlyIdentity(2E-15), True)
R_I = R.InvertAndCompose(other=R_AngleAxis)
numpy_compare.assert_equal(R_I.IsNearlyIdentity(2E-15), True)
# - Inverse, transpose, projection
R_I = R.inverse().multiply(R)
numpy_compare.assert_float_equal(R_I.matrix(), np.eye(3))
numpy_compare.assert_float_equal((R.inverse() @ R).matrix(), np.eye(3))
R_T = R.transpose().multiply(R)
numpy_compare.assert_float_equal(R_T.matrix(), np.eye(3))
R_P = RotationMatrix.ProjectToRotationMatrix(M=2*np.eye(3))
numpy_compare.assert_float_equal(R_P.matrix(), np.eye(3))
# - Multiplication.
R_AB = RotationMatrix([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
v_B = [10, 20, 30]
v_A = [20., -10., 30]
numpy_compare.assert_float_equal(R_AB.multiply(v_B=v_B), v_A)
# N.B. Remember that this takes ndarray[3, n], NOT ndarray[n, 3]!
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(R_AB.multiply(v_B=vlist_B), vlist_A)
# - Test shaping (#13885).
v = np.array([0., 0., 0.])
vs = np.array([[1., 2., 3.], [4., 5., 6.]]).T
self.assertEqual((R_AB @ v).shape, (3,))
self.assertEqual((R_AB @ v.reshape((3, 1))).shape, (3, 1))
self.assertEqual((R_AB @ vs).shape, (3, 2))
# Matrix checks
numpy_compare.assert_equal(R.IsValid(), True)
R = RotationMatrix()
numpy_compare.assert_equal(R.IsExactlyIdentity(), True)
numpy_compare.assert_equal(R.IsNearlyIdentity(0.0), True)
numpy_compare.assert_equal(R.IsNearlyIdentity(tolerance=1E-15), True)
# - Repr.
z = repr(T(0.0))
i = repr(T(1.0))
type_suffix = {
float: "",
AutoDiffXd: "_[AutoDiffXd]",
Expression: "_[Expression]",
}[T]
self.assertEqual(repr(RotationMatrix()), textwrap.dedent(f"""\
RotationMatrix{type_suffix}([
[{i}, {z}, {z}],
[{z}, {i}, {z}],
[{z}, {z}, {i}],
])"""))
if T == float:
# TODO(jwnimmer-tri) Once AutoDiffXd and Expression implement an
# eval-able repr, then we can test more than just T=float here.
roundtrip = eval(repr(RotationMatrix()))
self.assertTrue(roundtrip.IsExactlyIdentity())
# Test pickling.
assert_pickle(self, R_AB, RotationMatrix.matrix, T=T)
def test_multibody_dynamics(self, T):
MultibodyPlant = MultibodyPlant_[T]
MultibodyForces = MultibodyForces_[T]
SpatialForce = SpatialForce_[T]
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
Parser(plant_f).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant_f.mutable_gravity_field().set_gravity_vector(gravity_vector)
plant_f.Finalize()
plant = to_type(plant_f, T)
context = plant.CreateDefaultContext()
numpy_compare.assert_float_equal(
plant.gravity_field().gravity_vector(), gravity_vector)
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
potential_energy = plant.CalcPotentialEnergy(context)
numpy_compare.assert_float_not_equal(potential_energy, 0.)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
numpy_compare.assert_float_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
# N.B. Cannot use `ndarray[object]` to reference existing C arrays
# (#8116).
if T == float:
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(
link2.GetForceInWorld(context, forces), SpatialForce)
forces.SetZero()
F_expected = np.array([1., 2., 3., 4., 5., 6.])
link2.AddInForceInWorld(
context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
coeff = numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs())
numpy_compare.assert_float_equal(coeff, F_expected)
link2.AddInForce(
context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(), forces=forces)
# Also check accumulation.
np.testing.assert_equal(numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs()),
2 * F_expected)
def test_multibody_tree_kinematics(self, T):
RigidTransform = RigidTransform_[T]
SpatialVelocity = SpatialVelocity_[T]
plant_f = MultibodyPlant_[float]()
file_name = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
Parser(plant_f).AddModelFromFile(file_name)
plant_f.Finalize()
plant = to_type(plant_f, T)
context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
base = plant.GetBodyByName("base")
base_frame = plant.GetFrameByName("base")
X_WL = plant.CalcRelativeTransform(
context, frame_A=world_frame, frame_B=base_frame)
self.assertIsInstance(X_WL, RigidTransform)
p_AQi = plant.CalcPointsPositions(
context=context, frame_B=base_frame,
p_BQi=np.array([[0, 1, 2], [10, 11, 12]]).T,
frame_A=world_frame).T
self.assertTupleEqual(p_AQi.shape, (2, 3))
Jv_WL = plant.CalcFrameGeometricJacobianExpressedInWorld(
context=context, frame_B=base_frame,
p_BoFo_B=[0, 0, 0])
self.assertTupleEqual(Jv_WL.shape, (6, plant.num_velocities()))
nq = plant.num_positions()
nv = plant.num_velocities()
wrt_list = [
(JacobianWrtVariable.kQDot, nq),
(JacobianWrtVariable.kV, nv),
]
for wrt, nw in wrt_list:
Jw_ABp_E = plant.CalcJacobianSpatialVelocity(
context=context, with_respect_to=wrt, frame_B=base_frame,
p_BP=np.zeros(3), frame_A=world_frame,
frame_E=world_frame)
self.assert_sane(Jw_ABp_E)
self.assertEqual(Jw_ABp_E.shape, (6, nw))
# Compute body pose.
X_WBase = plant.EvalBodyPoseInWorld(context, base)
self.assertIsInstance(X_WBase, RigidTransform)
# Set pose for the base.
X_WB_desired = RigidTransform.Identity()
X_WB = plant.CalcRelativeTransform(context, world_frame, base_frame)
plant.SetFreeBodyPose(
context=context, body=base, X_WB=X_WB_desired)
numpy_compare.assert_float_equal(
X_WB.matrix(), numpy_compare.to_float(X_WB_desired.matrix()))
# Set a spatial velocity for the base.
v_WB = SpatialVelocity(w=[1, 2, 3], v=[4, 5, 6])
plant.SetFreeBodySpatialVelocity(
context=context, body=base, V_WB=v_WB)
v_base = plant.EvalBodySpatialVelocityInWorld(context, base)
numpy_compare.assert_float_equal(
v_base.rotational(), numpy_compare.to_float(v_WB.rotational()))
numpy_compare.assert_float_equal(
v_base.translational(),
numpy_compare.to_float(v_WB.translational()))
# Compute accelerations.
vdot = np.zeros(nv)
A_WB_array = plant.CalcSpatialAccelerationsFromVdot(
context=context, known_vdot=vdot)
self.assertEqual(len(A_WB_array), plant.num_bodies())
def assert_sane(self, x, nonzero=True):
self.assertTrue(np.all(np.isfinite(numpy_compare.to_float(x))))
if nonzero:
numpy_compare.assert_float_not_equal(x, 0.)