def test_shapes(self):
RigidTransform = RigidTransform_[float]
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
capsule = mut.Capsule(radius=1.0, length=2.0)
self.assertEqual(capsule.radius(), 1.0)
self.assertEqual(capsule.length(), 2.0)
ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0)
self.assertEqual(ellipsoid.a(), 1.0)
self.assertEqual(ellipsoid.b(), 2.0)
self.assertEqual(ellipsoid.c(), 3.0)
X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1])
self.assertIsInstance(X_FH, RigidTransform)
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
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.eye(4)
check_equality(RigidTransform(), X_I)
check_equality(RigidTransform(other=RigidTransform()), X_I)
check_equality(copy.copy(RigidTransform()), X_I)
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)
check_equality(RigidTransform(rpy=rpy_I, p=p_I), X_I)
check_equality(RigidTransform(quaternion=quaternion_I, p=p_I), X_I)
check_equality(RigidTransform(theta_lambda=angle_axis, p=p_I), X_I)
check_equality(RigidTransform(R=R_I), X_I)
check_equality(RigidTransform(p=p_I), X_I)
check_equality(RigidTransform(matrix=X_I), X_I)
check_equality(RigidTransform.FromMatrix4(matrix=X_I), X_I)
# - 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)
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)
numpy_compare.assert_float_equal(X.GetAsMatrix34(), X_I[:3])
self.assertIsInstance(X.GetAsIsometry3(), Isometry3)
check_equality(X.inverse(), X_I)
self.assertIsInstance(
X.multiply(other=RigidTransform()), RigidTransform)
self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray)
if six.PY3:
self.assertIsInstance(
eval("X @ RigidTransform()"), RigidTransform)
self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
def test_compare_and_concatenate(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[10.], [20.], [30.]]).transpose()
pp1 = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp2 = PiecewisePolynomial.FirstOrderHold([2., 3., 4.], x)
self.assertTrue(pp1.isApprox(
other=pp1, tol=1e-14, tol_type=ToleranceType.kRelative))
pp1.ConcatenateInTime(other=pp2)
numpy_compare.assert_float_equal(pp1.end_time(), 4.)
def test_first_order_hold_vector(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
numpy_compare.assert_float_equal(pp.value(.5), np.array([[2.], [3.]]))
deriv = pp.MakeDerivative(derivative_order=1)
numpy_compare.assert_float_equal(deriv.value(.5),
np.array([[2.], [2.]]))
pp.AppendFirstOrderSegment(time=3., sample=[-0.4, .57])
def test_piecewise_polynomial_constant_constructor(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[1.], [4.]])
pp = PiecewisePolynomial(x)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 1)
numpy_compare.assert_float_equal(pp.value(11.), x)
# Ensure we can copy.
self.assertEqual(copy.copy(pp).rows(), 2)
self.assertEqual(copy.deepcopy(pp).rows(), 2)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.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])
numpy_compare.assert_float_equal(
bspline.EvaluateBasisFunctionI(i=0, parameter_value=5.7), 0.)
def test_multibody_add_frame(self, T):
MultibodyPlant = MultibodyPlant_[T]
FixedOffsetFrame = FixedOffsetFrame_[T]
Frame = Frame_[T]
plant = MultibodyPlant()
frame = plant.AddFrame(frame=FixedOffsetFrame(
name="frame", P=plant.world_frame(),
X_PF=RigidTransform_[float].Identity(), model_instance=None))
self.assertIsInstance(frame, Frame)
numpy_compare.assert_float_equal(
frame.GetFixedPoseInBodyFrame().GetAsMatrix4(),
np.eye(4))
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_slice_and_shift(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp_sub = pp.slice(start_segment_index=1, num_segments=1)
self.assertEqual(pp_sub.get_number_of_segments(), 1)
numpy_compare.assert_float_equal(pp_sub.start_time(), 1.)
numpy_compare.assert_float_equal(pp_sub.end_time(), 2.)
values_sub = np.array(list(map(pp_sub.value, [1., 2.])))
numpy_compare.assert_float_equal(values_sub, [[[20.]], [[30.]]])
pp_sub.shiftRight(10.)
numpy_compare.assert_float_equal(pp_sub.start_time(), 11.)
numpy_compare.assert_float_equal(pp_sub.end_time(), 12.)
def test_bspline_trajectory(self):
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)
self.assertEqual(bspline.start_time(), 1.)
self.assertEqual(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)
def test_analysis_methods(self, T):
Polynomial = Polynomial_[T]
p = Polynomial([1, 2, 3])
self.assertEqual(p.GetNumberOfCoefficients(), 3)
self.assertEqual(p.GetDegree(), 2)
self.assertFalse(p.IsAffine())
numpy_compare.assert_float_equal(p.GetCoefficients(),
np.array([1., 2., 3.]))
p_d = p.Derivative(derivative_order=1)
self.assertEqual(p_d.GetDegree(), 1)
p_i = p.Integral(integration_constant=0)
self.assertEqual(p_i.GetDegree(), 3)
numpy_compare.assert_equal(
p.CoefficientsAlmostEqual(p, 1e-14, ToleranceType.kRelative), True)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
# Test for existence of deprecated accessors.
with catch_drake_warnings(expected_count=3):
cylinder.get_radius()
cylinder.get_length()
sphere.get_radius()
def test_reverse_and_scale_time(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0.5, 1., 2.], x)
pp.ReverseTime()
numpy_compare.assert_float_equal(pp.start_time(), -2.0)
numpy_compare.assert_float_equal(pp.end_time(), -0.5)
pp.ScaleTime(2.0)
numpy_compare.assert_float_equal(pp.start_time(), -4.0)
numpy_compare.assert_float_equal(pp.end_time(), -1.0)
def test_shapes(self):
sphere = mut.Sphere(radius=1.0)
self.assertEqual(sphere.radius(), 1.0)
cylinder = mut.Cylinder(radius=1.0, length=2.0)
self.assertEqual(cylinder.radius(), 1.0)
self.assertEqual(cylinder.length(), 2.0)
box = mut.Box(width=1.0, depth=2.0, height=3.0)
self.assertEqual(box.width(), 1.0)
self.assertEqual(box.depth(), 2.0)
self.assertEqual(box.height(), 3.0)
numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0]))
box_mesh_path = FindResourceOrThrow(
"drake/systems/sensors/test/models/meshes/box.obj")
mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(mesh.filename(), box_mesh_path)
self.assertEqual(mesh.scale(), 1.0)
convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0)
self.assertEqual(convex.filename(), box_mesh_path)
self.assertEqual(convex.scale(), 1.0)
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 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_custom_trajectory(self):
trajectory = CustomTrajectory()
self.assertEqual(trajectory.rows(), 1)
self.assertEqual(trajectory.cols(), 2)
self.assertEqual(trajectory.start_time(), 3.0)
self.assertEqual(trajectory.end_time(), 4.0)
self.assertTrue(trajectory.has_derivative())
numpy_compare.assert_float_equal(trajectory.value(t=1.5),
np.array([[2.5, 3.5]]))
numpy_compare.assert_float_equal(
trajectory.EvalDerivative(t=2.3, derivative_order=1),
np.ones((1, 2)))
numpy_compare.assert_float_equal(
trajectory.EvalDerivative(t=2.3, derivative_order=2),
np.zeros((1, 2)))
def test_piecewise_pose(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
PiecewisePose = PiecewisePose_[T]
PiecewiseQuaternionSlerp = PiecewiseQuaternionSlerp_[T]
Quaternion = Quaternion_[T]
RigidTransform = RigidTransform_[T]
# Test empty constructor.
ppose = PiecewisePose()
self.assertEqual(ppose.rows(), 4)
self.assertEqual(ppose.cols(), 4)
self.assertEqual(ppose.get_number_of_segments(), 0)
t = [0., 1., 2.]
q = Quaternion()
pp = PiecewisePolynomial.FirstOrderHold(t, np.zeros((3, 3)))
pq = PiecewiseQuaternionSlerp(t, [q, q, q])
ppose = PiecewisePose(position_trajectory=pp,
orientation_trajectory=pq)
self.assertEqual(ppose.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(
ppose.GetPose(time=0).GetAsMatrix4(), np.eye(4))
numpy_compare.assert_float_equal(ppose.GetVelocity(time=0),
np.zeros((6, )))
numpy_compare.assert_float_equal(ppose.GetAcceleration(time=0),
np.zeros((6, )))
self.assertTrue(ppose.IsApprox(other=ppose, tol=0.0))
self.assertIsInstance(ppose.get_position_trajectory(),
PiecewisePolynomial)
self.assertIsInstance(ppose.get_orientation_trajectory(),
PiecewiseQuaternionSlerp)
X = RigidTransform()
ppose = PiecewisePose.MakeLinear(times=t, poses=[X, X, X])
self.assertEqual(ppose.get_number_of_segments(), 2)
ppose = PiecewisePose.MakeCubicLinearWithEndLinearVelocity(
times=t,
poses=[X, X, X],
start_vel=np.zeros((3, 1)),
end_vel=np.zeros((3, 1)))
self.assertEqual(ppose.get_number_of_segments(), 2)
# Ensure we can copy.
self.assertEqual(copy.copy(ppose).get_number_of_segments(), 2)
self.assertEqual(copy.deepcopy(ppose).get_number_of_segments(), 2)
def test_matrix_trajectories(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
A0 = np.array([[1., 2., 3.], [4., 5., 6.]])
A1 = np.array([[7., 8., 9.], [10., 11., 12.]])
A2 = np.array([[13., 14., 15.], [16., 17., 18.]])
t = [0., 1., 2.]
pp = dict()
pp["zoh"] = PiecewisePolynomial.ZeroOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["foh"] = PiecewisePolynomial.FirstOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["hermite"] = PiecewisePolynomial.CubicShapePreserving(
breaks=t, samples=[A0, A1, A2], zero_end_point_derivatives=False)
pp["c1"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=[A0, A1, A2], periodic_end=False)
pp["c2"] = PiecewisePolynomial.CubicHermite(
breaks=t,
samples=[A0, A1, A2],
samples_dot=[0 * A0, 0 * A1, 0 * A2])
pp["c3"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t,
samples=[A0, A1, A2],
sample_dot_at_start=0 * A0,
sample_dot_at_end=0 * A0)
pp["lagrange"] = PiecewisePolynomial.LagrangeInterpolatingPolynomial(
times=t, samples=[A0, A1, A2])
for name, traj in pp.items():
if name == "lagrange":
self.assertEqual(traj.get_number_of_segments(), 1)
else:
self.assertEqual(traj.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(traj.start_time(), 0.)
numpy_compare.assert_float_equal(traj.end_time(), 2.)
self.assertEqual(traj.rows(), 2)
self.assertEqual(traj.cols(), 3)
# Check the values for the easy cases:
numpy_compare.assert_float_equal(pp["zoh"].value(.5), A0)
numpy_compare.assert_float_allclose(pp["foh"].value(.5),
0.5 * A0 + 0.5 * A1, 1e-15)
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_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)
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 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)
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_equality(X_actual, X_expected_matrix):
self.assertIsInstance(X_actual, RigidTransform)
numpy_compare.assert_float_equal(X_actual.GetAsMatrix4(),
X_expected_matrix)
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()])
def test_quaternion_slerp(self, T):
AngleAxis = AngleAxis_[T]
PiecewiseQuaternionSlerp = PiecewiseQuaternionSlerp_[T]
Quaternion = Quaternion_[T]
RotationMatrix = RotationMatrix_[T]
# Test empty constructor.
pq = PiecewiseQuaternionSlerp()
self.assertEqual(pq.rows(), 4)
self.assertEqual(pq.cols(), 1)
self.assertEqual(pq.get_number_of_segments(), 0)
t = [0., 1., 2.]
# Make identity rotations.
q = Quaternion()
m = np.identity(3)
a = AngleAxis()
R = RotationMatrix()
# Test quaternion constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, quaternions=[q, q, q])
self.assertEqual(pq.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(pq.value(0.5), np.eye(3))
# Test matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[m, m, m])
self.assertEqual(pq.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(pq.value(0.5), np.eye(3))
# Test axis angle constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, angle_axes=[a, a, a])
self.assertEqual(pq.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(pq.value(0.5), np.eye(3))
# Test rotation matrix constructor.
pq = PiecewiseQuaternionSlerp(breaks=t, rotation_matrices=[R, R, R])
self.assertEqual(pq.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(pq.value(0.5), np.eye(3))
# Test append operations.
pq.Append(time=3., quaternion=q)
pq.Append(time=4., rotation_matrix=R)
pq.Append(time=5., angle_axis=a)
# Test getters.
pq = PiecewiseQuaternionSlerp(
breaks=[0, 1], angle_axes=[a, AngleAxis(np.pi/2, [0, 0, 1])]
)
numpy_compare.assert_float_equal(
pq.orientation(time=0).wxyz(), np.array([1., 0., 0., 0.])
)
numpy_compare.assert_float_allclose(
pq.angular_velocity(time=0), np.array([0, 0, np.pi/2]),
atol=1e-15, rtol=0,
)
numpy_compare.assert_float_equal(
pq.angular_acceleration(time=0), np.zeros(3)
)
numpy_compare.assert_float_allclose(
pq.orientation(time=1).wxyz(),
np.array([np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)]),
atol=1e-15, rtol=0,
)
numpy_compare.assert_float_allclose(
pq.angular_velocity(time=1), np.array([0, 0, np.pi/2]),
atol=1e-15, rtol=0,
)
numpy_compare.assert_float_equal(
pq.angular_acceleration(time=1), np.zeros(3)
)
# Ensure we can copy.
numpy_compare.assert_float_allclose(
copy.copy(pq).orientation(time=1).wxyz(),
np.array([np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)]),
atol=1e-15, rtol=0,
)
numpy_compare.assert_float_allclose(
copy.deepcopy(pq).orientation(time=1).wxyz(),
np.array([np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)]),
atol=1e-15, rtol=0,
)
def check_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(npc.resolve_type(transform.matrix()), T)
X = np.eye(4, 4)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(copy.copy(transform).matrix(), X)
if T == float:
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = Isometry3(matrix=X)
npc.assert_float_equal(transform.matrix(), X)
# - Copy constructor.
cp = Isometry3(other=transform)
npc.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
npc.assert_float_equal(transform.matrix(), X)
# - Constructor with (R, p)
R = np.array([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
p = np.array([1., 2, 3])
X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1]))
transform = Isometry3(rotation=R, translation=p)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(transform.translation(), p)
transform.set_translation(-p)
npc.assert_float_equal(transform.translation(), -p)
npc.assert_float_equal(transform.rotation(), R)
transform.set_rotation(R.T)
npc.assert_float_equal(transform.rotation(), R.T)
# - Check transactions for bad values.
if T != Expression:
transform = Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
npc.assert_float_equal(transform.rotation(), R)
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
npc.assert_float_equal(transform.matrix(), X)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
npc.assert_float_equal(transform_I.matrix(), np.eye(4))
npc.assert_float_equal(
transform.multiply(position=[10, 20, 30]), [21., -8, 33])
if six.PY3:
npc.assert_float_equal(
eval("transform.inverse() @ transform").matrix(), np.eye(4))
npc.assert_float_equal(
eval("transform @ [10, 20, 30]"), [21., -8, 33])
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 check_quaternion(self, T):
# Simple API.
Quaternion = mut.Quaternion_[T]
cast = np.vectorize(T)
q_identity = Quaternion()
self.assertEqual(npc.resolve_type(q_identity.wxyz()), T)
npc.assert_float_equal(q_identity.wxyz(), [1., 0, 0, 0])
npc.assert_float_equal(copy.copy(q_identity).wxyz(), [1., 0, 0, 0])
npc.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)")
# 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.
npc.assert_float_equal(q.w(), q_wxyz[0])
npc.assert_float_equal(q.x(), q_wxyz[1])
npc.assert_float_equal(q.y(), q_wxyz[2])
npc.assert_float_equal(q.z(), q_wxyz[3])
npc.assert_float_equal(q.xyz(), q_wxyz[1:])
npc.assert_float_equal(q.wxyz(), q_wxyz)
# - Mutators.
q_wxyz_new = q_wxyz[::-1]
npc.assert_not_equal(q_wxyz, q_wxyz_new)
q.set_wxyz(wxyz=q_wxyz_new)
npc.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])
npc.assert_float_equal(q.wxyz(), q_wxyz_new)
# Alternative constructors.
q_other = Quaternion(wxyz=q_wxyz)
npc.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)
npc.assert_float_equal(q_other.rotation(), R)
R_I = np.eye(3, 3)
q_other.set_rotation(R_I)
npc.assert_equal(q_other.wxyz(), q_identity.wxyz())
# - Copy constructor.
cp = Quaternion(other=q)
npc.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)
npc.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)
npc.assert_float_equal(q_other.rotation(), R_I)
# Operations.
q = Quaternion(wxyz=[0.5, 0.5, 0.5, 0.5])
npc.assert_float_equal(q.multiply(position=[1, 2, 3]), [3., 1, 2])
q_I = q.inverse().multiply(q)
npc.assert_float_equal(q_I.wxyz(), [1., 0, 0, 0])
if six.PY3:
npc.assert_float_equal(
eval("q.inverse() @ q").wxyz(), [1., 0, 0, 0])
q_conj = q.conjugate()
npc.assert_float_equal(q_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)
def test_zero_order_hold_vector(self, T):
PiecewisePolynomial = PiecewisePolynomial_[T]
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.ZeroOrderHold([0., 1., 2.], x)
pp_d = pp.derivative(derivative_order=1)
numpy_compare.assert_float_equal(pp.value(.5), np.array([[1.], [2.]]))
numpy_compare.assert_float_equal(
pp_d.value(.5), np.array([[0.], [0.]]))
numpy_compare.assert_float_equal(
pp.EvalDerivative(t=.5, derivative_order=1),
np.array([[0.], [0.]]))
p = pp.getPolynomial(segment_index=0, row=1, col=0)
numpy_compare.assert_float_equal(p.GetCoefficients(), np.array([2.]))
self.assertEqual(pp.getSegmentPolynomialDegree(segment_index=1), 0)
self.assertEqual(pp.get_number_of_segments(), 2)
numpy_compare.assert_float_equal(pp.start_time(), 0.)
numpy_compare.assert_float_equal(pp.end_time(), 2.)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 1)
numpy_compare.assert_float_equal(pp.start_time(segment_index=0), 0.)
numpy_compare.assert_float_equal(pp.end_time(segment_index=0), 1.)
numpy_compare.assert_float_equal(pp.duration(segment_index=0), 1.)
numpy_compare.assert_equal(pp.is_time_in_range(t=1.5), True)
self.assertEqual(pp.get_segment_index(t=1.5), 1)
numpy_compare.assert_float_equal(pp.get_segment_times(), [0., 1., 2.])
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)
