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_map_qdot_to_v_and_back(self, T): MultibodyPlant = MultibodyPlant_[T] RigidTransform = RigidTransform_[T] RollPitchYaw = RollPitchYaw_[T] # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f = MultibodyPlant_[float]() iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" "iiwa_description/sdf/iiwa14_no_collision.sdf") # Use floating base to effectively add a quatnerion in the generalized # quaternion. iiwa_model = Parser(plant=plant_f).AddModelFromFile( file_name=iiwa_sdf_path, model_name='robot') plant_f.Finalize() plant = to_type(plant_f, T) context = plant.CreateDefaultContext() # Try mapping velocity to qdot and back. nq = plant.num_positions() nv = plant.num_velocities() q_init = np.linspace(start=1.0, stop=nq, num=nq) plant.SetPositions(context, q_init) # Overwrite the (invalid) base coordinates, wherever in `q` they are. plant.SetFreeBodyPose( context, plant.GetBodyByName("iiwa_link_0"), RigidTransform(RollPitchYaw([0.1, 0.2, 0.3]), p=[0.4, 0.5, 0.6])) v_expected = np.linspace(start=-1.0, stop=-nv, num=nv) qdot = plant.MapVelocityToQDot(context, v_expected) v_remap = plant.MapQDotToVelocity(context, qdot) numpy_compare.assert_float_allclose(v_remap, v_expected)
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_snopt_solver(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddLinearConstraint(x[0] + x[1] == 1) prog.AddBoundingBoxConstraint(0, 1, x[1]) prog.AddLinearCost(x[0]) solver = SnoptSolver() self.assertEqual(solver.solver_id(), SnoptSolver.id()) if solver.available(): self.assertTrue(solver.enabled()) self.assertEqual(solver.solver_type(), mp.SolverType.kSnopt) result = solver.Solve(prog, None, None) self.assertTrue(result.is_success()) numpy_compare.assert_float_allclose(result.GetSolution(x), [0., 1.], atol=1E-7) self.assertEqual(result.get_solver_details().info, 1) np.testing.assert_allclose(result.get_solver_details().xmul, np.array([0., -1])) np.testing.assert_allclose(result.get_solver_details().F, np.array([0, 1.])) np.testing.assert_allclose(result.get_solver_details().Fmul, np.array([0, 1.]))
def test_lagrange_interpolating_polynomial_vector(self, T): PiecewisePolynomial = PiecewisePolynomial_[T] t = [0., 1., 2.] x = np.diag([4., 5., 6.]) pp = PiecewisePolynomial.LagrangeInterpolatingPolynomial(times=t, samples=x) self.assertEqual(pp.get_number_of_segments(), 1) numpy_compare.assert_float_allclose(pp.value(1.), x[:, [1]], 1e-12)
def test_scs_solver(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddLinearConstraint(x[0] >= 1) prog.AddLinearConstraint(x[1] >= 1) prog.AddQuadraticCost(np.eye(2), np.zeros(2), x) solver = ScsSolver() self.assertEqual(solver.solver_id(), ScsSolver.id()) self.assertTrue(solver.available()) self.assertEqual(solver.solver_id().name(), "SCS") self.assertEqual(solver.SolverName(), "SCS") self.assertEqual(solver.solver_type(), mp.SolverType.kScs) result = solver.Solve(prog, None, None) self.assertTrue(result.is_success()) # Use a loose tolerance for the check since the correctness of the # solver has been validated in the C++ code, while the Python test # just verifies if the binding works or not. atol = 1E-3 numpy_compare.assert_float_allclose( result.GetSolution(x), [1.0, 1.0], atol=atol) numpy_compare.assert_float_allclose( result.get_solver_details().primal_objective, 1.0, atol=atol) numpy_compare.assert_float_allclose( result.get_solver_details().primal_residue, np.array([0.]), atol=atol) numpy_compare.assert_float_allclose( result.get_solver_details().y, np.array([1., 1., 1.5, 0.5, -1., -1]), atol=atol)
def test_scs_solver(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddLinearConstraint(x[0] >= 1) prog.AddLinearConstraint(x[1] >= 1) prog.AddQuadraticCost(np.eye(2), np.zeros(2), x) solver = ScsSolver() self.assertEqual(solver.solver_id(), ScsSolver.id()) self.assertTrue(solver.available()) self.assertEqual(solver.solver_id().name(), "SCS") self.assertEqual(solver.SolverName(), "SCS") self.assertEqual(solver.solver_type(), mp.SolverType.kScs) result = solver.Solve(prog, None, None) self.assertTrue(result.is_success()) numpy_compare.assert_float_allclose( result.GetSolution(x), [1.0, 1.0], atol=1E-7) numpy_compare.assert_float_allclose( result.get_solver_details().primal_objective, 1.0, atol=1E-7) numpy_compare.assert_float_allclose( result.get_solver_details().primal_residue, np.array([0.]), atol=1e-10) numpy_compare.assert_float_allclose( result.get_solver_details().y, np.array([1., 1., 1.5, 0.5, -1., -1]), atol=1e-10)
def test_ibex_solver(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddBoundingBoxConstraint(-1., 1., x[0]) prog.AddBoundingBoxConstraint(-1., 1., x[1]) prog.AddCost(x[0] - x[1]) solver = IbexSolver() self.assertEqual(solver.solver_id(), IbexSolver.id()) self.assertTrue(solver.available()) self.assertEqual(solver.solver_id().name(), "IBEX") self.assertEqual(solver.SolverName(), "IBEX") self.assertEqual(solver.solver_type(), mp.SolverType.kIbex) result = solver.Solve(prog, None, None) self.assertTrue(result.is_success()) numpy_compare.assert_float_allclose(result.GetSolution(x), [-1.0, 1.0], atol=1E-7)
def test_nlopt_solver(self): prog = mp.MathematicalProgram() x = prog.NewContinuousVariables(2, "x") prog.AddLinearConstraint(x[0] >= 1) prog.AddLinearConstraint(x[1] >= 1) prog.AddQuadraticCost(np.eye(2), np.zeros(2), x) x_expected = np.array([1., 1.]) solver = NloptSolver() self.assertTrue(solver.available()) self.assertEqual(solver.solver_id().name(), "NLopt") self.assertEqual(solver.SolverName(), "NLopt") self.assertEqual(solver.solver_type(), mp.SolverType.kNlopt) result = solver.Solve(prog, None, None) self.assertTrue(result.is_success()) numpy_compare.assert_float_allclose( result.GetSolution(x), x_expected, atol=1E-7) self.assertEqual(result.get_solver_details().status, 4)
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_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_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 test_multibody_state_access(self, T): MultibodyPlant = MultibodyPlant_[T] plant_f = MultibodyPlant_[float]() file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") # N.B. `Parser` only supports `MultibodyPlant_[float]`. Parser(plant_f).AddModelFromFile(file_name) plant_f.Finalize() plant = to_type(plant_f, T) context = plant.CreateDefaultContext() nq = 2 nv = 2 self.assertEqual(plant.num_positions(), nq) self.assertEqual(plant.num_velocities(), nv) q0 = np.array([3.14, 2.]) v0 = np.array([-0.5, 1.]) x0 = np.concatenate([q0, v0]) # The default state is all values set to zero. x = plant.GetPositionsAndVelocities(context) numpy_compare.assert_float_equal(x, np.zeros(4)) # WARNING: The following oddities occur from the fact that # `ndarray[object]` cannot be referenced (#8116). Be careful when # writing scalar-generic code. if T == float: # Can reference matrices. Use `x_ref`. # Write into a mutable reference to the state vector. x_ref = plant.GetMutablePositionsAndVelocities(context) x_ref[:] = x0 def set_zero(): x_ref.fill(0) else: # Cannot reference matrices. Use setters. plant.SetPositionsAndVelocities(context, x0) def set_zero(): plant.SetPositionsAndVelocities( context, np.zeros(nq + nv)) # Verify that positions and velocities were set correctly. numpy_compare.assert_float_equal(plant.GetPositions(context), q0) numpy_compare.assert_float_equal(plant.GetVelocities(context), v0) # Verify we did modify the state stored in context. x = plant.GetPositionsAndVelocities(context) numpy_compare.assert_float_equal(x, x0) # Now set positions and velocities independently and check them. zeros_2 = np.zeros([2, ]) set_zero() plant.SetPositions(context, q0) numpy_compare.assert_float_equal(plant.GetPositions(context), q0) numpy_compare.assert_float_equal(plant.GetVelocities(context), zeros_2) set_zero() plant.SetVelocities(context, v0) numpy_compare.assert_float_allclose( plant.GetPositions(context), zeros_2) numpy_compare.assert_float_allclose(plant.GetVelocities(context), v0) # Now test SetPositionsAndVelocities(). set_zero() plant.SetPositionsAndVelocities(context, x0) numpy_compare.assert_float_allclose( plant.GetPositionsAndVelocities(context), x0) # Test existence of context resetting methods. plant.SetDefaultState(context, state=context.get_mutable_state()) # Test existence of limits. self.assertEqual(plant.GetPositionLowerLimits().shape, (nq,)) self.assertEqual(plant.GetPositionUpperLimits().shape, (nq,)) self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv,)) self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv,)) self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv,)) self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv,))
def check_angle_axis(self, T): AngleAxis = mut.AngleAxis_[T] value_identity = AngleAxis.Identity() self.assertEqual(npc.resolve_type(value_identity.angle()), T) npc.assert_float_equal(value_identity.angle(), 0.) npc.assert_float_equal(value_identity.axis(), [1., 0, 0]) # Construct with rotation matrix. R = np.array([ [0., 1, 0], [-1, 0, 0], [0, 0, 1]]) value = AngleAxis(rotation=R) npc.assert_float_allclose(value.rotation(), R) npc.assert_float_allclose(copy.copy(value).rotation(), R) npc.assert_float_allclose(value.inverse().rotation(), R.T) npc.assert_float_allclose( value.multiply(value.inverse()).rotation(), np.eye(3)) if six.PY3: npc.assert_float_allclose( eval("value @ value.inverse()").rotation(), np.eye(3)) value.set_rotation(np.eye(3)) npc.assert_float_equal(value.rotation(), np.eye(3)) # Construct with quaternion. Quaternion = mut.Quaternion_[T] q = Quaternion(R) value = AngleAxis(quaternion=q) npc.assert_float_allclose( value.quaternion().wxyz(), npc.to_float(q.wxyz())) value.set_quaternion(Quaternion.Identity()) npc.assert_float_equal(value.quaternion().wxyz(), [1., 0, 0, 0]) # Test setters. value = AngleAxis(value_identity) value.set_angle(np.pi / 4) v = normalize(np.array([0.1, 0.2, 0.3])) if T != Expression: with self.assertRaises(RuntimeError): value.set_axis([0.1, 0.2, 0.3]) value.set_axis(v) npc.assert_float_equal(value.angle(), np.pi / 4) npc.assert_float_equal(value.axis(), v) # Test symmetry based on accessors. # N.B. `Eigen::AngleAxis` does not disambiguate by restricting internal # angles and axes to a half-plane. angle = np.pi / 6 axis = normalize([0.1, 0.2, 0.3]) value = AngleAxis(angle=angle, axis=axis) value_sym = AngleAxis(angle=-angle, axis=-axis) npc.assert_equal(value.rotation(), value_sym.rotation()) npc.assert_equal(value.angle(), -value_sym.angle()) npc.assert_equal(value.axis(), -value_sym.axis())
def test_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]) numpy_compare.assert_float_equal((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) q_AB_conj = q_AB.conjugate() numpy_compare.assert_float_equal(q_AB_conj.wxyz(), [0.5, -0.5, -0.5, -0.5]) numpy_compare.assert_float_equal( q_I.slerp(t=0, other=q_I).wxyz(), [1., 0, 0, 0]) # - Test shaping (#13885). v = np.array([0., 0., 0.]) vs = np.array([[1., 2., 3.], [4., 5., 6.]]).T self.assertEqual((q_AB @ v).shape, (3, )) self.assertEqual((q_AB @ v.reshape((3, 1))).shape, (3, 1)) self.assertEqual((q_AB @ vs).shape, (3, 2)) # Test `type_caster`s. if T == float: value = test_util.create_quaternion() self.assertTrue(isinstance(value, mut.Quaternion)) test_util.check_quaternion(value) assert_pickle(self, q_AB, Quaternion.wxyz, T=T)