def test_vector_pass_through(self):
model_value = BasicVector([1., 2, 3])
system = PassThrough(model_value.size())
context = system.CreateDefaultContext()
context.FixInputPort(0, model_value)
output = system.AllocateOutput()
input_eval = system.EvalVectorInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
output_value = output.get_vector_data(0)
compare_value(self, output_value, model_value)
def test_basic_vector_double(self):
# Test constructing vectors of sizes [0, 1, 2], and ensure that we can
# construct from both lists and `np.array` objects with no ambiguity.
for n in [0, 1, 2]:
for wrap in [pass_through, np.array]:
# Ensure that we can get vectors templated on double by
# reference.
expected_init = wrap([float(x) for x in range(n)])
expected_add = wrap([x + 1 for x in expected_init])
expected_set = wrap([x + 10 for x in expected_init])
value_data = BasicVector(expected_init)
value = value_data.get_mutable_value()
self.assertTrue(np.allclose(value, expected_init))
# Add value directly.
# TODO(eric.cousineau): Determine if there is a way to extract
# the pointer referred to by the buffer (e.g. `value.data`).
value[:] += 1
self.assertTrue(np.allclose(value, expected_add))
self.assertTrue(
np.allclose(value_data.get_value(), expected_add))
self.assertTrue(
np.allclose(value_data.get_mutable_value(), expected_add))
# Set value from `BasicVector`.
value_data.SetFromVector(expected_set)
self.assertTrue(np.allclose(value, expected_set))
self.assertTrue(
np.allclose(value_data.get_value(), expected_set))
self.assertTrue(
np.allclose(value_data.get_mutable_value(), expected_set))
# Ensure we can construct from size.
value_data = BasicVector(n)
self.assertEqual(value_data.size(), n)
# Ensure we can clone.
value_copies = [
value_data.Clone(),
copy.copy(value_data),
copy.deepcopy(value_data),
]
for value_copy in value_copies:
self.assertTrue(value_copy is not value_data)
self.assertEqual(value_data.size(), n)
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()
context.FixInputPort(0, 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 __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()
#
# self.torque_port_index = self._DeclareInputPort('joint_state_results_port',
# PortDataType.kVectorValued,
# self.num_position * 2).get_index()
# Output Port
self.com_outport_index = self._DeclareVectorOutputPort(
'state_output_port', BasicVector(3), self._OutputCOM).get_index()
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 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, 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 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, kuka_plans, gripper_setpoint_list, control_period=0.005, print_period=0.5):
LeafSystem.__init__(self)
assert len(kuka_plans) == len(gripper_setpoint_list)
self.set_name("Manipulation Plan Runner")
# Add a zero order hold to hold the current position of the robot
kuka_plans.insert(0, JointSpacePlanRelative(
duration=3.0, delta_q=np.zeros(7)))
gripper_setpoint_list.insert(0, 0.055)
if len(kuka_plans) > 1:
# Insert to the beginning of plan_list a plan that moves the robot from its
# current position to plan_list[0].traj.value(0)
kuka_plans.insert(1, JointSpacePlanGoToTarget(
duration=6.0, q_target=kuka_plans[1].traj.value(0).flatten()))
gripper_setpoint_list.insert(0, 0.055)
self.gripper_setpoint_list = gripper_setpoint_list
self.kuka_plans_list = kuka_plans
self.current_plan_start_time = 0.
self.current_plan = None
self.current_gripper_setpoint = None
self.current_plan_idx = 0
# Stuff for iiwa control
self.nu = 7
self.print_period = print_period
self.last_print_time = -print_period
self.control_period = control_period
# iiwa position input port
self.iiwa_position_input_port = \
self._DeclareInputPort(
"iiwa_position", PortDataType.kVectorValued, 7)
# iiwa velocity input port
self.iiwa_velocity_input_port = \
self._DeclareInputPort(
"iiwa_velocity", PortDataType.kVectorValued, 7)
# iiwa external torque input port
self.iiwa_external_torque_input_port = \
self._DeclareInputPort(
"iiwa_torque_external", PortDataType.kVectorValued, 7)
# position and torque command output port
self.iiwa_position_command_output_port = \
self._DeclareVectorOutputPort("iiwa_position_command",
BasicVector(self.nu), self._CalcIiwaPositionCommand)
self.iiwa_torque_command_output_port = \
self._DeclareVectorOutputPort("iiwa_torque_command",
BasicVector(self.nu), self._CalcIiwaTorqueCommand)
# gripper setpoint and torque limit
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self._CalcGripperSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self._CalcForceLimitOutput)
# Declare command publishing rate
# state[0:7]: position command
# state[7:14]: torque command
# state[14]: gripper_setpoint
self._DeclareDiscreteState(self.nu*2 + 1)
self._DeclarePeriodicDiscreteUpdate(period_sec=self.control_period)
self.kPlanDurationMultiplier = 1.1
xyz=[-2, 0, 2], # Ensure that the box is within range.
rpy=[0, np.pi / 4, 0])
tree.addFrame(frame)
# Create camera.
camera = RgbdCamera(
name="camera", tree=tree, frame=frame,
z_near=0.5, z_far=5.0,
fov_y=np.pi / 4, show_window=True)
# - Describe state.
x = np.zeros(tree.get_num_positions() + tree.get_num_velocities())
# Allocate context and render.
context = camera.CreateDefaultContext()
context.FixInputPort(0, BasicVector(x))
output = camera.AllocateOutput(context)
camera.CalcOutput(context, output)
# Get images from computed output.
color_index = camera.color_image_output_port().get_index()
color_image = output.get_data(color_index).get_value()
color_array = color_image.data
depth_index = camera.depth_image_output_port().get_index()
depth_image = output.get_data(depth_index).get_value()
depth_array = depth_image.data
# Show camera info and images.
print("Intrinsics:\n{}".format(camera.depth_camera_info().intrinsic_matrix()))
dpi = mpl.rcParams['figure.dpi']
def __init__(self):
LeafSystem.__init__(self)
self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6),
self._DoCalcOutput)
# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self._DeclarePeriodicPublish(0.01, 0.0)
self.window = tk.Tk()
self.window.title("End-Effector TeleOp")
self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="roll",
length=800,
orient=tk.HORIZONTAL)
self.roll.pack()
self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="pitch",
length=800,
orient=tk.HORIZONTAL)
self.pitch.pack()
self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="yaw",
length=800,
orient=tk.HORIZONTAL)
self.yaw.pack()
self.x = tk.Scale(self.window, from_=-0.6, to=0.8,
resolution=-1,
label="x",
length=800,
orient=tk.HORIZONTAL)
self.x.pack()
self.y = tk.Scale(self.window, from_=-0.8, to=0.3,
resolution=-1,
label="y",
length=800,
orient=tk.HORIZONTAL)
self.y.pack()
self.z = tk.Scale(self.window, from_=0, to=1.1,
resolution=-1,
label="z",
length=800,
orient=tk.HORIZONTAL)
self.z.pack()
# The key bindings below provide teleop functionality via the
# keyboard, and are somewhat arbitrary (inspired by gaming
# conventions). Note that in order for the keyboard bindings to
# be active, the teleop slider window must be the active window.
def update(scale, value):
return lambda event: scale.set(scale.get() + value)
# Delta displacements for motion via keyboard teleop.
rotation_delta = 0.05 # rad
position_delta = 0.01 # m
# Linear motion key bindings.
self.window.bind("<Up>", update(self.z, +position_delta))
self.window.bind("<Down>", update(self.z, -position_delta))
self.window.bind("<d>", update(self.y, +position_delta))
self.window.bind("<a>", update(self.y, -position_delta))
self.window.bind("<w>", update(self.x, +position_delta))
self.window.bind("<s>", update(self.x, -position_delta))
# Rotational motion key bindings.
self.window.bind("<Control-d>", update(self.pitch, +rotation_delta))
self.window.bind("<Control-a>", update(self.pitch, -rotation_delta))
self.window.bind("<Control-w>", update(self.roll, +rotation_delta))
self.window.bind("<Control-s>", update(self.roll, -rotation_delta))
self.window.bind("<Control-Up>", update(self.yaw, +rotation_delta))
self.window.bind("<Control-Down>", update(self.yaw, -rotation_delta))
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("header_t",
AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def test_inverse_dynamics_controller(self):
sdf_path = FindResourceOrThrow(
"drake/manipulation/models/" +
"iiwa_description/sdf/iiwa14_no_collision.sdf")
plant = MultibodyPlant(time_step=0.01)
Parser(plant).AddModelFromFile(sdf_path)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
plant.mutable_gravity_field().set_gravity_vector([0.0, 0.0, 0.0])
plant.Finalize()
# We verify the (known) size of the model.
kNumPositions = 7
kNumVelocities = 7
kNumActuators = 7
kStateSize = kNumPositions + kNumVelocities
self.assertEqual(plant.num_positions(), kNumPositions)
self.assertEqual(plant.num_velocities(), kNumVelocities)
self.assertEqual(plant.num_actuators(), kNumActuators)
kp = np.array([1., 2., 3., 4., 5., 6., 7.])
ki = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
kd = np.array([.5, 1., 1.5, 2., 2.5, 3., 3.5])
controller = InverseDynamicsController(robot=plant,
kp=kp,
ki=ki,
kd=kd,
has_reference_acceleration=True)
context = controller.CreateDefaultContext()
output = controller.AllocateOutput()
estimated_state_port = 0
desired_state_port = 1
desired_acceleration_port = 2
control_port = 0
self.assertEqual(
controller.get_input_port(desired_acceleration_port).size(),
kNumVelocities)
self.assertEqual(
controller.get_input_port(estimated_state_port).size(), kStateSize)
self.assertEqual(
controller.get_input_port(desired_state_port).size(), kStateSize)
self.assertEqual(
controller.get_output_port(control_port).size(), kNumVelocities)
# Current state.
q = np.array([-0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3])
v = np.array([-0.9, -0.6, -0.3, 0.0, 0.3, 0.6, 0.9])
x = np.concatenate([q, v])
# Reference state and acceleration.
q_r = q + 0.1 * np.ones_like(q)
v_r = v + 0.1 * np.ones_like(v)
x_r = np.concatenate([q_r, v_r])
vd_r = np.array([1., 2., 3., 4., 5., 6., 7.])
integral_term = np.array([-1., -2., -3., -4., -5., -6., -7.])
vd_d = vd_r + kp * (q_r - q) + kd * (v_r - v) + ki * integral_term
context.FixInputPort(estimated_state_port, BasicVector(x))
context.FixInputPort(desired_state_port, BasicVector(x_r))
context.FixInputPort(desired_acceleration_port, BasicVector(vd_r))
controller.set_integral_value(context, integral_term)
# Set the plant's context.
plant_context = plant.CreateDefaultContext()
x_plant = plant.GetMutablePositionsAndVelocities(plant_context)
x_plant[:] = x
# Compute the expected value of the generalized forces using
# inverse dynamics.
tau_id = plant.CalcInverseDynamics(plant_context, vd_d,
MultibodyForces(plant))
# Verify the result.
controller.CalcOutput(context, output)
self.assertTrue(
np.allclose(output.get_vector_data(0).CopyToVector(), tau_id))
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 __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 __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 __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 __init__(self,
mbp,
symbol_list,
primitive_list,
dfa_json_file,
update_period=0.1):
LeafSystem.__init__(self)
self.mbp = mbp
# Our version of MBP context, which we'll modify
# in the publish method.
self.mbp_context = mbp.CreateDefaultContext()
self.set_name('task_execution_system')
# Take robot state vector as input.
self.DeclareVectorInputPort(
"mbp_state_vector",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
# State of system is current RPYXYZ and gripper goals
self.DeclareAbstractState(AbstractValue.Make(self.TESState()))
self.DeclarePeriodicEvent(period_sec=update_period,
offset_sec=0.0,
event=UnrestrictedUpdateEvent(
self.UpdateAbstractState))
# Produce RPYXYZ goals + gripper goals
self.DeclareVectorOutputPort("rpy_xyz_desired", BasicVector(6),
self.CopyRpyXyzDesiredOut)
self.DeclareVectorOutputPort("wsg_position", BasicVector(1),
self.CopyWsgPositionOut)
# Load the JSON file
with open(dfa_json_file, 'r') as f:
json_data = json.load(f)
# Figure out which states in the JSON dict are environment
# symbols, and which are primitives that we can execute.
self.environment_symbol_indices = []
self.environment_symbols = []
self.action_primitive_indices = []
self.action_primitives = []
for var_i, var_name in enumerate(json_data["variables"]):
# Split into symbols and primitives.
found_symbol = None
found_primitive = None
for symbol in symbol_list:
if symbol.name == var_name:
if found_symbol is not None:
raise ValueError(
"Ambiguous matching symbol names provided for symbol {}."
.format(var_name))
found_symbol = symbol
for primitive in primitive_list:
if primitive.name == var_name:
if found_primitive is not None:
raise ValueError(
"Ambiguous matching primitive names provided for symbol {}."
.format(var_name))
found_primitive = primitive
if found_primitive and found_symbol:
raise ValueError(
"Both matching symbol AND primitive found for symbol {}.".
format(var_name))
if not found_primitive and not found_symbol:
raise ValueError(
"No matching symbol or primitive found for symbol {}.".
format(var_name))
if found_symbol is not None:
self.environment_symbol_indices.append(var_i)
self.environment_symbols.append(found_symbol)
elif found_primitive:
self.action_primitive_indices.append(var_i)
self.action_primitives.append(found_primitive)
# And now build the ultimate lookup dictionary containing,
# at each node name:
# ( [environment symbol truth vector for this state],
# [possibly-empty list of primitives that can be taken,
# [next state names]] )
self.state_lookup_table = {}
for node_name in json_data["nodes"].keys():
node_symbols = []
node_state = np.array(json_data["nodes"][node_name]["state"])
next_node_names = [
str(x) for x in json_data["nodes"][node_name]["trans"]
]
node_primitives = []
for i, prim in zip(self.action_primitive_indices,
self.action_primitives):
if node_state[i]:
node_primitives.append(prim)
self.state_lookup_table[node_name] = (
node_state[self.environment_symbol_indices], node_primitives,
next_node_names)
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 test_basic_vector_double(self):
# Test constructing vectors of sizes [0, 1, 2], and ensure that we can
# construct from both lists and `np.array` objects with no ambiguity.
for n in [0, 1, 2]:
for wrap in [pass_through, np.array]:
# Ensure that we can get vectors templated on double by
# reference.
expected_init = wrap(map(float, range(n)))
expected_add = wrap([x + 1 for x in expected_init])
expected_set = wrap([x + 10 for x in expected_init])
value_data = BasicVector(expected_init)
value = value_data.get_mutable_value()
self.assertTrue(np.allclose(value, expected_init))
# Add value directly.
# TODO(eric.cousineau): Determine if there is a way to extract
# the pointer referred to by the buffer (e.g. `value.data`).
value[:] += 1
self.assertTrue(np.allclose(value, expected_add))
self.assertTrue(
np.allclose(value_data.get_value(), expected_add))
self.assertTrue(
np.allclose(value_data.get_mutable_value(), expected_add))
# Set value from `BasicVector`.
value_data.SetFromVector(expected_set)
self.assertTrue(np.allclose(value, expected_set))
self.assertTrue(
np.allclose(value_data.get_value(), expected_set))
self.assertTrue(
np.allclose(value_data.get_mutable_value(), expected_set))
# Ensure we can construct from size.
value_data = BasicVector(n)
self.assertEquals(value_data.size(), n)
# Ensure we can clone.
value_copies = [
value_data.Clone(),
copy.copy(value_data),
copy.deepcopy(value_data),
]
for value_copy in value_copies:
self.assertTrue(value_copy is not value_data)
self.assertEquals(value_data.size(), n)
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)
def test_basic_vector_set_get(self):
value = BasicVector(np.arange(3., 5.))
self.assertEqual(value.GetAtIndex(1), 4.)
value.SetAtIndex(1, 5.)
self.assertEqual(value.GetAtIndex(1), 5.)
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, planar=False):
"""
@param planar if True, restricts the GUI and the output to have y=0,
roll=0, yaw=0.
"""
LeafSystem.__init__(self)
self.DeclareVectorOutputPort("rpy_xyz", BasicVector(6),
self.DoCalcOutput)
# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self.DeclarePeriodicPublish(0.01, 0.0)
self.planar = planar
self.window = tk.Tk()
self.window.title("End-Effector TeleOp")
self.roll = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="roll (keys: ctrl-right, ctrl-left)",
length=800,
orient=tk.HORIZONTAL)
self.roll.pack()
self.roll.set(0)
self.pitch = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="pitch (keys: ctrl-d, ctrl-a)",
length=800,
orient=tk.HORIZONTAL)
if not planar:
self.pitch.pack()
self.pitch.set(0)
self.yaw = tk.Scale(self.window, from_=-2 * np.pi, to=2 * np.pi,
resolution=-1,
label="yaw (keys: ctrl-up, ctrl-down)",
length=800,
orient=tk.HORIZONTAL)
if not planar:
self.yaw.pack()
self.yaw.set(1.57)
self.x = tk.Scale(self.window, from_=-0.6, to=0.8,
resolution=-1,
label="x (keys: right, left)",
length=800,
orient=tk.HORIZONTAL)
self.x.pack()
self.x.set(0)
self.y = tk.Scale(self.window, from_=-0.8, to=0.3,
resolution=-1,
label="y (keys: d, a)",
length=800,
orient=tk.HORIZONTAL)
if not planar:
self.y.pack()
self.y.set(0)
self.z = tk.Scale(self.window, from_=0, to=1.1,
resolution=-1,
label="z (keys: up, down)",
length=800,
orient=tk.HORIZONTAL)
self.z.pack()
self.z.set(0)
# The key bindings below provide teleop functionality via the
# keyboard, and are somewhat arbitrary (inspired by gaming
# conventions). Note that in order for the keyboard bindings to
# be active, the teleop slider window must be the active window.
def update(scale, value):
return lambda event: scale.set(scale.get() + value)
# Delta displacements for motion via keyboard teleop.
rotation_delta = 0.05 # rad
position_delta = 0.01 # m
# Linear motion key bindings.
self.window.bind("<Up>", update(self.z, +position_delta))
self.window.bind("<Down>", update(self.z, -position_delta))
if (not planar):
self.window.bind("<d>", update(self.y, +position_delta))
self.window.bind("<a>", update(self.y, -position_delta))
self.window.bind("<Right>", update(self.x, +position_delta))
self.window.bind("<Left>", update(self.x, -position_delta))
# Rotational motion key bindings.
self.window.bind("<Control-Right>", update(self.roll, +rotation_delta))
self.window.bind("<Control-Left>", update(self.roll, -rotation_delta))
if (not planar):
self.window.bind("<Control-d>",
update(self.pitch, +rotation_delta))
self.window.bind("<Control-a>",
update(self.pitch, -rotation_delta))
self.window.bind("<Control-Up>",
update(self.yaw, +rotation_delta))
self.window.bind("<Control-Down>",
update(self.yaw, -rotation_delta))
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 test_diagram_simulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
self.assertTrue(builder.empty())
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
self.assertFalse(builder.empty())
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
self.assertEqual(
builder.GetMutableSystems(),
[adder0, adder1, 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))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
self.assertEqual(
diagram.GetSystems(),
[adder0, adder1, integrator])
# 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])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# 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.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
# Test binding for PrintSimulatorStatistics
PrintSimulatorStatistics(simulator)
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)
self.assertTrue(np.allclose(xc, xc_expected))
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]))
class _ArgHelper:
"""Provides information and functions to aid in interfacing a Python
function with the Systems framework."""
def __init__(self, name, cls, scalar_as_vector):
"""Given a class (or type annotation), figure out the type (vector
port, abstract port, or context time), the model value (for ports), and
example value (for output inference)."""
# Name can be overridden.
self.name = name
self._scalar_needs_conversion = False
self._is_direct_type = False
if isinstance(cls, VectorArg):
self.type = PortDataType.kVectorValued
self.model = BasicVector(cls._size)
self.model.get_mutable_value()[:] = 0
self.example = self.model.get_value()
elif BasicVector_.is_instantiation(cls):
assert False, (
f"Must supply BasicVector_[] instance, not type: {cls}")
elif BasicVector_.is_instantiation(type(cls)):
self.type = PortDataType.kVectorValued
self.model = cls
self.example = self.model
self._is_direct_type = True
elif scalar_as_vector and cls in SCALAR_TYPES:
self.type = PortDataType.kVectorValued
self.model = BasicVector(1)
self.model.get_mutable_value()[:] = 0
self.example = float() # Should this be smarter about the type?
self._scalar_needs_conversion = True
elif cls is ContextTimeArg:
self.type = ContextTimeArg
self.model = None
self.example = float()
else:
self.type = PortDataType.kAbstractValued
self.model, self.example = _get_abstract_model_and_example(cls)
if self.model is self.example:
self._is_direct_type = True
def _squeeze(self, x):
if self._scalar_needs_conversion:
assert x.shape == (1, ), f"Bad input: {x}"
return x.item(0)
else:
return x
def _unsqueeze(self, x):
if self._scalar_needs_conversion:
return np.array([x])
else:
return x
def declare_input_eval(self, system):
"""Declares an input evaluation function. If a port is needed, will
declare the port."""
if self.type is ContextTimeArg:
return Context.get_time
elif self.type == PortDataType.kAbstractValued:
# DeclareInputPort does not work with kAbstractValued :(
port = system.DeclareAbstractInputPort(name=self.name,
model_value=self.model)
if self._is_direct_type:
return port.EvalAbstract
else:
return port.Eval
else:
port = system.DeclareVectorInputPort(name=self.name,
model_vector=self.model)
if self._is_direct_type:
return port.EvalBasicVector
else:
return lambda context: self._squeeze(port.Eval(context))
def declare_output_port(self, system, calc):
"""Declares an output port on a given system."""
if self.type is ContextTimeArg:
assert False, dedent(r"""\
ContextTimeArg is disallowed for output arguments. If needed,
explicitly pass it through, e.g.:
def context_time(t: ContextTimeArg):
return t
""")
elif self.type == PortDataType.kAbstractValued:
system.DeclareAbstractOutputPort(name=self.name,
alloc=self.model.Clone,
calc=calc)
else:
system.DeclareVectorOutputPort(name=self.name,
model_value=self.model,
calc=calc)
def get_set_output_func(self):
assert self.type is not ContextTimeArg
if self.type == PortDataType.kAbstractValued:
if self._is_direct_type:
return lambda output, value: output.SetFrom(value)
else:
return lambda output, value: output.set_value(value)
else:
if self._is_direct_type:
# TODO(eric.cousineau): Bind VectorBase.SetFrom().
return lambda output, value: output.SetFromVector(value.
get_value())
else:
return lambda output, value: output.SetFromVector(
self._unsqueeze(value))
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):
LeafSystem_[float].__init__(self)
self._DeclareAbstractInputPort("in")
self._DeclareVectorOutputPort("out", BasicVector(1), self._Out)
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 __init__(self, num_inputs, size):
LeafSystem.__init__(self)
for i in range(num_inputs):
self._DeclareInputPort(kUseDefaultName, PortDataType.kVectorValued,
size)
self._DeclareVectorOutputPort("sum", BasicVector(size), self._calc_sum)
