def make_box_flipup(generator,
observations="state",
meshcat=None,
time_limit=10):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
# TODO(russt): randomize parameters.
box = AddPlanarBinAndSimpleBox(plant)
finger = AddPointFinger(plant)
plant.Finalize()
plant.set_name("plant")
SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id())
controller_plant = MultibodyPlant(time_step=0.005)
AddPointFinger(controller_plant)
if meshcat:
MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3)
ContactVisualizer.AddToBuilder(
builder, plant, meshcat,
ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0))
# Use the controller plant to visualize the set point geometry.
controller_scene_graph = builder.AddSystem(SceneGraph())
controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph)
SetColor(controller_scene_graph,
color=[1.0, 165.0 / 255, 0.0, 1.0],
source_id=controller_plant.get_source_id())
controller_vis = MeshcatVisualizerCpp.AddToBuilder(
builder, controller_scene_graph, meshcat,
MeshcatVisualizerParams(prefix="controller"))
controller_vis.set_name("controller meshcat")
controller_plant.Finalize()
# Stiffness control. (For a point finger with unit mass, the
# InverseDynamicsController is identical)
N = controller_plant.num_positions()
kp = [100] * N
ki = [1] * N
kd = [2 * np.sqrt(kp[0])] * N
controller = builder.AddSystem(
InverseDynamicsController(controller_plant, kp, ki, kd, False))
builder.Connect(plant.get_state_output_port(finger),
controller.get_input_port_estimated_state())
actions = builder.AddSystem(PassThrough(N))
positions_to_state = builder.AddSystem(Multiplexer([N, N]))
builder.Connect(actions.get_output_port(),
positions_to_state.get_input_port(0))
zeros = builder.AddSystem(ConstantVectorSource([0] * N))
builder.Connect(zeros.get_output_port(),
positions_to_state.get_input_port(1))
builder.Connect(positions_to_state.get_output_port(),
controller.get_input_port_desired_state())
builder.Connect(controller.get_output_port(),
plant.get_actuation_input_port())
if meshcat:
positions_to_poses = builder.AddSystem(
MultibodyPositionToGeometryPose(controller_plant))
builder.Connect(
positions_to_poses.get_output_port(),
controller_scene_graph.get_source_pose_port(
controller_plant.get_source_id()))
builder.ExportInput(actions.get_input_port(), "actions")
if observations == "state":
builder.ExportOutput(plant.get_state_output_port(), "observations")
# TODO(russt): Add 'time', and 'keypoints'
else:
raise ValueError("observations must be one of ['state']")
class RewardSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("box_state", 6)
self.DeclareVectorInputPort("finger_state", 4)
self.DeclareVectorInputPort("actions", 2)
self.DeclareVectorOutputPort("reward", 1, self.CalcReward)
def CalcReward(self, context, output):
box_state = self.get_input_port(0).Eval(context)
finger_state = self.get_input_port(1).Eval(context)
actions = self.get_input_port(2).Eval(context)
angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2
cost = 2 * angle_from_vertical**2 # box angle
cost += 0.1 * box_state[5]**2 # box velocity
effort = actions - finger_state[:2]
cost += 0.1 * effort.dot(effort) # effort
# finger velocity
cost += 0.1 * finger_state[2:].dot(finger_state[2:])
# Add 10 to make rewards positive (to avoid rewarding simulator
# crashes).
output[0] = 10 - cost
reward = builder.AddSystem(RewardSystem())
builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0))
builder.Connect(plant.get_state_output_port(finger),
reward.get_input_port(1))
builder.Connect(actions.get_output_port(), reward.get_input_port(2))
builder.ExportOutput(reward.get_output_port(), "reward")
# Set random state distributions.
uniform_random = Variable(name="uniform_random",
type=Variable.Type.RANDOM_UNIFORM)
box_joint = plant.GetJointByName("box")
x, y = box_joint.get_default_translation()
box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0)
diagram = builder.Build()
simulator = Simulator(diagram)
# Termination conditions:
def monitor(context):
if context.get_time() > time_limit:
return EventStatus.ReachedTermination(diagram, "time limit")
return EventStatus.Succeeded()
simulator.set_monitor(monitor)
return simulator
builder.Connect(inner_controller.get_output_port(0), vibrating_pendulum.get_actuation_input_port())
# scene graph (i.e. all the bodies in the diagram) -> visualizer
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
# finalize block diagram
diagram = builder.Build()
"""When connecting all the blocks by hand, it is possible to do some mistakes.
To double check your work, you can use the function `plot_system_graphviz`, which plots the overall block diagram you built.
You can compare the automatically-generated block diagram with the one above
"""
# give names to the blocks (just to make the plot nicer)
diagram.set_name('Block Diagram for the Control of the Vibrating Pendulum')
vibrating_pendulum.set_name('Vibrating Pendulum')
inner_controller.set_name('Inner Controller')
outer_controller.set_name('Outer Controller')
mux.set_name('Mux')
selector.set_name('Pendulum-State Selector')
visualizer.set_name('Visualizer')
logger.set_name('Logger')
scene_graph.set_name('Scene Graph')
# plot current diagram
plt.figure(figsize=(20, 10))
plot_system_graphviz(diagram)
"""## Closed-Loop Simulation
Now we can finally simulate the system in closed loop with the controllers we wrote.
In the meanwhile, we will also set up a "video recording" with which we will be able to playback the simulation.
