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.