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)
