def test_context_fix_input_port(self):
# WARNING: This is not the recommend workflow; instead, use
# `InputPort.FixValue` instead. This is here just for testing /
# coverage purposes.
dt = 0.1 # Arbitrary.
system_vec = ZeroOrderHold(period_sec=dt, vector_size=1)
context_vec = system_vec.CreateDefaultContext()
context_vec.FixInputPort(index=0, data=[0.])
context_vec.FixInputPort(index=0, vec=BasicVector([0.]))
# Test abstract.
model_value = AbstractValue.Make("Hello")
system_abstract = ZeroOrderHold(
period_sec=dt, abstract_model_value=model_value.Clone())
context_abstract = system_abstract.CreateDefaultContext()
context_abstract.FixInputPort(index=0, value=model_value.Clone())
def test_linear_affine_system(self):
# Just make sure linear system is spelled correctly.
A = np.identity(2)
B = np.array([[0], [1]])
f0 = np.array([[0], [0]])
C = np.array([[0, 1]])
D = [0]
y0 = [0]
system = LinearSystem(A, B, C, D)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context
.get_mutable_continuous_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), 0.)
Co = ControllabilityMatrix(system)
self.assertEqual(Co.shape, (2, 2))
self.assertFalse(IsControllable(system))
self.assertFalse(IsControllable(system, 1e-6))
Ob = ObservabilityMatrix(system)
self.assertEqual(Ob.shape, (2, 2))
self.assertFalse(IsObservable(system))
system = AffineSystem(A, B, f0, C, D, y0, .1)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_discrete_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), .1)
context.FixInputPort(0, BasicVector([0]))
linearized = Linearize(system, context)
self.assertTrue((linearized.A() == A).all())
taylor = FirstOrderTaylorApproximation(system, context)
self.assertTrue((taylor.y0() == y0).all())
def __init__(self, actuators, motor_gain=None):
LeafSystem.__init__(self)
self.num_actuators = len(actuators)
self.actuators_init = np.ones(self.num_actuators) * 0.0
if motor_gain is None:
motor_gain = np.ones(self.num_actuators)
assert motor_gain.shape == (self.num_actuators, )
self.motor_gain = motor_gain
self.robot_command_port_index = self._DeclareInputPort(
'robot_command_port', PortDataType.kVectorValued,
self.num_actuators).get_index()
self.desired_effort_port_index = self._DeclareVectorOutputPort(
BasicVector(self.num_actuators), self.OutputActuation).get_index()
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_discrete = False
# Ensure we have desired overloads.
self._DeclarePeriodicPublish(0.1)
self._DeclarePeriodicPublish(0.1, 0)
self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.)
self._DeclarePeriodicDiscreteUpdate(
period_sec=0.1, offset_sec=0.)
self._DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareVectorOutputPort(BasicVector(1), noop)
def test_drake_visualizer_api(self):
tree = self._make_tree()
lcm = DrakeMockLcm()
# Test for existence.
viz = mut.DrakeVisualizer(tree=tree, lcm=lcm, enable_playback=True)
viz.set_publish_period(period=0.01)
# - Force publish to ensure we have enough knot points.
context = viz.CreateDefaultContext()
x0 = np.zeros(tree.get_num_positions() + tree.get_num_velocities())
context.FixInputPort(0, BasicVector(x0))
context.set_time(0.)
viz.Publish(context)
# Use a small time period, since it uses realtime playback.
context.set_time(0.01)
viz.Publish(context)
viz.ReplayCachedSimulation()
def __init__(self, tree):
lcm = DrakeLcm()
self.tree = tree
self.visualizer = DrakeVisualizer(tree=self.tree,
lcm=lcm,
enable_playback=True)
self.x = np.concatenate([
robot.getZeroConfiguration(),
np.zeros(tree.get_num_velocities())
])
# Workaround for the fact that PublishLoadRobot is not public.
# Ultimately that should be fixed.
sim = Simulator(self.visualizer)
context = sim.get_mutable_context()
context.FixInputPort(0, BasicVector(self.x))
sim.Initialize()
def test_vector_input_port_fix(self, T):
np_zeros = np.array([0.])
model_value = AbstractValue.Make(BasicVector(np_zeros))
system = PassThrough_[T](len(np_zeros))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a scalar.
input_port.FixValue(context, T(1.))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
numpy_compare.assert_equal(value, np.array([T(1.)]))
# Fix to an ndarray.
input_port.FixValue(context, np.array([T(2.)]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
numpy_compare.assert_equal(value, np.array([T(2.)]))
# Fix to a BasicVector.
input_port.FixValue(context, BasicVector_[T]([3.]))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
numpy_compare.assert_equal(value, np.array([T(3.)]))
# Fix to a type-erased BasicVector.
input_port.FixValue(context, AbstractValue.Make(BasicVector_[T]([4.])))
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
numpy_compare.assert_equal(value, np.array([T(4.)]))
# Fix to wrong-sized vector.
with self.assertRaises(RuntimeError):
input_port.FixValue(context, np.array([0., 1.]))
with self.assertRaises(RuntimeError):
input_port.FixValue(
context, AbstractValue.Make(BasicVector_[T]([0., 1.])))
# Fix to a non-vector.
with self.assertRaises(TypeError):
# A TypeError occurs when pybind Value.set_value cannot match any
# overload for how to assign the argument into the erased storage.
input_port.FixValue(context, "string")
with self.assertRaises(RuntimeError):
# A RuntimeError occurs when the Context detects that the
# type-erased Value objects are incompatible.
input_port.FixValue(context, AbstractValue.Make("string"))
def test_basic_vector_double(self):
# Ensure that we can get vectors templated on double by reference.
init = [1., 2, 3]
value_data = BasicVector(init)
value = value_data.get_mutable_value()
# TODO(eric.cousineau): Determine if there is a way to extract the
# pointer referred to by the buffer (e.g. `value.data`).
value[:] += 1
expected = [2., 3, 4]
self.assertTrue(np.allclose(value, expected))
self.assertTrue(np.allclose(value_data.get_value(), expected))
self.assertTrue(np.allclose(value_data.get_mutable_value(), expected))
expected = [5., 6, 7]
value_data.SetFromVector(expected)
self.assertTrue(np.allclose(value, expected))
self.assertTrue(np.allclose(value_data.get_value(), expected))
self.assertTrue(np.allclose(value_data.get_mutable_value(), expected))
def test_value_to_annotation(self):
test_vector = np.array([1., 2., 3.])
test_basic_vector = BasicVector(test_vector)
self.assert_annotations_equal(
value_to_annotation(test_vector.tolist()), VectorArg(3))
self.assert_annotations_equal(value_to_annotation(test_vector),
VectorArg(3))
self.assert_annotations_equal(value_to_annotation(test_basic_vector),
test_basic_vector)
self.assert_annotations_equal(
value_to_annotation(AbstractValue.Make(test_basic_vector)),
test_basic_vector)
self.assert_annotations_equal(value_to_annotation(1.), float)
self.assert_annotations_equal(value_to_annotation(np.array(1.)), float)
self.assert_annotations_equal(value_to_annotation("hello"), str)
self.assert_annotations_equal(
value_to_annotation(AbstractValue.Make("hello")), Value[str])
self.assert_annotations_equal(value_to_annotation(None), None)
def __init__(self, scene_graph):
LeafSystem.__init__(self)
assert scene_graph
mbp = MultibodyPlant(0.0)
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(
FindResource("models/glider/glider.urdf"))
mbp.Finalize()
self.source_id = mbp.get_source_id()
self.body_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[0])
self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[1])
self.DeclareVectorInputPort("state", BasicVector(7))
self.DeclareAbstractOutputPort(
"geometry_pose", lambda: AbstractValue.Make(FramePoseVector()),
self.OutputGeometryPose)
def __init__(self, rb_tree):
LeafSystem.__init__(self)
self.rb_tree = rb_tree
self.num_position = self.rb_tree.get_num_positions()
self.num_controlled_q_ = self.rb_tree.get_num_actuators()
self.n = 0
# Input Port
self.joint_state_results_port_index = self._DeclareInputPort(
'joint_state_results_port', PortDataType.kVectorValued,
self.num_position * 2).get_index()
# self.contact_results_port_index = self._DeclareAbstractInputPort('contact_results_port',
# AbstractValue.Make(ContactResults)).get_index()
# Output Port
self.joint_state_outport_index = self._DeclareVectorOutputPort(
'state_output_port', BasicVector(self.num_controlled_q_ * 2),
self._OutputRobotState).get_index()
def test_parameters_api(self):
def compare(actual, expected):
self.assertEquals(type(actual), type(expected))
if isinstance(actual, VectorBase):
self.assertTrue(
np.allclose(actual.get_value(), expected.get_value()))
else:
self.assertEquals(actual.get_value(), expected.get_value())
model_numeric = BasicVector([0.])
model_abstract = AbstractValue.Make("Hello")
params = Parameters(
numeric=[model_numeric.Clone()], abstract=[model_abstract.Clone()])
self.assertEquals(params.num_numeric_parameters(), 1)
self.assertEquals(params.num_abstract_parameters(), 1)
# Numeric.
compare(params.get_numeric_parameter(index=0), model_numeric)
compare(params.get_mutable_numeric_parameter(index=0), model_numeric)
# WARNING: This will invalidate old references!
params.set_numeric_parameters(params.get_numeric_parameters().Clone())
# Abstract.
compare(params.get_abstract_parameter(index=0), model_abstract)
compare(params.get_mutable_abstract_parameter(index=0), model_abstract)
# WARNING: This will invalidate old references!
params.set_abstract_parameters(
params.get_abstract_parameters().Clone())
# WARNING: This may invalidate old references!
params.SetFrom(copy.deepcopy(params))
# Test alternative constructors.
ctor_test = [
Parameters(),
Parameters(numeric=[model_numeric.Clone()]),
Parameters(abstract=[model_abstract.Clone()]),
Parameters(
numeric=[model_numeric.Clone()],
abstract=[model_abstract.Clone()]),
Parameters(vec=model_numeric.Clone()),
Parameters(value=model_abstract.Clone()),
]
def __init__(self, mbp, grab_period=0.1, symbol_logger=None):
LeafSystem.__init__(self)
self.mbp = mbp
# Our version of MBP context, which we'll modify
# in the publish method.
self.mbp_context = mbp.CreateDefaultContext()
# Object body names we care about
self.body_names = ["blue_box", "red_box"]
self.set_name('symbol_detection_system')
self.DeclarePeriodicPublish(grab_period, 0.0)
# Take robot state vector as input.
self.DeclareVectorInputPort(
"mbp_state_vector",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
self._symbol_logger = symbol_logger
def test_vector_output_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
source = ConstantVectorSource(np_value)
context = source.CreateDefaultContext()
output_port = source.get_output_port(0)
value = output_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = output_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = output_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def __init__(self, rbtree, kp, ki, kd):
LeafSystem.__init__(self)
self.rb_tree = rbtree
self.num_controlled_q_ = self.rb_tree.get_num_actuators()
self.kp = kp
self.ki = ki
self.kd = kd
self.input_index_state = self._DeclareInputPort(
'robot_state_port', PortDataType.kVectorValued,
self.num_controlled_q_ * 2).get_index()
self.input_index_desired_state = self._DeclareInputPort(
'state_ref_port', PortDataType.kVectorValued,
self.num_controlled_q_ * 2).get_index()
self.robot_command_port_index = self._DeclareVectorOutputPort(
BasicVector(self.num_controlled_q_),
self._OutputCommand).get_index()
def test_vector_input_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
system = PassThrough(len(np_value))
context = system.CreateDefaultContext()
system.get_input_port(0).FixValue(context, np_value)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = input_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_linear_quadratic_regulator(self):
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.identity(2)
D = np.array([[0], [0]])
double_integrator = LinearSystem(A, B, C, D)
Q = np.identity(2)
R = np.identity(1)
K_expected = np.array([[1, math.sqrt(3.)]])
S_expected = np.array([[math.sqrt(3), 1.], [1., math.sqrt(3)]])
(K, S) = LinearQuadraticRegulator(A, B, Q, R)
np.testing.assert_almost_equal(K, K_expected)
np.testing.assert_almost_equal(S, S_expected)
controller = LinearQuadraticRegulator(double_integrator, Q, R)
np.testing.assert_almost_equal(controller.D(), -K_expected)
context = double_integrator.CreateDefaultContext()
context.FixInputPort(0, BasicVector([0]))
controller = LinearQuadraticRegulator(double_integrator, context, Q, R)
np.testing.assert_almost_equal(controller.D(), -K_expected)
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
# Ensure we have desired overloads.
self._DeclarePeriodicPublish(0.1)
self._DeclarePeriodicPublish(0.1, 0)
self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.)
self._DeclarePeriodicDiscreteUpdate(
period_sec=0.1, offset_sec=0.)
self._DeclareInitializationEvent(
event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self._DeclareContinuousState(2)
self._DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareVectorOutputPort(BasicVector(1), noop)
def __init__(self, arms, limit, timestep=0.005):
LeafSystem.__init__(self)
self.limit = limit
self.timestep = timestep
self.arms = arms
self.target_input_ports = {}
self.setpoint_output_ports = {}
for arm in self.arms:
self.target_input_ports[arm] = {}
self.setpoint_output_ports[arm] = {}
self.target_input_ports[arm][
"position"] = self.DeclareVectorInputPort(
f"{arm}_target",
BasicVector(6),
)
self.target_input_ports[arm][
"width"] = self.DeclareVectorInputPort(
f"{arm}_width_target",
BasicVector(1),
)
if "left" in self.arms:
self.setpoint_output_ports["left"][
"position"] = self.DeclareVectorOutputPort(
f"left_setpoint", BasicVector(6), self.DoCalcLeftOutput)
self.setpoint_output_ports["left"][
"width"] = self.DeclareVectorOutputPort(
f"left_width_setpoint", BasicVector(1),
self.DoCalcLeftWidthOutput)
if "right" in self.arms:
self.setpoint_output_ports["right"][
"position"] = self.DeclareVectorOutputPort(
f"right_setpoint", BasicVector(6), self.DoCalcRightOutput)
self.setpoint_output_ports["right"][
"width"] = self.DeclareVectorOutputPort(
f"right_width_setpoint", BasicVector(1),
self.DoCalcRightWidthOutput)
self.current_states = {}
for arm in self.arms:
self.current_states[arm] = {}
self.current_states[arm]["position"] = self.DeclareDiscreteState(6)
self.current_states[arm]["width"] = self.DeclareDiscreteState(1)
self.DeclarePeriodicDiscreteUpdate(self.timestep)
def __init__(self, arm_info, timestep=0.0005):
LeafSystem.__init__(self)
self.set_name("low_level_hand_controller")
self.arm_info = arm_info
self.previous_error = {}
self.integral = {}
self.desired_input_ports = {}
self.estimated_input_ports = {}
self.body_positions_input_port = self.DeclareAbstractInputPort(
"body_positions", Value[List[RigidTransform]]())
for arm in self.arm_info:
self.desired_input_ports[arm] = self.DeclareVectorInputPort(
f"{arm}_desired", BasicVector(6))
self.previous_error[arm] = self.DeclareDiscreteState(6)
self.integral[arm] = self.DeclareDiscreteState(6)
self.DeclareAbstractOutputPort(
f"spatial_forces_vector",
lambda: Value[List[ExternallyAppliedSpatialForce]](),
self.DoCalcAbstractOutput)
self.kp = [100, 100, 100, 10000, 10000, 10000]
self.ki = [0, 0, 0, 1, 1, 1]
self.kd = [10, 10, 10, 10, 10, 10]
self.timestep = timestep
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_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)
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)
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())
# 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 __init__(self, num_inputs, size):
LeafSystem.__init__(self)
for i in range(num_inputs):
self.DeclareVectorInputPort("input{}".format(i), BasicVector(size))
self.DeclareVectorOutputPort("sum", BasicVector(size), self._calc_sum)
def _fix_adder_inputs(self, context):
self.assertEqual(context.num_input_ports(), 2)
context.FixInputPort(0, BasicVector([1, 2, 3]))
context.FixInputPort(1, BasicVector([4, 5, 6]))
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_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = np.array([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2) # Test the BasicVector overload.
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
def mytest(input, expected):
context.FixInputPort(0, BasicVector(input))
system.CalcOutput(context, output)
self.assertTrue(
np.allclose(
output.get_vector_data(0).CopyToVector(), expected))
def __init__(self, plant, contacts_per_frame, is_wbc=False):
LeafSystem.__init__(self)
self.plant = plant
self.contacts_per_frame = contacts_per_frame
self.is_wbc = is_wbc
self.upright_context = self.plant.CreateDefaultContext()
self.q_nom = self.plant.GetPositions(
self.upright_context) # Nominal upright pose
self.input_q_v_idx = self.DeclareVectorInputPort(
"q_v",
BasicVector(self.plant.num_positions() +
self.plant.num_velocities())).get_index()
self.output_tau_idx = self.DeclareVectorOutputPort(
"tau", BasicVector(Atlas.NUM_ACTUATED_DOF),
self.calcTorqueOutput).get_index()
if self.is_wbc:
com_dim = 3
self.x_size = 2 * com_dim
self.u_size = com_dim
self.input_r_des_idx = self.DeclareVectorInputPort(
"r_des", BasicVector(com_dim)).get_index()
self.input_rd_des_idx = self.DeclareVectorInputPort(
"rd_des", BasicVector(com_dim)).get_index()
self.input_rdd_des_idx = self.DeclareVectorInputPort(
"rdd_des", BasicVector(com_dim)).get_index()
self.input_q_des_idx = self.DeclareVectorInputPort(
"q_des", BasicVector(self.plant.num_positions())).get_index()
self.input_v_des_idx = self.DeclareVectorInputPort(
"v_des", BasicVector(self.plant.num_velocities())).get_index()
self.input_vd_des_idx = self.DeclareVectorInputPort(
"vd_des",
BasicVector(self.plant.num_velocities())).get_index()
Q = 1.0 * np.identity(self.x_size)
R = 0.1 * np.identity(self.u_size)
A = np.vstack([
np.hstack([0 * np.identity(com_dim),
1 * np.identity(com_dim)]),
np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)])
])
B_1 = np.vstack(
[0 * np.identity(com_dim), 1 * np.identity(com_dim)])
K, S = LinearQuadraticRegulator(A, B_1, Q, R)
def V_full(x, u, r, rd, rdd):
x_bar = x - np.concatenate([r, rd])
u_bar = u - rdd
'''
xd_bar = d(x - [r, rd].T)/dt
= xd - [rd, rdd].T
= Ax + Bu - [rd, rdd].T
'''
xd_bar = A.dot(x) + B_1.dot(u) - np.concatenate([rd, rdd])
return x_bar.T.dot(Q).dot(x_bar) + u_bar.T.dot(R).dot(
u_bar) + 2 * x_bar.T.dot(S).dot(xd_bar)
self.V_full = V_full
else:
# Only x, y coordinates of COM is considered
com_dim = 2
self.x_size = 2 * com_dim
self.u_size = com_dim
self.input_y_des_idx = self.DeclareVectorInputPort(
"y_des", BasicVector(zmp_state_size)).get_index()
''' Eq(1) '''
A = np.vstack([
np.hstack([0 * np.identity(com_dim),
1 * np.identity(com_dim)]),
np.hstack([0 * np.identity(com_dim), 0 * np.identity(com_dim)])
])
B_1 = np.vstack(
[0 * np.identity(com_dim), 1 * np.identity(com_dim)])
''' Eq(4) '''
C_2 = np.hstack([np.identity(2), np.zeros((2, 2))]) # C in Eq(2)
D = -z_com / Atlas.g * np.identity(zmp_state_size)
Q = 1.0 * np.identity(zmp_state_size)
''' Eq(6) '''
'''
y.T*Q*y
= (C*x+D*u)*Q*(C*x+D*u).T
= x.T*C.T*Q*C*x + u.T*D.T*Q*D*u + x.T*C.T*Q*D*u + u.T*D.T*Q*C*X
= .. + 2*x.T*C.T*Q*D*u
'''
K, S = LinearQuadraticRegulator(A, B_1,
C_2.T.dot(Q).dot(C_2),
D.T.dot(Q).dot(D),
C_2.T.dot(Q).dot(D))
# Use original formulation
def V_full(x, u,
y_des): # Assume constant z_com, we don't need tvLQR
y = C_2.dot(x) + D.dot(u)
def dJ_dx(x):
return x.T.dot(
S.T + S
) # https://math.stackexchange.com/questions/20694/vector-derivative-w-r-t-its-transpose-fracdaxdxt
y_bar = y - y_des
x_bar = x - np.concatenate(
[y_des, [0.0, 0.0]]) # FIXME: This doesn't seem right...
# FIXME: xd_bar should depend on yd_des
xd_bar = A.dot(x_bar) + B_1.dot(u)
return y_bar.T.dot(Q).dot(y_bar) + dJ_dx(x_bar).dot(xd_bar)
self.V_full = V_full
# Calculate values that don't depend on context
self.B_7 = self.plant.MakeActuationMatrix()
# From np.sort(np.nonzero(B_7)[0]) we know that indices 0-5 are the unactuated 6 DOF floating base and 6-35 are the actuated 30 DOF robot joints
self.v_idx_act = 6 # Start index of actuated joints in generalized velocities
self.B_a = self.B_7[self.v_idx_act:, :]
self.B_a_inv = np.linalg.inv(self.B_a)
# Sort joint effort limits to be the same order as tau in Eq(13)
self.sorted_max_efforts = np.array([
entry[1].effort
for entry in getJointLimitsSortedByActuator(self.plant)
])
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 __init__(self, num_inputs, size):
LeafSystem.__init__(self)
for i in xrange(num_inputs):
self._DeclareInputPort(PortDataType.kVectorValued, size)
self._DeclareVectorOutputPort(BasicVector(size), self._calc_sum)
