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)