def make_diagram():
# Use a nested function to ensure that all locals get garbage
# collected quickly.
# Construct a trivial plant and ID controller.
# N.B. We explicitly do *not* add this plant to the diagram.
controller_plant = MultibodyPlant(time_step=0.002)
controller_plant.Finalize()
builder = DiagramBuilder()
controller = builder.AddSystem(
InverseDynamicsController(
controller_plant,
kp=[],
ki=[],
kd=[],
has_reference_acceleration=False,
)
)
# Forward ports for ease of testing.
builder.ExportInput(
controller.get_input_port_estimated_state(), "x_estimated")
builder.ExportInput(
controller.get_input_port_desired_state(), "x_desired")
builder.ExportOutput(controller.get_output_port_control(), "u")
diagram = builder.Build()
return diagram
def CreateManipStationPlanRunnerDiagram(station,
kuka_plans,
gripper_setpoint_list,
print_period=1.0):
builder = DiagramBuilder()
iiwa_controller = IiwaController(station, print_period=print_period)
builder.AddSystem(iiwa_controller)
plan_scheduler = PlanScheduler(kuka_plans, gripper_setpoint_list)
builder.AddSystem(plan_scheduler)
builder.Connect(plan_scheduler.iiwa_plan_output_port,
iiwa_controller.plan_input_port)
builder.ExportInput(iiwa_controller.iiwa_position_input_port,
"iiwa_position")
builder.ExportInput(iiwa_controller.iiwa_velocity_input_port,
"iiwa_velocity")
builder.ExportOutput(iiwa_controller.iiwa_position_command_output_port,
"iiwa_position_and_torque_command")
builder.ExportOutput(plan_scheduler.hand_setpoint_output_port,
"gripper_setpoint")
builder.ExportOutput(plan_scheduler.gripper_force_limit_output_port,
"force_limit")
plan_runner = builder.Build()
plan_runner.set_name("Plan Runner")
return plan_runner
def test_diagram_fan_out(self):
builder = DiagramBuilder()
adder = builder.AddSystem(Adder(6, 1))
adder.set_name("adder")
builder.ExportOutput(adder.get_output_port())
in0_index = builder.ExportInput(adder.get_input_port(0), "in0")
in1_index = builder.ExportInput(adder.get_input_port(1), "in1")
# Exercise ConnectInput overload bindings, with and without argument
# names.
builder.ConnectInput(in0_index, adder.get_input_port(2))
builder.ConnectInput("in1", adder.get_input_port(3))
builder.ConnectInput(diagram_port_name="in0",
input=adder.get_input_port(4))
builder.ConnectInput(diagram_port_index=in1_index,
input=adder.get_input_port(5))
diagram = builder.Build()
diagram.set_name("fan_out_diagram")
graph = diagram.GetGraphvizString()
# Check the desired input topology is in the graph.
self.assertRegex(graph, "_u0 -> .*:u0")
self.assertRegex(graph, "_u1 -> .*:u1")
self.assertRegex(graph, "_u0 -> .*:u2")
self.assertRegex(graph, "_u1 -> .*:u3")
self.assertRegex(graph, "_u0 -> .*:u4")
self.assertRegex(graph, "_u1 -> .*:u5")
def __init__(self, num_inputs, size):
Diagram.__init__(self)
builder = DiagramBuilder()
adder = Adder(num_inputs, size)
builder.AddSystem(adder)
builder.ExportOutput(adder.get_output_port(0))
for i in range(num_inputs):
builder.ExportInput(adder.get_input_port(i))
builder.BuildInto(self)
def make_diagram():
builder = DiagramBuilder()
adder1 = builder.AddNamedSystem("adder1", Adder(2, 2))
adder2 = builder.AddNamedSystem("adder2", Adder(1, 2))
builder.Connect(adder1.get_output_port(), adder2.get_input_port())
builder.ExportInput(adder1.get_input_port(0), "in0")
builder.ExportInput(adder1.get_input_port(1), "in1")
builder.ExportOutput(adder2.get_output_port(), "out")
diagram = builder.Build()
return adder1, adder2, diagram
def test_adder_simulation(self):
builder = DiagramBuilder()
adder = builder.AddSystem(self._create_adder_system())
adder.set_name("custom_adder")
# Add ZOH so we can easily extract state.
zoh = builder.AddSystem(ZeroOrderHold(0.1, 3))
zoh.set_name("zoh")
builder.ExportInput(adder.get_input_port(0))
builder.ExportInput(adder.get_input_port(1))
builder.Connect(adder.get_output_port(0), zoh.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
self._fix_adder_inputs(context)
simulator = Simulator(diagram, context)
simulator.Initialize()
simulator.AdvanceTo(1)
# Ensure that we have the outputs we want.
value = (diagram.GetMutableSubsystemContext(
zoh, context).get_discrete_state_vector().get_value())
self.assertTrue(np.allclose([5, 7, 9], value))
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
AddModelFromSdfFile(file_name=file_name, plant=cart_pole)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram, max_depth=2)
if not args.test:
plt.show()
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, config=None):
if config is None:
config_path = os.path.join(os.path.dirname(__file__),
"config.yaml")
config = yaml.safe_load(open(config_path, 'r'))
self._config = config
self._step_dt = config["step_dt"]
self._model_name = config["model_name"]
self._sim_diagram = DrakeSimDiagram(config)
mbp = self._sim_diagram.mbp
builder = self._sim_diagram.builder
# === Add table =====
dims = config["table"]["size"]
color = np.array(config["table"]["color"])
friction_params = config["table"]["coulomb_friction"]
box_shape = Box(*dims)
X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.]))
# This rigid body will be added to the world model instance since
# the model instance is not specified.
box_body = mbp.AddRigidBody(
"table",
SpatialInertia(mass=1.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T)
mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape,
"table_vis", color)
mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape,
"table_collision",
CoulombFriction(*friction_params))
# === Add pusher ====
parser = Parser(mbp, self._sim_diagram.sg)
self._pusher_model_id = parser.AddModelFromFile(
path_util.pusher_peg, "pusher")
base_link = mbp.GetBodyByName("base", self._pusher_model_id)
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
def pusher_port_func():
actuation_input_port = mbp.get_actuation_input_port(
self._pusher_model_id)
state_output_port = mbp.get_state_output_port(
self._pusher_model_id)
builder.ExportInput(actuation_input_port, "pusher_actuation_input")
builder.ExportOutput(state_output_port, "pusher_state_output")
self._sim_diagram.finalize_functions.append(pusher_port_func)
# === Add slider ====
parser = Parser(mbp, self._sim_diagram.sg)
self._slider_model_id = parser.AddModelFromFile(
path_util.model_paths[self._model_name], self._model_name)
def slider_port_func():
state_output_port = mbp.get_state_output_port(
self._slider_model_id)
builder.ExportOutput(state_output_port, "slider_state_output")
self._sim_diagram.finalize_functions.append(slider_port_func)
if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]:
self._sim_diagram.add_rgbd_sensors_from_config(config)
if "visualization" in config:
if not config["visualization"]:
pass
elif config["visualization"] == "meshcat":
self._sim_diagram.connect_to_meshcat()
elif config["visualization"] == "drake_viz":
self._sim_diagram.connect_to_drake_visualizer()
else:
raise ValueError("Unknown visualization:",
config["visualization"])
self._sim_diagram.finalize()
builder = DiagramBuilder()
builder.AddSystem(self._sim_diagram)
pid = builder.AddSystem(
PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0]))
builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"),
pid.get_input_port_estimated_state())
builder.Connect(
pid.get_output_port_control(),
self._sim_diagram.GetInputPort("pusher_actuation_input"))
builder.ExportInput(pid.get_input_port_desired_state(),
"pid_input_port_desired_state")
self._diagram = builder.Build()
self._pid_desired_state_port = self._diagram.get_input_port(0)
self._simulator = Simulator(self._diagram)
self.reset()
self.action_space = spaces.Box(low=-1.,
high=1.,
shape=(2, ),
dtype=np.float32)
# TODO: Observation space for images is currently too loose
self.observation_space = convert_observation_to_space(
self.get_observation())
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))
input_port_index=diagram.get_input_port().get_index())
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
cartpole = Parser(plant).AddModelFromFile("cart_pole.sdf")
plant.Finalize()
# Setup visualization
visualizer = ConnectMeshcatVisualizer(builder,
scene_graph=scene_graph,
zmq_url="new")
visualizer.vis.delete()
visualizer.set_planar_viewpoint(xmin=-2.5, xmax=2.5, ymin=-1.0, ymax=2.5)
builder.ExportInput(plant.get_actuation_input_port(), "command")
diagram = builder.Build()
# problem block 1: trying to do LQR with the diagram
# returns an error about the diagram not supporting toAutoDiffXd
# same issue when trying to call diagram.toAutoDiffXd() directly
"""
controller = builder.AddSystem(BalancingLQR(diagram))
builder.Connect(plant.get_state_output_port(), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
"""
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)
class DrakeSimDiagram(Diagram):
def __init__(self, config):
Diagram.__init__(self)
dt = config["mbp_dt"]
self._builder = DiagramBuilder()
self._mbp, self._sg = AddMultibodyPlantSceneGraph(self._builder, dt)
self._finalize_functions = []
self._finalized = False
self._rgbd_sensors = dict()
self._renderer_name = None
# === Property accessors ========================================
@property
def mbp(self):
return self._mbp
@property
def sg(self):
return self._sg
@property
def builder(self):
return self._builder
@property
def finalize_functions(self):
return self._finalize_functions
@property
def rgbd_sensors(self):
return self._rgbd_sensors
# === Add visualizers ===========================================
def connect_to_meshcat(self):
self._meshcat = ConnectMeshcatVisualizer(
self._builder,
scene_graph=self._sg,
zmq_url="tcp://127.0.0.1:6000",
draw_period=1)
return self._meshcat
def connect_to_drake_visualizer(self):
self._drake_viz = DrakeVisualizer.AddToBuilder(builder=self._builder,
scene_graph=self._sg)
return self._drake_viz
# === Add Cameras ===============================================
def add_rgbd_sensors_from_config(self, config):
if not config["rgbd_sensors"]["enabled"]:
return
for camera_name, sensor_config in iteritems(
config["rgbd_sensors"]["sensor_list"]):
self.add_rgbd_sensor(camera_name, sensor_config)
def add_rgbd_sensor(self, camera_name, sensor_config):
"""
Adds Rgbd camera to the diagram
"""
builder = self._builder
if self._renderer_name is None:
self._renderer_name = "vtk_renderer"
self._sg.AddRenderer(self._renderer_name,
MakeRenderEngineVtk(RenderEngineVtkParams()))
width = sensor_config['width']
height = sensor_config['height']
fov_y = sensor_config['fov_y']
z_near = sensor_config['z_near']
z_far = sensor_config['z_far']
# This is in right-down-forward convention
X_W_camera = transform_from_dict(sensor_config)
color_camera = ColorRenderCamera(
RenderCameraCore(self._renderer_name,
CameraInfo(width, height, fov_y),
ClippingRange(z_near, z_far), RigidTransform()),
False)
depth_camera = DepthRenderCamera(color_camera.core(),
DepthRange(z_near, z_far))
# add camera system
camera = builder.AddSystem(
RgbdSensor(parent_id=self._sg.world_frame_id(),
X_PB=X_W_camera,
color_camera=color_camera,
depth_camera=depth_camera))
builder.Connect(self._sg.get_query_output_port(),
camera.query_object_input_port())
self._rgbd_sensors[camera_name] = camera
# === Finalize the completed diagram ============================
def finalize(self):
self._mbp.Finalize()
self._finalized = True
for func in self._finalize_functions:
func()
self._builder.ExportOutput(self._sg.get_pose_bundle_output_port(),
"pose_bundle")
self._builder.ExportOutput(self._mbp.get_contact_results_output_port(),
"contact_results")
self._builder.ExportOutput(self._mbp.get_state_output_port(),
"plant_continuous_state")
self._builder.ExportOutput(self._mbp.get_geometry_poses_output_port(),
"geometry_poses")
self._builder.ExportInput(
self._mbp.get_applied_spatial_force_input_port(), "spatial_input")
self._builder.ExportOutput(self._mbp.get_body_poses_output_port(),
"body_poses")
self._builder.BuildInto(self)
def is_finalized(self):
return self._finalized
# === Camera helpers ============================================
def get_image_observations_single_sensor(self, sensor_name, context):
assert self.is_finalized()
sensor = self._rgbd_sensors[sensor_name]
sensor_context = self.GetSubsystemContext(sensor, context)
rgb = sensor.color_image_output_port().Eval(sensor_context)
depth_32F = sensor.depth_image_32F_output_port().Eval(sensor_context)
depth_16U = sensor.depth_image_16U_output_port().Eval(sensor_context)
label = sensor.label_image_output_port().Eval(sensor_context)
return {
'rgb': np.copy(rgb.data[:, :, :3]),
'depth_32F': np.copy(depth_32F.data).squeeze(),
'depth_16U': np.copy(depth_16U.data).squeeze(),
'label': np.copy(label.data).squeeze()
}
def get_image_observations(self, context):
image_dict = dict()
for sensor_name in self._rgbd_sensors:
image_dict[
sensor_name] = self.get_image_observations_single_sensor(
sensor_name, context)
return image_dict
def get_label_db(self):
"""
Builds database that associates bodies and labels
:return: TinyDB database
"""
db = TinyDB(storage=MemoryStorage)
for i in range(self._mbp.num_bodies()):
body = self._mbp.get_body(BodyIndex(i))
model_instance_id = int(body.model_instance())
body_name = body.name()
model_name = self._mbp.GetModelInstanceName(body.model_instance())
entry = {
'label': i,
'model_instance_id': model_instance_id,
'body_name': body_name,
'model_name': model_name
}
db.insert(entry)
return db
import matplotlib.pyplot as plt
from pydrake.systems.drawing import plot_system_graphviz
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import Adder
builder = DiagramBuilder()
size = 1
adders = [
builder.AddSystem(Adder(1, size)),
builder.AddSystem(Adder(1, size)),
]
for i, adder in enumerate(adders):
adder.set_name("adders[{}]".format(i))
builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0))
builder.ExportInput(adders[0].get_input_port(0))
builder.ExportOutput(adders[1].get_output_port(0))
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram)
plt.show()
def __init__(self):
#import pdb; pdb.set_trace()
# Create a block diagram containing our system.
builder = DiagramBuilder()
# add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2)
plant_x = builder.AddSystem(DoubleIntegrator())
plant_y = builder.AddSystem(DoubleIntegrator())
plant_x.set_name("double_integrator_x")
plant_y.set_name("double_integrator_y")
# add the controller, lead compensator for now just to stablize it
controller_x = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_y = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_x.set_name("controller_x")
controller_y.set_name("controller_y")
# the perception's "Beam" model with the four sources of noise
x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
x_meas_fb.set_name("x_fb")
y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
y_meas_fb.set_name("y_fb")
y_perception = builder.AddSystem(Adder(num_inputs=2, size=1))
y_perception.set_name("range_measurment_y")
neg_y_meas = builder.AddSystem(Gain(k=-1., size=1))
neg_y_meas.set_name("neg_y_meas")
wall_position = builder.AddSystem(ConstantVectorSource([Y_wall]))
p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\
sampling_interval_sec=0.01))
p_hit.set_name("GaussionNoise(0,1)")
p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\
sampling_interval_sec=0.01))
p_short.set_name("ExponentialNoise(1)")
#p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\
# sampling_interval_sec=0.01))
p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\
sampling_interval_sec=0.01))
p_rand.set_name("UniformNoise(0,1)")
#import pdb; pdb.set_trace()
p_hit_Dx = builder.AddSystem(Demultiplexer(size=2))
p_hit_Dx.set_name('Dmux1')
p_short_Dx = builder.AddSystem(Demultiplexer(size=2))
p_short_Dx.set_name('Dmux2')
p_rand_Dx = builder.AddSystem(Demultiplexer(size=2))
p_rand_Dx.set_name('Dmux3')
normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_x.set_name("Sigma_x")
normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_y.set_name("Sigma_y")
expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_x.set_name("Exp_x")
expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_y.set_name("Exp_y")
randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_x.set_name("Rand_x")
randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_y.set_name("Rand_y")
#maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_x.set_name("Max_x")
#maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_y.set_name("Max_y")
# the summation to get the error (closing the loop)
summ_x = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_y = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_x.set_name("summation_x")
summ_y.set_name("summation_y")
neg_x = builder.AddSystem(Gain(k=-1., size=1))
neg_y = builder.AddSystem(Gain(k=-1., size=1))
neg_uy = builder.AddSystem(Gain(k=-1., size=1))
neg_x.set_name("neg_x")
neg_y.set_name("neg_y")
neg_uy.set_name("neg_uy")
# wire up all the blocks (summation to the controller to the plant ...)
builder.Connect(summ_x.get_output_port(0),
controller_x.get_input_port(0)) # e_x
builder.Connect(summ_y.get_output_port(0),
controller_y.get_input_port(0)) # e_y
builder.Connect(controller_x.get_output_port(0),
plant_x.get_input_port(0)) # u_x
builder.Connect(
controller_y.get_output_port(0),
neg_uy.get_input_port(0)) # u_y (to deal with directions)
builder.Connect(neg_uy.get_output_port(0),
plant_y.get_input_port(0)) # u_y
builder.Connect(
plant_x.get_output_port(0),
x_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(wall_position.get_output_port(0),
y_perception.get_input_port(0)) # perception
builder.Connect(plant_y.get_output_port(0),
neg_y_meas.get_input_port(0)) # perception
builder.Connect(neg_y_meas.get_output_port(0),
y_perception.get_input_port(1)) # perception, z meas
builder.Connect(
y_perception.get_output_port(0),
y_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(p_hit.get_output_port(0),
p_hit_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_hit_Dx.get_output_port(0),
normgain_x.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_x.get_output_port(0),
x_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_hit_Dx.get_output_port(1),
normgain_y.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_y.get_output_port(0),
y_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_short.get_output_port(0),
p_short_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_short_Dx.get_output_port(0),
expgain_x.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_x.get_output_port(0),
x_meas_fb.get_input_port(2)) # Exp dist
builder.Connect(p_short_Dx.get_output_port(1),
expgain_y.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_y.get_output_port(0),
y_meas_fb.get_input_port(2)) # Exp dist
#builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist
#builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand.get_output_port(0),
p_rand_Dx.get_input_port(0)) # normalize Uniform dist
builder.Connect(p_rand_Dx.get_output_port(0),
randgain_x.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_x.get_output_port(0),
x_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand_Dx.get_output_port(1),
randgain_y.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_y.get_output_port(0),
y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(x_meas_fb.get_output_port(0),
neg_x.get_input_port(0)) # -x
builder.Connect(y_meas_fb.get_output_port(0),
neg_y.get_input_port(0)) # -y
builder.Connect(neg_x.get_output_port(0),
summ_x.get_input_port(1)) # r-x
builder.Connect(neg_y.get_output_port(0),
summ_y.get_input_port(1)) # r-y
# Make the desired_state input of the controller, an input to the diagram.
builder.ExportInput(summ_x.get_input_port(0))
builder.ExportInput(summ_y.get_input_port(0))
# get telemetry
logger_x = LogOutput(plant_x.get_output_port(0), builder)
logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder)
logger_y = LogOutput(plant_y.get_output_port(0), builder)
logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder)
logger_x.set_name("logger_x_state")
logger_y.set_name("logger_y_state")
logger_noise_x.set_name("x_state_meas")
logger_noise_y.set_name("y_meas")
# compile the system
diagram = builder.Build()
diagram.set_name("diagram")
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# First we extract the subsystem context for the plant
plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context)
plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context)
# Then we can set the pendulum state, which is (x, y).
z0 = X_0
plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0)
z0 = Y_0
plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0)
# The diagram has a single input port (port index 0), which is the desired_state.
context.FixInputPort(
0,
[X_d]) # here we assume no perception, closing feedback on state X
context.FixInputPort(
1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0
# run the simulation
simulator.AdvanceTo(10)
t = logger_x.sample_times()
#import pdb; pdb.set_trace()
# Plot the results.
plt.figure()
plot_system_graphviz(diagram, max_depth=2)
plt.figure()
plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas')
plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))')
plt.plot(t, logger_x.data().transpose(), label='x_state')
plt.plot(t, logger_y.data().transpose(), label='y_state')
plt.xlabel('t')
plt.ylabel('x(t),y(t)')
plt.legend()
plt.show(block=True)
