def test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x[:2], t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay[:2],
b=self.by[:2],
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
assert_pickle(self, vpoly, lambda S: S.vertices())
v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
# Test creating a vpolytope from a non-minimal set of vertices
# 2D: Random points inside a circle
r = 2.0
n = 400
vertices = np.zeros((2, n + 4))
theta = np.linspace(0, 2 * np.pi, n, endpoint=False)
vertices[0, 0:n] = r * np.cos(theta)
vertices[1, 0:n] = r * np.sin(theta)
vertices[:, n:] = np.array([[r / 2, r / 3, r / 4, r / 5],
[r / 2, r / 3, r / 4, r / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), np.pi * r * r, delta=1e-3)
self.assertEqual(vpoly.vertices().shape[1], n)
# Calculate the length of the path that visits all the vertices
# sequentially.
# If the vertices are in clockwise/counter-clockwise order,
# the length of the path will coincide with the perimeter of a
# circle.
self.assertAlmostEqual(self._calculate_path_length(vpoly.vertices()),
2 * np.pi * r,
delta=1e-3)
# 3D: Random points inside a box
a = 2.0
vertices = np.array(
[[0, a, 0, a, 0, a, 0, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, a, a, 0, 0, a, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, 0, 0, a, a, a, a, a / 2, a / 3, a / 4, a / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), a * a * a)
self.assertEqual(vpoly.vertices().shape[1], 8)
def test_method(self):
DummyC.method = m.TemplateMethod("method", DummyC)
DummyC.method.add_instantiation(int, DummyC.dummy_c)
DummyC.method.add_instantiation(float, DummyC.dummy_d)
self.assertEqual(str(DummyC.method),
"<unbound TemplateMethod DummyC.method>")
self.assertTrue(DummyC.method.is_instantiation(DummyC.dummy_c))
if six.PY2:
self.assertEqual(str(DummyC.method[int]),
"<unbound method DummyC.method[int]>")
else:
self.assertTrue(
str(DummyC.method[int]).startswith(
"<function DummyC.method[int] at "),
str(DummyC.method[int]))
obj = DummyC()
self.assertTrue(
str(obj.method).startswith(
"<bound TemplateMethod DummyC.method of "))
self.assertIn("<bound method DummyC.method[int] of ",
str(obj.method[int]))
self.assertEqual(obj.method[int](), (obj, 3))
self.assertEqual(DummyC.method[int](obj), (obj, 3))
self.assertEqual(obj.method[float](), (obj, 4))
self.assertEqual(DummyC.method[float](obj), (obj, 4))
# Cannot pickle instancemethod's in Python 2.
if not six.PY2:
assert_pickle(self, DummyC.method[int])
def test_scalar_api(self):
a = AD(1, [1., 0])
self.assertEqual(a.value(), 1.)
numpy_compare.assert_equal(a.derivatives(), [1., 0])
self.assertEqual(str(a), "AD{1.0, nderiv=2}")
self.assertEqual(repr(a), "<AutoDiffXd 1.0 nderiv=2>")
numpy_compare.assert_equal(a, a)
# Test construction from `float` and `int`.
numpy_compare.assert_equal(AD(1), AD(1., []))
numpy_compare.assert_equal(AD(1.), AD(1., []))
# Test implicit conversion.
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1), # int
AD(1., []))
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1.), # float
AD(1., []))
# Test multi-element pass-through.
x = np.array([AD(1.), AD(2.), AD(3.)])
numpy_compare.assert_equal(autodiff_vector_pass_through(x), x)
# Ensure fixed-size vectors are correctly converted (#9886).
numpy_compare.assert_equal(autodiff_vector3_pass_through(x), x)
# Ensure we can copy.
numpy_compare.assert_equal(copy.copy(a), a)
numpy_compare.assert_equal(copy.deepcopy(a), a)
# Ensure that we can pickle.
assert_pickle(self, a, lambda x: x)
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 test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x[:2], t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay[:2],
b=self.by[:2],
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
assert_pickle(self, vpoly, lambda S: S.vertices())
v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
def test_bspline_basis(self, T):
BsplineBasis = mut.BsplineBasis_[T]
bspline = BsplineBasis()
self.assertEqual(bspline.order(), 0)
self.assertEqual(BsplineBasis(other=bspline).order(), 0)
bspline = BsplineBasis(order=2, knots=[0, 1, 3, 5])
self.assertEqual(bspline.order(), 2)
bspline = BsplineBasis(order=2, num_basis_functions=3,
type=mut.KnotVectorType.kUniform,
initial_parameter_value=5.,
final_parameter_value=6.)
self.assertEqual(bspline.order(), 2)
self.assertEqual(bspline.degree(), 1)
self.assertEqual(bspline.num_basis_functions(), 3)
numpy_compare.assert_float_equal(bspline.knots(),
[4.5, 5.0, 5.5, 6.0, 6.5])
numpy_compare.assert_float_equal(bspline.initial_parameter_value(), 5.)
numpy_compare.assert_float_equal(bspline.final_parameter_value(), 6.)
self.assertEqual(
bspline.FindContainingInterval(parameter_value=5.2), 1)
self.assertEqual(
bspline.ComputeActiveBasisFunctionIndices(
parameter_interval=[5.2, 5.7]),
[0, 1, 2])
self.assertEqual(
bspline.ComputeActiveBasisFunctionIndices(parameter_value=5.4),
[0, 1])
val = bspline.EvaluateCurve(control_points=[[1, 2], [2, 3], [3, 4]],
parameter_value=5.7)
self.assertEqual(val.shape, (2,))
numpy_compare.assert_float_equal(
bspline.EvaluateBasisFunctionI(i=0, parameter_value=5.7), 0.)
assert_pickle(self, bspline, BsplineBasis.knots, T=T)
def test_camera_info(self):
width = 640
height = 480
fov_y = np.pi / 4
focal_y = height / 2 / np.tan(fov_y / 2)
focal_x = focal_y
center_x = width / 2 - 0.5
center_y = height / 2 - 0.5
intrinsic_matrix = np.array([[focal_x, 0, center_x],
[0, focal_y, center_y], [0, 0, 1]])
infos = [
mut.CameraInfo(width=width, height=height, fov_y=fov_y),
mut.CameraInfo(width=width,
height=height,
focal_x=focal_x,
focal_y=focal_y,
center_x=center_x,
center_y=center_y),
]
for info in infos:
self.assertEqual(info.width(), width)
self.assertEqual(info.height(), height)
self.assertEqual(info.focal_x(), focal_x)
self.assertEqual(info.focal_y(), focal_y)
self.assertEqual(info.center_x(), center_x)
self.assertEqual(info.center_y(), center_y)
self.assertTrue(
(info.intrinsic_matrix() == intrinsic_matrix).all())
assert_pickle(self, info, mut.CameraInfo.intrinsic_matrix)
def test_base(self):
template = m.TemplateBase("BaseTpl")
self.assertEqual(str(template),
"<TemplateBase {}.BaseTpl>".format(_TEST_MODULE))
# Single arguments.
template.add_instantiation(int, 1)
self.assertEqual(template[int], 1)
self.assertEqual(template.get_instantiation(int), (1, (int, )))
self.assertEqual(template.get_param_set(1), {(int, )})
self.assertTrue(template.is_instantiation(1))
self.assertFalse(template.is_instantiation(10))
# Duplicate parameters.
self.assertRaises(RuntimeError,
lambda: template.add_instantiation(int, 4))
# Invalid parameters.
self.assertRaises(RuntimeError, lambda: template[float])
# New instantiation.
template.add_instantiation(float, 2)
self.assertEqual(template[float], 2)
# Default instantiation.
self.assertEqual(template[None], 1)
self.assertEqual(template.get_instantiation(), (1, (int, )))
# Multiple arguments.
template.add_instantiation((int, int), 3)
self.assertEqual(template[int, int], 3)
# Duplicate instantiation.
template.add_instantiation((float, float), 1)
self.assertEqual(template.get_param_set(1), {(int, ), (float, float)})
# Nested getitem indices.
self.assertEqual(template[(int, int)], 3)
self.assertEqual(template[[int, int]], 3)
# List instantiation.
def instantiation_func(param):
return 100 + len(param)
dummy_a = (str, ) * 5
dummy_b = (str, ) * 10
template.add_instantiations(instantiation_func, [dummy_a, dummy_b])
self.assertEqual(template[dummy_a], 105)
self.assertEqual(template[dummy_b], 110)
# Ensure that we can only call this once.
dummy_c = (str, ) * 7
with self.assertRaises(RuntimeError):
template.add_instantiations(instantiation_func, [dummy_c])
with self.assertRaises(TypeError) as cm:
assert_pickle(self, template)
if sys.version_info[:2] >= (3, 8):
pickle_error = "cannot pickle 'module' object"
else:
pickle_error = "can't pickle module objects"
self.assertIn(pickle_error, str(cm.exception))
def test_point_convex_set(self):
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2 * p)
np.testing.assert_array_equal(point.x(), 2 * p)
point.set_x(x=p)
assert_pickle(self, point, lambda S: S.x())
def test_user_class(self):
test = self
@m.TemplateClass.define("MyTemplate", param_list=((int, ), (float, )))
def MyTemplate(param):
T, = param
# Ensure that we have deferred evaluation.
test.assertEqual(MyTemplate.param_list, [(int, ), (float, )])
class Impl(object):
def __init__(self):
self.T = T
self.mangled_result = self.__mangled_method()
def __mangled_method(self):
# Ensure that that mangled methods are usable.
return (T, 10)
return Impl
self.assertEqual(str(MyTemplate),
f"<TemplateClass {_TEST_MODULE}.MyTemplate>")
self.assertIsInstance(MyTemplate, m.TemplateClass)
MyDefault = MyTemplate[None]
MyInt = MyTemplate[int]
self.assertEqual(MyDefault, MyInt)
self.assertEqual(MyInt().T, int)
MyFloat = MyTemplate[float]
self.assertEqual(MyFloat().T, float)
# Test subclass checks.
class Subclass(MyTemplate[float]):
pass
self.assertFalse(MyTemplate.is_instantiation(Subclass))
self.assertFalse(MyTemplate.is_subclass_of_instantiation(object))
result = MyTemplate.is_subclass_of_instantiation(Subclass)
self.assertTrue(result)
self.assertEqual(result, MyTemplate[float])
# Test mangling behavior.
# TODO(eric.couisneau): If we use the name `Impl` for instantiations,
# then mangling among inherited templated classes will not work as
# intended. Consider an alternative, if it's ever necessary.
self.assertEqual(MyInt().mangled_result, (int, 10))
self.assertEqual(MyInt._original_name, "Impl")
self.assertTrue(hasattr(MyInt, "_Impl__mangled_method"))
self.assertEqual(MyFloat._original_name, "Impl")
self.assertEqual(MyFloat().mangled_result, (float, 10))
self.assertTrue(hasattr(MyFloat, "_Impl__mangled_method"))
assert_pickle(self, MyTemplate[int]())
def test_function(self):
template = m.TemplateFunction("func")
template.add_instantiation(int, dummy_a)
template.add_instantiation(float, dummy_b)
self.assertEqual(template[int](), 1)
self.assertIn("<function func[int] ", str(template[int]))
self.assertEqual(template[float](), 2)
self.assertEqual(str(template),
"<TemplateFunction {}.func>".format(_TEST_MODULE))
assert_pickle(self, template[int])
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
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_class(self):
template = m.TemplateClass("ClassTpl")
self.assertEqual(str(template),
"<TemplateClass {}.ClassTpl>".format(_TEST_MODULE))
template.add_instantiation(int, DummyA)
template.add_instantiation(float, DummyB)
self.assertEqual(template[int], DummyA)
self.assertEqual(str(DummyA),
"<class '{}.ClassTpl[int]'>".format(_TEST_MODULE))
self.assertEqual(template[float], DummyB)
self.assertEqual(str(DummyB),
"<class '{}.ClassTpl[float]'>".format(_TEST_MODULE))
assert_pickle(self, template[int]())
def test_scalar_api(self):
a = AD(1, [1., 0])
self.assertEqual(a.value(), 1.)
numpy_compare.assert_equal(a.derivatives(), [1., 0])
self.assertEqual(str(a), "AD{1.0, nderiv=2}")
self.assertEqual(repr(a), "<AutoDiffXd 1.0 nderiv=2>")
numpy_compare.assert_equal(a, a)
# Test construction from `float` and `int`.
numpy_compare.assert_equal(AD(1), AD(1., []))
numpy_compare.assert_equal(AD(1.), AD(1., []))
# Test implicit conversion from a simple dtype to AutoDiff.
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1), # int
AD(1., []))
numpy_compare.assert_equal(
autodiff_scalar_pass_through(1.), # float
AD(1., []))
# Test explicit conversion to float.
with self.assertRaises(TypeError) as cm:
float(a)
self.assertIn(
"not 'pydrake.autodiffutils.AutoDiffXd'", str(cm.exception))
a_scalar = np.array(a)
with self.assertRaises(TypeError) as cm:
float(a_scalar)
if np.lib.NumpyVersion(np.__version__) < "1.14.0":
self.assertEqual(
"don't know how to convert scalar number to float",
str(cm.exception))
else:
self.assertEqual(
"float() argument must be a string or a number, not "
"'pydrake.autodiffutils.AutoDiffXd'",
str(cm.exception))
# Test multi-element pass-through.
x = np.array([AD(1.), AD(2.), AD(3.)])
numpy_compare.assert_equal(autodiff_vector_pass_through(x), x)
# Ensure fixed-size vectors are correctly converted (#9886).
numpy_compare.assert_equal(autodiff_vector3_pass_through(x), x)
# Ensure we can copy.
numpy_compare.assert_equal(copy.copy(a), a)
numpy_compare.assert_equal(copy.deepcopy(a), a)
# Ensure that we can pickle.
assert_pickle(self, a, lambda x: x)
def test_bspline_trajectory(self, T):
BsplineBasis = BsplineBasis_[T]
BsplineTrajectory = BsplineTrajectory_[T]
bspline = BsplineTrajectory()
self.assertIsInstance(bspline, BsplineTrajectory)
self.assertEqual(BsplineBasis().num_basis_functions(), 0)
bspline = BsplineTrajectory(
basis=BsplineBasis(2, [0, 1, 2, 3]),
control_points=[np.zeros((3, 4)),
np.ones((3, 4))])
self.assertIsInstance(bspline.Clone(), BsplineTrajectory)
numpy_compare.assert_float_equal(bspline.value(t=1.5), 0.5 * np.ones(
(3, 4)))
self.assertEqual(bspline.rows(), 3)
self.assertEqual(bspline.cols(), 4)
numpy_compare.assert_float_equal(bspline.start_time(), 1.)
numpy_compare.assert_float_equal(bspline.end_time(), 2.)
self.assertEqual(bspline.num_control_points(), 2)
numpy_compare.assert_float_equal(bspline.control_points()[1],
np.ones((3, 4)))
numpy_compare.assert_float_equal(bspline.InitialValue(),
np.zeros((3, 4)))
numpy_compare.assert_float_equal(bspline.FinalValue(), np.ones((3, 4)))
self.assertIsInstance(bspline.basis(), BsplineBasis)
bspline.InsertKnots(additional_knots=[1.3, 1.6])
self.assertEqual(len(bspline.control_points()), 4)
self.assertIsInstance(
bspline.CopyBlock(start_row=1,
start_col=2,
block_rows=2,
block_cols=1), BsplineTrajectory)
bspline = BsplineTrajectory(basis=BsplineBasis(2, [0, 1, 2, 3]),
control_points=[np.zeros(3),
np.ones(3)])
self.assertIsInstance(bspline.CopyHead(n=2), BsplineTrajectory)
# Ensure we can copy.
self.assertEqual(copy.copy(bspline).rows(), 3)
self.assertEqual(copy.deepcopy(bspline).rows(), 3)
assert_pickle(self,
bspline,
lambda traj: np.array(traj.control_points()),
T=T)
def test_hyper_ellipsoid(self):
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
np.testing.assert_array_equal(ellipsoid.center(), self.b)
self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0))
ellipsoid.AddPointInSetConstraints(self.prog, self.x)
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
assert_pickle(self, ellipsoid, lambda S: np.vstack(
(S.A(), S.center())))
e_ball = mut.Hyperellipsoid.MakeAxisAligned(radius=[1, 1, 1],
center=self.b)
np.testing.assert_array_equal(e_ball.A(), self.A)
np.testing.assert_array_equal(e_ball.center(), self.b)
e_ball2 = mut.Hyperellipsoid.MakeHypersphere(radius=1, center=self.b)
np.testing.assert_array_equal(e_ball2.A(), self.A)
np.testing.assert_array_equal(e_ball2.center(), self.b)
e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), self.A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T)
X_I_np = np.eye(4, 4)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X_I_np)
if T == float:
self.assertEqual(str(transform), str(X_I_np))
# - Constructor with (X_I_np)
transform = Isometry3(matrix=X_I_np)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Copy constructor.
cp = Isometry3(other=transform)
numpy_compare.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Constructor with (R, p)
R_AB = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p_AB = np.array([1., 2, 3])
X_AB_np = np.eye(4)
X_AB_np[:3, :3] = R_AB
X_AB_np[:3, 3] = p_AB
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
numpy_compare.assert_float_equal(X_AB.matrix(), X_AB_np)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Setters.
X_AB = Isometry3()
X_AB.set_translation(p_AB)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
X_AB.set_rotation(R_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Cast
self.check_cast(mut.Isometry3_, T)
# - Check transactions for bad values.
if T != Expression:
X_temp = Isometry3(rotation=R_AB, translation=p_AB)
R_bad = np.copy(R_AB)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
X_temp.set_rotation(R_bad)
numpy_compare.assert_float_equal(X_temp.rotation(), R_AB)
X_bad_np = np.copy(X_I_np)
X_bad_np[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
X_temp.set_matrix(X_bad_np)
numpy_compare.assert_float_equal(X_temp.matrix(), X_AB_np)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
X_I = X_AB.inverse().multiply(X_AB)
numpy_compare.assert_float_equal(X_I.matrix(), X_I_np)
p_BQ = [10, 20, 30]
p_AQ = [21., -8, 33]
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQ), p_AQ)
p_BQlist = np.array([p_BQ, p_BQ]).T
p_AQlist = np.array([p_AQ, p_AQ]).T
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQlist),
p_AQlist)
if six.PY3:
numpy_compare.assert_float_equal(
eval("X_AB.inverse() @ X_AB").matrix(), X_I_np)
numpy_compare.assert_float_equal(eval("X_AB @ p_BQ"), p_AQ)
assert_pickle(self, X_AB, Isometry3.matrix, T=T)
def test_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(numpy_compare.resolve_type(q_identity.wxyz()), T)
numpy_compare.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
numpy_compare.assert_float_equal(
copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
numpy_compare.assert_equal(q_identity.wxyz(),
Quaternion.Identity().wxyz())
if T == float:
self.assertEqual(str(q_identity),
"Quaternion_[float](w=1.0, x=0.0, y=0.0, z=0.0)")
self.check_cast(mut.Quaternion_, T)
# Test ordering.
q_wxyz = normalize([0.1, 0.3, 0.7, 0.9])
q = Quaternion(w=q_wxyz[0], x=q_wxyz[1], y=q_wxyz[2], z=q_wxyz[3])
# - Accessors.
numpy_compare.assert_float_equal(q.w(), q_wxyz[0])
numpy_compare.assert_float_equal(q.x(), q_wxyz[1])
numpy_compare.assert_float_equal(q.y(), q_wxyz[2])
numpy_compare.assert_float_equal(q.z(), q_wxyz[3])
numpy_compare.assert_float_equal(q.xyz(), q_wxyz[1:])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
numpy_compare.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
q.set_wxyz(w=q_wxyz_new[0],
x=q_wxyz_new[1],
y=q_wxyz_new[2],
z=q_wxyz_new[3])
numpy_compare.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
numpy_compare.assert_float_equal(q_other.wxyz(), q_wxyz)
R = np.array([[0., 0, 1], [1, 0, 0], [0, 1, 0]])
q_wxyz_expected = np.array([0.5, 0.5, 0.5, 0.5])
q_other = Quaternion(q_wxyz_expected)
numpy_compare.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
numpy_compare.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
numpy_compare.assert_equal(q.wxyz(), cp.wxyz())
# Bad values.
if T != Expression:
q = Quaternion.Identity()
# - wxyz
q_wxyz_bad = [1., 2, 3, 4]
with self.assertRaises(RuntimeError):
q.set_wxyz(q_wxyz_bad)
numpy_compare.assert_float_equal(q.wxyz(), [1., 0, 0, 0])
# - Rotation.
R_bad = np.copy(R)
R_bad[0, 0] = 10
with self.assertRaises(RuntimeError):
q_other.set_rotation(R_bad)
numpy_compare.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q_AB = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
q_I = q_AB.inverse().multiply(q_AB)
numpy_compare.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
if six.PY3:
numpy_compare.assert_float_equal(
eval("q_AB.inverse() @ q_AB").wxyz(), [1., 0, 0, 0])
v_B = np.array([1., 2, 3])
v_A = np.array([3., 1, 2])
numpy_compare.assert_float_allclose(q_AB.multiply(vector=v_B), v_A)
vlist_B = np.array([v_B, v_B]).T
vlist_A = np.array([v_A, v_A]).T
numpy_compare.assert_float_equal(q_AB.multiply(vector=vlist_B),
vlist_A)
# Test deprecation.
with catch_drake_warnings(expected_count=2):
self.assertEqual(q_AB.multiply(position=v_B).shape, v_B.shape)
self.assertEqual(
q_AB.multiply(position=vlist_B).shape, vlist_B.shape)
with catch_drake_warnings(expected_count=0):
# No deprecation should happen with position arguments.
self.assertEqual(q_AB.multiply(v_B).shape, v_B.shape)
self.assertEqual(q_AB.multiply(vlist_B).shape, vlist_B.shape)
q_AB_conj = q_AB.conjugate()
numpy_compare.assert_float_equal(q_AB_conj.wxyz(),
[0.5, -0.5, -0.5, -0.5])
# Test `type_caster`s.
if T == float:
value = test_util.create_quaternion()
self.assertTrue(isinstance(value, mut.Quaternion))
test_util.check_quaternion(value)
assert_pickle(self, q_AB, Quaternion.wxyz, 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_rigid_transform(self, T):
RigidTransform = mut.RigidTransform_[T]
RotationMatrix = mut.RotationMatrix_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
Isometry3 = Isometry3_[T]
Quaternion = Quaternion_[T]
AngleAxis = AngleAxis_[T]
def check_equality(X_actual, X_expected_matrix):
self.assertIsInstance(X_actual, RigidTransform)
numpy_compare.assert_float_equal(X_actual.GetAsMatrix4(),
X_expected_matrix)
# - Constructors.
X_I_np = np.eye(4)
check_equality(RigidTransform(), X_I_np)
check_equality(RigidTransform(other=RigidTransform()), X_I_np)
check_equality(copy.copy(RigidTransform()), X_I_np)
R_I = RotationMatrix()
p_I = np.zeros(3)
rpy_I = RollPitchYaw(0, 0, 0)
quaternion_I = Quaternion.Identity()
angle = np.pi * 0
axis = [0, 0, 1]
angle_axis = AngleAxis(angle=angle, axis=axis)
check_equality(RigidTransform(R=R_I, p=p_I), X_I_np)
check_equality(RigidTransform(rpy=rpy_I, p=p_I), X_I_np)
check_equality(RigidTransform(quaternion=quaternion_I, p=p_I), X_I_np)
check_equality(RigidTransform(theta_lambda=angle_axis, p=p_I), X_I_np)
check_equality(RigidTransform(R=R_I), X_I_np)
check_equality(RigidTransform(p=p_I), X_I_np)
check_equality(RigidTransform(pose=p_I), X_I_np)
check_equality(RigidTransform(pose=X_I_np), X_I_np)
check_equality(RigidTransform(pose=X_I_np[:3]), X_I_np)
with catch_drake_warnings(expected_count=2):
check_equality(RigidTransform(matrix=X_I_np), X_I_np)
check_equality(RigidTransform.FromMatrix4(matrix=X_I_np), X_I_np)
# - Cast.
self.check_cast(mut.RigidTransform_, T)
# - Accessors, mutators, and general methods.
X = RigidTransform()
X.set(R=R_I, p=p_I)
X.SetFromIsometry3(pose=Isometry3.Identity())
check_equality(RigidTransform.Identity(), X_I_np)
self.assertIsInstance(X.rotation(), RotationMatrix)
X.set_rotation(R=R_I)
X.set_rotation(rpy=rpy_I)
X.set_rotation(quaternion=quaternion_I)
X.set_rotation(theta_lambda=angle_axis)
self.assertIsInstance(X.translation(), np.ndarray)
X.set_translation(p=np.zeros(3))
numpy_compare.assert_float_equal(X.GetAsMatrix4(), X_I_np)
numpy_compare.assert_float_equal(X.GetAsMatrix34(), X_I_np[:3])
self.assertIsInstance(X.GetAsIsometry3(), Isometry3)
check_equality(X.inverse(), X_I_np)
self.assertIsInstance(X.multiply(other=RigidTransform()),
RigidTransform)
self.assertIsInstance(X @ RigidTransform(), RigidTransform)
self.assertIsInstance(X @ [0, 0, 0], np.ndarray)
# - Test vector multiplication.
R_AB = RotationMatrix([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p_AB = np.array([1., 2, 3])
X_AB = RigidTransform(R=R_AB, p=p_AB)
p_BQ = [10, 20, 30]
p_AQ = [21., -8, 33]
numpy_compare.assert_float_equal(X_AB.multiply(p_BoQ_B=p_BQ), p_AQ)
# N.B. Remember that this takes ndarray[3, n], NOT ndarray[n, 3]!
p_BQlist = np.array([p_BQ, p_BQ]).T
p_AQlist = np.array([p_AQ, p_AQ]).T
numpy_compare.assert_float_equal(X_AB.multiply(p_BoQ_B=p_BQlist),
p_AQlist)
# Test pickling.
assert_pickle(self, X_AB, RigidTransform.GetAsMatrix4, T=T)
def test_rigid_transform(self, T):
RigidTransform = mut.RigidTransform_[T]
RotationMatrix = mut.RotationMatrix_[T]
RollPitchYaw = mut.RollPitchYaw_[T]
Isometry3 = Isometry3_[T]
Quaternion = Quaternion_[T]
AngleAxis = AngleAxis_[T]
def check_equality(X_actual, X_expected_matrix):
self.assertIsInstance(X_actual, RigidTransform)
numpy_compare.assert_float_equal(
X_actual.GetAsMatrix4(), X_expected_matrix)
# - Constructors.
X_I_np = np.eye(4)
check_equality(RigidTransform(), X_I_np)
check_equality(RigidTransform(other=RigidTransform()), X_I_np)
check_equality(copy.copy(RigidTransform()), X_I_np)
R_I = RotationMatrix()
p_I = np.zeros(3)
rpy_I = RollPitchYaw(0, 0, 0)
quaternion_I = Quaternion.Identity()
angle = np.pi * 0
axis = [0, 0, 1]
angle_axis = AngleAxis(angle=angle, axis=axis)
check_equality(RigidTransform(R=R_I, p=p_I), X_I_np)
check_equality(RigidTransform(rpy=rpy_I, p=p_I), X_I_np)
check_equality(RigidTransform(quaternion=quaternion_I, p=p_I), X_I_np)
check_equality(RigidTransform(theta_lambda=angle_axis, p=p_I), X_I_np)
check_equality(RigidTransform(R=R_I), X_I_np)
check_equality(RigidTransform(p=p_I), X_I_np)
check_equality(RigidTransform(pose=p_I), X_I_np)
check_equality(RigidTransform(pose=X_I_np), X_I_np)
check_equality(RigidTransform(pose=X_I_np[:3]), X_I_np)
# - Cast.
self.check_cast(mut.RigidTransform_, T)
# - Accessors, mutators, and general methods.
X = RigidTransform()
X.set(R=R_I, p=p_I)
X.SetFromIsometry3(pose=Isometry3.Identity())
check_equality(RigidTransform.Identity(), X_I_np)
self.assertIsInstance(X.rotation(), RotationMatrix)
X.set_rotation(R=R_I)
X.set_rotation(rpy=rpy_I)
X.set_rotation(quaternion=quaternion_I)
X.set_rotation(theta_lambda=angle_axis)
self.assertIsInstance(X.translation(), np.ndarray)
X.set_translation(p=np.zeros(3))
numpy_compare.assert_float_equal(X.GetAsMatrix4(), X_I_np)
numpy_compare.assert_float_equal(X.GetAsMatrix34(), X_I_np[:3])
self.assertIsInstance(X.GetAsIsometry3(), Isometry3)
check_equality(X.inverse(), X_I_np)
self.assertIsInstance(
X.multiply(other=RigidTransform()), RigidTransform)
self.assertIsInstance(
X.InvertAndCompose(other=RigidTransform()), RigidTransform)
self.assertIsInstance(X @ RigidTransform(), RigidTransform)
self.assertIsInstance(X @ [0, 0, 0], np.ndarray)
if T != Expression:
self.assertTrue(X.IsExactlyIdentity())
self.assertTrue(X.IsNearlyIdentity(translation_tolerance=0))
self.assertTrue(X.IsNearlyEqualTo(other=X, tolerance=0))
# - Test shaping (#13885).
v = np.array([0., 0., 0.])
vs = np.array([[1., 2., 3.], [4., 5., 6.]]).T
self.assertEqual((X @ v).shape, (3,))
self.assertEqual((X @ v.reshape((3, 1))).shape, (3, 1))
self.assertEqual((X @ vs).shape, (3, 2))
# - Test vector multiplication.
R_AB = RotationMatrix([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
p_AB = np.array([1., 2, 3])
X_AB = RigidTransform(R=R_AB, p=p_AB)
p_BQ = [10, 20, 30]
p_AQ = [21., -8, 33]
numpy_compare.assert_float_equal(X_AB.multiply(p_BoQ_B=p_BQ), p_AQ)
# N.B. Remember that this takes ndarray[3, n], NOT ndarray[n, 3]!
p_BQlist = np.array([p_BQ, p_BQ]).T
p_AQlist = np.array([p_AQ, p_AQ]).T
numpy_compare.assert_float_equal(
X_AB.multiply(p_BoQ_B=p_BQlist), p_AQlist)
# - Repr.
z = repr(T(0.0))
i = repr(T(1.0))
type_suffix = {
float: "",
AutoDiffXd: "_[AutoDiffXd]",
Expression: "_[Expression]",
}[T]
self.assertEqual(repr(RigidTransform()), textwrap.dedent(f"""\
RigidTransform{type_suffix}(
R=RotationMatrix{type_suffix}([
[{i}, {z}, {z}],
[{z}, {i}, {z}],
[{z}, {z}, {i}],
]),
p=[{z}, {z}, {z}],
)"""))
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(RigidTransform()))
# TODO(jwnimmer-tri) Once IsExactlyEqualTo is bound, we can easily
# check the contents of the roundtrip object here.
self.assertIsInstance(roundtrip, RigidTransform)
# Test pickling.
assert_pickle(self, X_AB, RigidTransform.GetAsMatrix4, T=T)
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
A_l1 = np.array([[1, 1, 1], [-1, 1, 1], [1, -1, 1], [-1, -1, 1],
[1, 1, -1], [-1, 1, -1], [1, -1, -1], [-1, -1, -1]])
b_l1 = np.ones(8)
h_l1_ball = mut.HPolyhedron.MakeL1Ball(dim=3)
np.testing.assert_array_equal(A_l1, h_l1_ball.A())
np.testing.assert_array_equal(b_l1, h_l1_ball.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
generator = RandomGenerator()
sample = h_box.UniformSample(generator=generator)
self.assertEqual(sample.shape, (3, ))
self.assertEqual(
h_box.UniformSample(generator=generator,
previous_sample=sample).shape, (3, ))
h_half_box = mut.HPolyhedron.MakeBox(lb=[-0.5, -0.5, -0.5],
ub=[0.5, 0.5, 0.5])
self.assertTrue(h_half_box.ContainedIn(other=h_unit_box))
h_half_box2 = h_half_box.Intersection(other=h_unit_box,
check_for_redundancy=True)
self.assertIsInstance(h_half_box2, mut.HPolyhedron)
self.assertEqual(h_half_box2.ambient_dimension(), 3)
np.testing.assert_array_almost_equal(h_half_box2.A(), h_half_box.A())
np.testing.assert_array_almost_equal(h_half_box2.b(), h_half_box.b())
# Intersection of 1/2*unit_box and unit_box and reducing the redundant
# inequalities should result in the 1/2*unit_box.
h_half_box_intersect_unit_box = h_half_box.Intersection(
other=h_unit_box, check_for_redundancy=False)
# Check that the ReduceInequalities binding works.
h_half_box3 = h_half_box_intersect_unit_box.ReduceInequalities()
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_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 test_shapes(self):
# We'll test some invariants on all shapes as inherited from the Shape
# API.
def assert_shape_api(shape):
self.assertIsInstance(shape, mut.Shape)
shape_cls = type(shape)
shape_copy = shape.Clone()
self.assertIsInstance(shape_copy, shape_cls)
self.assertIsNot(shape, shape_copy)
# Note: these are ordered alphabetical order and not in the declared
# order in shape_specification.h
box = mut.Box(width=1.0, depth=2.0, height=3.0)
assert_shape_api(box)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
assert_pickle(
self, box,
lambda shape: [shape.width(
), shape.depth(), shape.height()])
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
self.assertAlmostEqual(mut.CalcVolume(box), 6.0, 1e-14)
capsule = mut.Capsule(radius=1.0, length=2.0)
assert_shape_api(capsule)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
assert_pickle(
self, capsule,
lambda shape: [shape.radius(), shape.length()])
junk_path = "arbitrary/path"
convex = mut.Convex(absolute_filename=junk_path, scale=1.0)
assert_shape_api(convex)
self.assertEqual(convex.filename(), junk_path)
self.assertEqual(convex.scale(), 1.0)
assert_pickle(
self, convex,
lambda shape: [shape.filename(), shape.scale()])
cylinder = mut.Cylinder(radius=1.0, length=2.0)
assert_shape_api(cylinder)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
assert_pickle(
self, cylinder,
lambda shape: [shape.radius(), shape.length()])
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
assert_shape_api(ellipsoid)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
assert_pickle(self, ellipsoid, lambda shape:
[shape.a(), shape.b(), shape.c()])
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
mesh = mut.Mesh(absolute_filename=junk_path, scale=1.0)
assert_shape_api(mesh)
self.assertEqual(mesh.filename(), junk_path)
self.assertEqual(mesh.scale(), 1.0)
assert_pickle(
self, mesh,
lambda shape: [shape.filename(), shape.scale()])
sphere = mut.Sphere(radius=1.0)
assert_shape_api(sphere)
self.assertEqual(sphere.radius(), 1.0)
assert_pickle(self, sphere, mut.Sphere.radius)
cone = mut.MeshcatCone(height=1.2, a=3.4, b=5.6)
assert_shape_api(cone)
self.assertEqual(cone.height(), 1.2)
self.assertEqual(cone.a(), 3.4)
self.assertEqual(cone.b(), 5.6)
assert_pickle(self, cone,
lambda shape: [shape.height(
), shape.a(), shape.b()])