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_multibody_dynamics(self, T):
MultibodyPlant = MultibodyPlant_[T]
MultibodyForces = MultibodyForces_[T]
SpatialForce = SpatialForce_[T]
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
# N.B. `Parser` only supports `MultibodyPlant_[float]`.
plant_f = MultibodyPlant_[float]()
Parser(plant_f).AddModelFromFile(file_name)
# Getting ready for when we set foot on Mars :-).
gravity_vector = np.array([0.0, 0.0, -3.71])
plant_f.mutable_gravity_field().set_gravity_vector(gravity_vector)
plant_f.Finalize()
plant = to_type(plant_f, T)
context = plant.CreateDefaultContext()
numpy_compare.assert_float_equal(
plant.gravity_field().gravity_vector(), gravity_vector)
# Set an arbitrary configuration away from the model's fixed point.
plant.SetPositions(context, [0.1, 0.2])
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)
self.assertTrue(M.shape == (2, 2))
self.assert_sane(M)
self.assertTrue(Cv.shape == (2, ))
self.assert_sane(Cv, nonzero=False)
nv = plant.num_velocities()
vd_d = np.zeros(nv)
tau = plant.CalcInverseDynamics(
context, vd_d, MultibodyForces(plant))
self.assertEqual(tau.shape, (2,))
self.assert_sane(tau, nonzero=False)
# - Existence checks.
# Gravity leads to non-zero potential energy.
potential_energy = plant.CalcPotentialEnergy(context)
numpy_compare.assert_float_not_equal(potential_energy, 0.)
plant.CalcConservativePower(context)
tau_g = plant.CalcGravityGeneralizedForces(context)
self.assertEqual(tau_g.shape, (nv,))
self.assert_sane(tau_g, nonzero=True)
B = plant.MakeActuationMatrix()
numpy_compare.assert_float_equal(B, np.array([[0.], [1.]]))
forces = MultibodyForces(plant=plant)
plant.CalcForceElementsContribution(context=context, forces=forces)
# Test generalized forces.
# N.B. Cannot use `ndarray[object]` to reference existing C arrays
# (#8116).
if T == float:
forces.mutable_generalized_forces()[:] = 1
np.testing.assert_equal(forces.generalized_forces(), 1)
forces.SetZero()
np.testing.assert_equal(forces.generalized_forces(), 0)
# Test body force accessors and mutators.
link2 = plant.GetBodyByName("Link2")
self.assertIsInstance(
link2.GetForceInWorld(context, forces), SpatialForce)
forces.SetZero()
F_expected = np.array([1., 2., 3., 4., 5., 6.])
link2.AddInForceInWorld(
context, F_Bo_W=SpatialForce(F=F_expected), forces=forces)
coeff = numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs())
numpy_compare.assert_float_equal(coeff, F_expected)
link2.AddInForce(
context, p_BP_E=[0, 0, 0], F_Bp_E=SpatialForce(F=F_expected),
frame_E=plant.world_frame(), forces=forces)
# Also check accumulation.
np.testing.assert_equal(numpy_compare.to_float(
link2.GetForceInWorld(context, forces).get_coeffs()),
2 * F_expected)
def assert_sane(self, x, nonzero=True):
self.assertTrue(np.all(np.isfinite(numpy_compare.to_float(x))))
if nonzero:
numpy_compare.assert_float_not_equal(x, 0.)