def test_pose_bundle(self):
num_poses = 7
bundle = PoseBundle(num_poses)
# - Accessors.
self.assertEqual(bundle.get_num_poses(), num_poses)
with catch_drake_warnings(expected_count=1):
self.assertTrue(isinstance(bundle.get_pose(0), Isometry3))
self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform))
self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity))
# - Mutators.
kIndex = 5
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
with catch_drake_warnings(expected_count=2):
bundle.set_pose(kIndex, Isometry3(q, p))
self.assertTrue((bundle.get_pose(kIndex).matrix()
== Isometry3(q, p).matrix()).all())
pose = RigidTransform(quaternion=q, p=p)
bundle.set_transform(kIndex, pose)
self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34()
== pose.GetAsMatrix34()).all())
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
bundle.set_velocity(kIndex, frame_velocity)
vel_actual = bundle.get_velocity(kIndex).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
name = "Alice"
bundle.set_name(kIndex, name)
self.assertEqual(bundle.get_name(kIndex), name)
instance_id = 42 # Supply a random instance id.
bundle.set_model_instance_id(kIndex, instance_id)
self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_diagram_adder(self):
system = CustomDiagram(2, 3)
self.assertEqual(system.num_input_ports(), 2)
with catch_drake_warnings(expected_count=1):
system.get_num_input_ports()
self.assertEqual(system.get_input_port(0).size(), 3)
self.assertEqual(system.num_output_ports(), 1)
with catch_drake_warnings(expected_count=1):
system.get_num_output_ports()
self.assertEqual(system.get_output_port(0).size(), 3)
def test_pose_vector(self):
value = PoseVector()
self.assertTrue(isinstance(value, BasicVector))
self.assertTrue(isinstance(copy.copy(value), PoseVector))
self.assertTrue(isinstance(value.Clone(), PoseVector))
self.assertEqual(value.size(), PoseVector.kSize)
# - Accessors.
with catch_drake_warnings(expected_count=1):
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_transform(), RigidTransform))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - Value.
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = RigidTransform(quaternion=q, p=p)
value.set_translation(p)
value.set_rotation(q)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz))
vector_expected = np.hstack((p, q.wxyz()))
vector_actual = value.get_value()
self.assertTrue(np.allclose(vector_actual, vector_expected))
# - Fully-parameterized constructor.
value1 = PoseVector(rotation=q, translation=p)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value1.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# Test mutation via RigidTransform
p2 = [10, 20, 30]
q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8]))
X2_expected = RigidTransform(quaternion=q2, p=p2)
value.set_transform(X2_expected)
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
def test_simulator_ctor(self):
# Tests a simple simulation for supported scalar types.
for T in [float, AutoDiffXd]:
# Create simple system.
system = ConstantVectorSource_[T]([1.])
def check_output(context):
# Check number of output ports and value for a given context.
output = system.AllocateOutput()
self.assertEqual(output.num_ports(), 1)
system.CalcOutput(context, output)
if T == float:
value = output.get_vector_data(0).get_value()
self.assertTrue(np.allclose([1], value))
elif T == AutoDiffXd:
value = output.get_vector_data(0)._get_value_copy()
# TODO(eric.cousineau): Define `isfinite` ufunc, if
# possible, to use for `np.allclose`.
self.assertEqual(value.shape, (1,))
self.assertEqual(value[0], AutoDiffXd(1.))
else:
raise RuntimeError("Bad T: {}".format(T))
# Create simulator with basic constructor.
simulator = Simulator_[T](system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
simulator.set_publish_every_time_step(True)
self.assertTrue(simulator.get_context() is
simulator.get_mutable_context())
check_output(simulator.get_context())
simulator.AdvanceTo(1)
# Create simulator specifying context.
context = system.CreateDefaultContext()
with catch_drake_warnings(expected_count=1):
context.set_time(0.)
context.SetTime(0.)
with catch_drake_warnings(expected_count=1):
context.set_accuracy(1e-4)
context.SetAccuracy(1e-4)
self.assertEqual(context.get_accuracy(), 1e-4)
# @note `simulator` now owns `context`.
simulator = Simulator_[T](system, context)
self.assertTrue(simulator.get_context() is context)
check_output(context)
simulator.AdvanceTo(1)
def test_AddAngleBetweenVectorsConstraint(self):
na_A = np.array([0.2, -0.4, 0.9])
nb_B = np.array([1.4, -0.1, 1.8])
angle_lower = 0.2 * math.pi
angle_upper = 0.2 * math.pi
self.ik_two_bodies.AddAngleBetweenVectorsConstraint(
frameA=self.body1_frame, na_A=na_A,
frameB=self.body2_frame, nb_B=nb_B,
angle_lower=angle_lower, angle_upper=angle_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
na_W = body1_rotmat.dot(na_A)
nb_W = body2_rotmat.dot(nb_B)
angle = math.acos(na_W.transpose().dot(nb_W) /
(np.linalg.norm(na_W) * np.linalg.norm(nb_W)))
self.assertLess(math.fabs(angle - angle_lower), 1E-6)
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
def test_mock_lcm_induce_subscriber_callback_deprecated(self):
dut = DrakeMockLcm()
dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler)
with catch_drake_warnings(expected_count=1):
dut.InduceSubscriberCallback(
channel="TEST_CHANNEL", buffer=self.quat.encode())
self.assertEqual(self.count, 1)
def test_AddPositionConstraint(self):
p_BQ = np.array([0.2, 0.3, 0.5])
p_AQ_lower = np.array([-0.1, -0.2, -0.3])
p_AQ_upper = np.array([-0.05, -0.12, -0.28])
self.ik_two_bodies.AddPositionConstraint(
frameB=self.body1_frame, p_BQ=p_BQ,
frameA=self.body2_frame,
p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_AQ = body2_rotmat.transpose().dot(
body1_rotmat.dot(p_BQ) + body1_pos - body2_pos)
self.assertTrue(np.greater(p_AQ, p_AQ_lower -
1E-6 * np.ones((3, 1))).all())
self.assertTrue(np.less(p_AQ, p_AQ_upper +
1E-6 * np.ones((3, 1))).all())
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(self.q), q_val))
def test_deprecated_protected_aliases(self):
"""Tests a subset of protected aliases, pursuant to #9651."""
class OldSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
# Check a non-overridable method
with catch_drake_warnings(expected_count=1):
self._DeclareVectorInputPort("x", BasicVector(1))
def _DoPublish(self, context, events):
self.called_publish = True
# Ensure old overrides are still used
system = OldSystem()
context = system.CreateDefaultContext()
with catch_drake_warnings(expected_count=1):
system.Publish(context)
self.assertTrue(system.called_publish)
# Ensure documentation doesn't duplicate stuff.
with catch_drake_warnings(expected_count=1):
self.assertIn("deprecated", LeafSystem._DoPublish.__doc__)
# This will warn both on (a) calling the method and (b) on the
# invocation of the override.
with catch_drake_warnings(expected_count=2):
LeafSystem._DoPublish(system, context, [])
class AccidentallyBothSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_old_publish = False
self.called_new_publish = False
def DoPublish(self, context, events):
self.called_new_publish = True
def _DoPublish(self, context, events):
self.called_old_publish = True
system = AccidentallyBothSystem()
context = system.CreateDefaultContext()
# This will trigger no deprecations, as the newer publish is called.
system.Publish(context)
self.assertTrue(system.called_new_publish)
self.assertFalse(system.called_old_publish)
def test_deprecated_parsing(self):
sdf_file = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
with catch_drake_warnings(expected_count=1):
result = AddModelFromSdfFile(plant=plant, file_name=sdf_file)
self.assertIsInstance(result, ModelInstanceIndex)
def test_ipopt_solver_deprecated(self):
# This serves as the (only) unit test coverage for the deprecated
# SolverBase method that writes the solution back into the program.
prog, x, x_expected = self._make_prog()
solver = IpoptSolver()
with catch_drake_warnings(expected_count=2):
result = solver.Solve(prog)
solution = prog.GetSolution(x)
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(solution, x_expected))
def test_lcm(self):
dut = DrakeLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
# Quickly start and stop the receiving thread.
with catch_drake_warnings(expected_count=2):
dut.StartReceiveThread()
dut.StopReceiveThread()
# Test virtual function names.
dut.Publish
dut.HandleSubscriptions
def DoHasDirectFeedthrough(self, input_port, output_port):
# Test inputs.
test.assertIn(input_port, [0, 1])
test.assertEqual(output_port, 0)
# Call base method to ensure we do not get recursion.
with catch_drake_warnings(expected_count=1):
base_return = LeafSystem.DoHasDirectFeedthrough(
self, input_port, output_port)
test.assertTrue(base_return is None)
# Return custom methods.
self.called_feedthrough = True
return False
def test_deprecated_tree_api(self):
plant = MultibodyPlant()
plant.Finalize()
with catch_drake_warnings() as w:
num_expected_warnings = [0]
def expect_new_warning(msg_part):
num_expected_warnings[0] += 1
self.assertEqual(len(w), num_expected_warnings[0])
self.assertIn(msg_part, str(w[-1].message))
tree = plant.tree()
expect_new_warning("`tree()`")
MobilizerIndex(0)
expect_new_warning("`MobilizerIndex`")
BodyNodeIndex(0)
expect_new_warning("`BodyNodeIndex`")
MultibodyForces(model=tree)
expect_new_warning("`MultibodyForces(plant)`")
element = plant.world_body()
self.assertIsInstance(element.get_parent_tree(), MultibodyTree)
expect_new_warning("`get_parent_tree()`")
# Check old spellings (no deprecation warnings).
self.check_old_spelling_exists(tree.CalcRelativeTransform)
self.check_old_spelling_exists(tree.CalcPointsPositions)
self.check_old_spelling_exists(
tree.CalcFrameGeometricJacobianExpressedInWorld)
self.check_old_spelling_exists(tree.EvalBodyPoseInWorld)
self.check_old_spelling_exists(tree.SetFreeBodyPoseOrThrow)
self.check_old_spelling_exists(tree.SetFreeBodySpatialVelocityOrThrow)
self.check_old_spelling_exists(tree.EvalBodySpatialVelocityInWorld)
self.check_old_spelling_exists(tree.GetPositionsFromArray)
self.check_old_spelling_exists(tree.GetVelocitiesFromArray)
self.check_old_spelling_exists(tree.CalcMassMatrixViaInverseDynamics)
self.check_old_spelling_exists(tree.CalcBiasTerm)
self.check_old_spelling_exists(tree.CalcInverseDynamics)
self.check_old_spelling_exists(tree.num_frames)
self.check_old_spelling_exists(tree.get_body)
self.check_old_spelling_exists(tree.get_joint)
self.check_old_spelling_exists(tree.get_joint_actuator)
self.check_old_spelling_exists(tree.get_frame)
self.check_old_spelling_exists(tree.GetModelInstanceName)
context = plant.CreateDefaultContext()
# All body poses.
X_WB, = tree.CalcAllBodyPosesInWorld(context)
self.assertIsInstance(X_WB, Isometry3)
v_WB, = tree.CalcAllBodySpatialVelocitiesInWorld(context)
self.assertIsInstance(v_WB, SpatialVelocity)
def test_eval_binding(self):
qp = TestQP()
prog = qp.prog
x = qp.x
x_expected = np.array([1., 1.])
costs = qp.costs
cost_values_expected = [2., 1.]
constraints = qp.constraints
constraint_values_expected = [1., 1., 2., 3.]
with catch_drake_warnings(action='ignore'):
prog.Solve()
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))
enum = zip(constraints, constraint_values_expected)
for (constraint, value_expected) in enum:
value = prog.EvalBindingAtSolution(constraint)
self.assertTrue(np.allclose(value, value_expected))
enum = zip(costs, cost_values_expected)
for (cost, value_expected) in enum:
value = prog.EvalBindingAtSolution(cost)
self.assertTrue(np.allclose(value, value_expected))
# Existence check for non-autodiff versions.
self.assertIsInstance(
prog.EvalBinding(costs[0], x_expected), np.ndarray)
self.assertIsInstance(
prog.EvalBindings(prog.GetAllConstraints(), x_expected),
np.ndarray)
# Existence check for autodiff versions.
self.assertIsInstance(
jacobian(partial(prog.EvalBinding, costs[0]), x_expected),
np.ndarray)
self.assertIsInstance(
jacobian(partial(prog.EvalBindings, prog.GetAllConstraints()),
x_expected),
np.ndarray)
# Bindings for `Eval`.
x_list = (float(1.), AutoDiffXd(1.), sym.Variable("x"))
T_y_list = (float, AutoDiffXd, sym.Expression)
evaluator = costs[0].evaluator()
for x_i, T_y_i in zip(x_list, T_y_list):
y_i = evaluator.Eval(x=[x_i, x_i])
self.assertIsInstance(y_i[0], T_y_i)
def test_AddGazeTargetConstraint(self):
p_AS = np.array([0.1, 0.2, 0.3])
n_A = np.array([0.3, 0.5, 1.2])
p_BT = np.array([1.1, 0.2, 1.5])
cone_half_angle = 0.2 * math.pi
self.ik_two_bodies.AddGazeTargetConstraint(
frameA=self.body1_frame, p_AS=p_AS, n_A=n_A,
frameB=self.body2_frame, p_BT=p_BT,
cone_half_angle=cone_half_angle)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6)
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body1_pos = self._body1_xyz(q_val)
body2_quat = self._body2_quat(q_val)
body2_pos = self._body2_xyz(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
p_WS = body1_pos + body1_rotmat.dot(p_AS)
p_WT = body2_pos + body2_rotmat.dot(p_BT)
p_ST_W = p_WT - p_WS
n_W = body1_rotmat.dot(n_A)
self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm(
p_ST_W) * np.linalg.norm(n_W) *
math.cos(cone_half_angle) - 1E-6)
def test_sos(self):
# Find a,b,c,d subject to
# a(0) + a(1)*x,
# b(0) + 2*b(1)*x + b(2)*x^2 is SOS,
# c(0)*x^2 + 2*c(1)*x*y + c(2)*y^2 is SOS,
# d(0)*x^2 is SOS.
# d(1)*x^2 is SOS.
prog = mp.MathematicalProgram()
x = prog.NewIndeterminates(1, "x")
poly = prog.NewFreePolynomial(sym.Variables(x), 1)
(poly, binding) = prog.NewSosPolynomial(
indeterminates=sym.Variables(x), degree=2)
y = prog.NewIndeterminates(1, "y")
(poly, binding) = prog.NewSosPolynomial(
monomial_basis=(sym.Monomial(x[0]), sym.Monomial(y[0])))
d = prog.NewContinuousVariables(2, "d")
prog.AddSosConstraint(d[0]*x.dot(x))
prog.AddSosConstraint(d[1]*x.dot(x), [sym.Monomial(x[0])])
result = mp.Solve(prog)
self.assertTrue(result.is_success())
# Test SubstituteSolution(sym.Expression)
with catch_drake_warnings(action='ignore'):
prog.Solve()
# TODO(eric.cousineau): Expose `SymbolicTestCase` so that other
# tests can use the assertion utilities.
self.assertEqual(
prog.SubstituteSolution(d[0] + d[1]).Evaluate(),
prog.GetSolution(d[0]) + prog.GetSolution(d[1]))
# Test SubstituteSolution(sym.Polynomial)
poly = d[0]*x.dot(x)
poly_sub_actual = prog.SubstituteSolution(
sym.Polynomial(poly, sym.Variables(x)))
poly_sub_expected = sym.Polynomial(
prog.SubstituteSolution(d[0])*x.dot(x), sym.Variables(x))
# TODO(soonho): At present, these must be converted to `Expression`
# to compare, because as `Polynomial`s the comparison fails with
# `0*x(0)^2` != `0`, which indicates that simplification is not
# happening somewhere.
self.assertTrue(
poly_sub_actual.ToExpression().EqualTo(
poly_sub_expected.ToExpression()),
"{} != {}".format(poly_sub_actual, poly_sub_expected))
def _do_test_direct_transcription(self, use_deprecated_solve):
# Integrator.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1)
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant, context, num_time_samples=21)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dirtran.time()
dt = dirtran.fixed_timestep()
x = dirtran.state()
x2 = dirtran.state(2)
x0 = dirtran.initial_state()
xf = dirtran.final_state()
u = dirtran.input()
u2 = dirtran.input(2)
dirtran.AddRunningCost(x.dot(x))
dirtran.AddConstraintToAllKnotPoints(u[0] == 0)
dirtran.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dirtran.SetInitialTrajectory(initial_u, initial_x)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=6):
dirtran.Solve()
times = dirtran.GetSampleTimes()
inputs = dirtran.GetInputSamples()
states = dirtran.GetStateSamples()
input_traj = dirtran.ReconstructInputTrajectory()
state_traj = dirtran.ReconstructStateTrajectory()
else:
result = mp.Solve(dirtran)
times = dirtran.GetSampleTimes(result)
inputs = dirtran.GetInputSamples(result)
states = dirtran.GetStateSamples(result)
input_traj = dirtran.ReconstructInputTrajectory(result)
state_traj = dirtran.ReconstructStateTrajectory(result)
def test_abstract_io_port(self):
test = self
# N.B. Since this has trivial operations, we can test all scalar types.
for T in [float, AutoDiffXd, Expression]:
default_value = ("default", T(0.))
expected_input_value = ("input", T(np.pi))
expected_output_value = ("output", 2*T(np.pi))
class CustomAbstractSystem(LeafSystem_[T]):
def __init__(self):
LeafSystem_[T].__init__(self)
self.input_port = self.DeclareAbstractInputPort(
"in", AbstractValue.Make(default_value))
self.output_port = self.DeclareAbstractOutputPort(
"out",
lambda: AbstractValue.Make(default_value),
self.DoCalcAbstractOutput,
prerequisites_of_calc=set([
self.input_port.ticket()]))
def DoCalcAbstractOutput(self, context, y_data):
input_value = self.EvalAbstractInput(
context, 0).get_value()
# The allocator function will populate the output with
# the "input"
test.assertTupleEqual(input_value, expected_input_value)
y_data.set_value(expected_output_value)
test.assertTupleEqual(y_data.get_value(),
expected_output_value)
system = CustomAbstractSystem()
context = system.CreateDefaultContext()
self.assertEqual(context.num_input_ports(), 1)
context.FixInputPort(0, AbstractValue.Make(expected_input_value))
output = system.AllocateOutput()
self.assertEqual(output.num_ports(), 1)
with catch_drake_warnings(expected_count=1):
output.get_num_ports()
system.CalcOutput(context, output)
value = output.get_data(0)
self.assertEqual(value.get_value(), expected_output_value)
def test_AddMinimumDistanceConstraint(self):
ik = self.ik_two_bodies
W = self.plant.world_frame()
B1 = self.body1_frame
B2 = self.body2_frame
min_distance = 0.1
tol = 1e-2
radius1 = 0.1
radius2 = 0.2
ik.AddMinimumDistanceConstraint(minimal_distance=min_distance)
context = self.plant.CreateDefaultContext()
R_I = np.eye(3)
self.plant.SetFreeBodyPose(
context, B1.body(), Isometry3(R_I, [0, 0, 0.01]))
self.plant.SetFreeBodyPose(
context, B2.body(), Isometry3(R_I, [0, 0, -0.01]))
def get_min_distance_actual():
X = partial(self.plant.CalcRelativeTransform, context)
distance = norm(X(W, B1).translation() - X(W, B2).translation())
return distance - radius1 - radius2
self.assertLess(get_min_distance_actual(), min_distance - tol)
self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context))
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(ik.q())
self.plant.SetPositions(context, q_val)
self.assertGreater(get_min_distance_actual(), min_distance - tol)
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(
self.prog.GetSolution(ik.q()), q_val))
def test_AddOrientationConstraint(self):
theta_bound = 0.2 * math.pi
R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5))
R_BbarB = RotationMatrix(
quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3))
self.ik_two_bodies.AddOrientationConstraint(
frameAbar=self.body1_frame, R_AbarA=R_AbarA,
frameBbar=self.body2_frame, R_BbarB=R_BbarB,
theta_bound=theta_bound)
result = mp.Solve(self.prog)
self.assertTrue(result.is_success())
q_val = result.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6)
with catch_drake_warnings(expected_count=2):
self.assertEqual(
self.prog.Solve(), mp.SolutionResult.kSolutionFound)
q_val = self.prog.GetSolution(self.q)
body1_quat = self._body1_quat(q_val)
body2_quat = self._body2_quat(q_val)
body1_rotmat = Quaternion(body1_quat).rotation()
body2_rotmat = Quaternion(body2_quat).rotation()
R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat)
R_AB = R_AbarA.matrix().transpose().dot(
R_AbarBbar.dot(R_BbarB.matrix()))
self.assertGreater(R_AB.trace(),
1 + 2 * math.cos(theta_bound) - 1E-6)
def _fix_adder_inputs(self, context):
self.assertEqual(context.num_input_ports(), 2)
with catch_drake_warnings(expected_count=1):
context.get_num_input_ports()
context.FixInputPort(0, BasicVector([1, 2, 3]))
context.FixInputPort(1, BasicVector([4, 5, 6]))
def test_multibody_plant_api_via_parsing(self):
# TODO(eric.cousineau): Decouple this when construction can be done
# without parsing.
# This a subset of `multibody_plant_sdf_parser_test.cc`.
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(file_name)
self.assertIsInstance(model_instance, ModelInstanceIndex)
plant.Finalize()
benchmark = MakeAcrobotPlant(AcrobotParameters(), True)
self.assertEqual(plant.num_bodies(), benchmark.num_bodies())
self.assertEqual(plant.num_joints(), benchmark.num_joints())
self.assertEqual(plant.num_actuators(), benchmark.num_actuators())
self.assertEqual(
plant.num_model_instances(), benchmark.num_model_instances() + 1)
self.assertEqual(plant.num_positions(), benchmark.num_positions())
self.assertEqual(
plant.num_positions(model_instance=model_instance),
benchmark.num_positions())
self.assertEqual(
plant.num_velocities(), benchmark.num_velocities())
self.assertEqual(
plant.num_multibody_states(), benchmark.num_multibody_states())
self.assertEqual(
plant.num_actuated_dofs(), benchmark.num_actuated_dofs())
self.assertTrue(plant.is_finalized())
self.assertTrue(plant.HasBodyNamed(name="Link1"))
self.assertTrue(plant.HasBodyNamed(
name="Link1", model_instance=model_instance))
self.assertTrue(plant.HasJointNamed(name="ShoulderJoint"))
self.assertTrue(plant.HasJointNamed(
name="ShoulderJoint", model_instance=model_instance))
shoulder = plant.GetJointByName(name="ShoulderJoint")
self._test_joint_api(shoulder)
np.testing.assert_array_equal(
shoulder.position_lower_limits(), [-np.inf])
np.testing.assert_array_equal(
shoulder.position_upper_limits(), [np.inf])
self.assertIs(shoulder, plant.GetJointByName(
name="ShoulderJoint", model_instance=model_instance))
self._test_joint_actuator_api(
plant.GetJointActuatorByName(name="ElbowJoint"))
self._test_body_api(plant.GetBodyByName(name="Link1"))
self.assertIs(
plant.GetBodyByName(name="Link1"),
plant.GetBodyByName(name="Link1", model_instance=model_instance))
self.assertEqual(len(plant.GetBodyIndices(model_instance)), 2)
self._test_frame_api(plant.GetFrameByName(name="Link1"))
self.assertIs(
plant.GetFrameByName(name="Link1"),
plant.GetFrameByName(name="Link1", model_instance=model_instance))
self.assertEqual(
model_instance, plant.GetModelInstanceByName(name="acrobot"))
self.assertIsInstance(
plant.get_actuation_input_port(), InputPort)
self.assertIsInstance(
plant.get_state_output_port(), OutputPort)
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=2):
plant.get_continuous_state_output_port()
plant.get_continuous_state_output_port(model_instance)
self.assertIsInstance(
plant.get_contact_results_output_port(), OutputPort)
self.assertIsInstance(plant.num_frames(), int)
self.assertIsInstance(plant.get_body(body_index=BodyIndex(0)), Body)
self.assertIs(shoulder, plant.get_joint(joint_index=JointIndex(0)))
self.assertIsInstance(plant.get_joint_actuator(
actuator_index=JointActuatorIndex(0)), JointActuator)
self.assertIsInstance(
plant.get_frame(frame_index=FrameIndex(0)), Frame)
self.assertEqual("acrobot", plant.GetModelInstanceName(
model_instance=model_instance))
from six import text_type as unicode
import numpy as np
from pydrake.common import FindResourceOrThrow
from pydrake.common.eigen_geometry import Isometry3
from pydrake.common.test_utilities.deprecation import catch_drake_warnings
from pydrake.systems.framework import InputPort, OutputPort
from pydrake.math import RigidTransform, RollPitchYaw
from pydrake.systems.lcm import LcmPublisherSystem
# Deprecated modules.
# N.B. Place `with` afterwards to avoid needing to place `#noqa` on other
# modules. Additionally, assert length of warnings here rather than in the test
# so that we do not have to worry about whether a module has already been
# loaded.
with catch_drake_warnings(expected_count=3):
from pydrake.multibody.multibody_tree import (
BodyNodeIndex,
MobilizerIndex,
MultibodyTree,
)
from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile
def get_index_class(cls):
# Maps a class to its corresponding index class, accommdating inheritance.
class_to_index_class_map = {
Body: BodyIndex,
ForceElement: ForceElementIndex,
Frame: FrameIndex,
Joint: JointIndex,
def test_mock_lcm_get_last_published_message_deprecated(self):
dut = DrakeMockLcm()
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
with catch_drake_warnings(expected_count=1):
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, self.quat.encode())
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
# Check a non-overridable method
with catch_drake_warnings(expected_count=1):
self._DeclareVectorInputPort("x", BasicVector(1))
def test_context_api(self):
# Capture miscellaneous functions not yet tested.
model_value = AbstractValue.Make("Hello")
model_vector = BasicVector([1., 2.])
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareContinuousState(1)
self.DeclareDiscreteState(2)
self.DeclareAbstractState(model_value.Clone())
self.DeclareAbstractParameter(model_value.Clone())
self.DeclareNumericParameter(model_vector.Clone())
system = TrivialSystem()
context = system.CreateDefaultContext()
self.assertTrue(
context.get_state() is context.get_mutable_state())
self.assertEqual(context.num_continuous_states(), 1)
self.assertTrue(
context.get_continuous_state_vector() is
context.get_mutable_continuous_state_vector())
self.assertEqual(context.num_discrete_state_groups(), 1)
with catch_drake_warnings(expected_count=1):
context.get_num_discrete_state_groups()
self.assertTrue(
context.get_discrete_state_vector() is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state().get_vector(0))
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state().get_vector(0))
self.assertEqual(context.num_abstract_states(), 1)
with catch_drake_warnings(expected_count=1):
context.get_num_abstract_states()
self.assertTrue(
context.get_abstract_state() is
context.get_mutable_abstract_state())
self.assertTrue(
context.get_abstract_state(0) is
context.get_mutable_abstract_state(0))
self.assertEqual(
context.get_abstract_state(0).get_value(), model_value.get_value())
# Check abstract state API (also test AbstractValues).
values = context.get_abstract_state()
self.assertEqual(values.size(), 1)
self.assertEqual(
values.get_value(0).get_value(), model_value.get_value())
self.assertEqual(
values.get_mutable_value(0).get_value(), model_value.get_value())
values.SetFrom(values.Clone())
with catch_drake_warnings(expected_count=1):
values.CopyFrom(values.Clone())
# Check parameter accessors.
self.assertEqual(system.num_abstract_parameters(), 1)
self.assertEqual(
context.get_abstract_parameter(index=0).get_value(),
model_value.get_value())
self.assertEqual(system.num_numeric_parameter_groups(), 1)
np.testing.assert_equal(
context.get_numeric_parameter(index=0).get_value(),
model_vector.get_value())
# Check diagram context accessors.
builder = DiagramBuilder()
builder.AddSystem(system)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Existence check.
self.assertIsNot(
diagram.GetMutableSubsystemState(system, context), None)
subcontext = diagram.GetMutableSubsystemContext(system, context)
self.assertIsNot(subcontext, None)
self.assertIs(
diagram.GetSubsystemContext(system, context), subcontext)
def _do_test_direct_collocation(self, use_deprecated_solve):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3*21, 0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=1):
dircol.Solve()
result = None
else:
result = mp.Solve(dircol)
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=5):
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
else:
times = dircol.GetSampleTimes(result)
inputs = dircol.GetInputSamples(result)
states = dircol.GetStateSamples(result)
input_traj = dircol.ReconstructInputTrajectory(result)
state_traj = dircol.ReconstructStateTrajectory(result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def __init__(self):
LeafSystem_[float].__init__(self)
with catch_drake_warnings(expected_count=1):
self.DeclareAbstractInputPort("in")
self.DeclareVectorOutputPort("out", BasicVector(1), self._Out)
def test_bindings(self):
qp = TestQP()
prog = qp.prog
x = qp.x
self.assertEqual(prog.FindDecisionVariableIndices(vars=[x[0], x[1]]),
[0, 1])
for binding in prog.GetAllCosts():
self.assertIsInstance(binding.evaluator(), mp.Cost)
for binding in prog.GetLinearConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
for binding in prog.GetAllConstraints():
self.assertIsInstance(binding.evaluator(), mp.Constraint)
self.assertTrue(prog.linear_costs())
for (i, binding) in enumerate(prog.linear_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.a(), np.ones((1, 2))))
self.assertTrue(prog.quadratic_costs())
for (i, binding) in enumerate(prog.quadratic_costs()):
cost = binding.evaluator()
self.assertTrue(np.allclose(cost.Q(), np.eye(2)))
self.assertTrue(np.allclose(cost.b(), np.zeros(2)))
self.assertTrue(prog.bounding_box_constraints())
for (i, binding) in enumerate(prog.bounding_box_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[i]))
num_constraints = constraint.num_constraints()
if num_constraints == 1:
self.assertEqual(constraint.A(), 1)
self.assertEqual(constraint.lower_bound(), 1)
self.assertEqual(constraint.upper_bound(), np.inf)
else:
self.assertTrue(np.allclose(constraint.A(), np.eye(2)))
self.assertTrue(np.allclose(constraint.lower_bound(),
[1, -np.inf]))
self.assertTrue(np.allclose(constraint.upper_bound(),
[np.inf, 2]))
self.assertTrue(prog.linear_constraints())
for (i, binding) in enumerate(prog.linear_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [3, -1]))
self.assertTrue(constraint.lower_bound(), -2)
self.assertTrue(constraint.upper_bound(), np.inf)
self.assertTrue(prog.linear_equality_constraints())
for (i, binding) in enumerate(prog.linear_equality_constraints()):
constraint = binding.evaluator()
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[0]),
prog.FindDecisionVariableIndex(var=x[0]))
self.assertEqual(
prog.FindDecisionVariableIndex(var=binding.variables()[1]),
prog.FindDecisionVariableIndex(var=x[1]))
self.assertTrue(np.allclose(constraint.A(), [1, 2]))
self.assertTrue(constraint.lower_bound(), 3)
self.assertTrue(constraint.upper_bound(), 3)
result = mp.Solve(prog)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
# Test deprecated method.
with catch_drake_warnings(expected_count=1):
c = binding.constraint()
def test_multibody_gravity_default(self):
plant = MultibodyPlant()
# Smoke test of deprecated methods.
with catch_drake_warnings(expected_count=1):
plant.AddForceElement(UniformGravityFieldElement())
plant.Finalize()
def test_frame_pose_vector_api(self):
mut.FramePoseVector()
with catch_drake_warnings(expected_count=1):
mut.FramePoseVector(source_id=mut.SourceId.get_new_id(),
ids=[mut.FrameId.get_new_id()])