def Simulate2dBallAndBeam(x0, duration): builder = DiagramBuilder() # Load in the ball and beam from a description file. tree = RigidBodyTree() AddModelInstancesFromSdfString( open("ball_and_beam.sdf", 'r').read(), FloatingBaseType.kFixed, None, tree) # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. plant = builder.AddSystem(RigidBodyPlant(tree)) # Spawn a controller and hook it up controller = builder.AddSystem(BallAndBeam2dController(tree)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. #simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.001) # Set the initial state state = simulator.get_mutable_context( ).get_mutable_continuous_state_vector() state.SetFromVector(x0) # Simulate! simulator.StepTo(duration) return tree, controller, state_log
def simulateUntil(t, state_init, u_opt_poly, x_opt_poly, K_poly): ################################################################################## # Setup diagram for simulation diagram = makeDiagram(n_quadrotors, n_balls, use_visualizer=True, trajectory_u=u_opt_poly, trajectory_x=x_opt_poly, trajectory_K=K_poly) simulator = Simulator(diagram) integrator = simulator.get_mutable_integrator() integrator.set_maximum_step_size( 0.01 ) # Reduce the max step size so that we can always detect collisions context = simulator.get_mutable_context() context.SetAccuracy(1e-4) context.SetTime(0.) context.SetContinuousState(state_init) simulator.Initialize() simulator.AdvanceTo(t) return context.get_continuous_state_vector().CopyToVector()
normed=True, facecolor='b') self.ax.set_title('t = ' + str(context.get_time())) builder = DiagramBuilder() num_particles = 1000 num_bins = 100 xlim = [-2, 2] ylim = [-1, 3.5] draw_timestep = .25 visualizer = builder.AddSystem( HistogramVisualizer(num_particles, num_bins, xlim, ylim, draw_timestep)) x = np.linspace(xlim[0], xlim[1], 100) visualizer.ax.plot(x, dynamics(x, 0), 'k', linewidth=2) for i in range(0, num_particles): sys = builder.AddSystem(SimpleStochasticSystem()) builder.Connect(sys.get_output_port(0), visualizer.get_input_port(i)) AddRandomInputs(.1, builder) diagram = builder.Build() simulator = Simulator(diagram) simulator.get_mutable_integrator().set_fixed_step_mode(True) simulator.get_mutable_integrator().set_maximum_step_size(0.1) simulator.AdvanceTo(20) plt.show()
def SimulateHand(duration=10., num_fingers=3, mu=0.5, manipuland_sdf="models/manipuland_box.sdf", initial_manipuland_pose=np.array([1.5, 0., 0.]), n_grasp_search_iters=100, manipuland_trajectory_callback=None, control_period=0.0333, print_period=1.0): ''' Given a great many passthrough arguments (see docs for HandController and usage example in set_5_mpc.ipynb), constructs a simulation of a num_fingers-fingered hand and simulates it for duration seconds from a specified initial manipuland pose. Returns: (hand, plant, controller, state_log, contact_log) hand: The RigidBodyTree of the complete hand. plant: The RigidBodyPlant that owns the hand RBT controller: The HandController object state_log: A SignalLogger that has logged the output of the state output port of plant. contact_log: A PlanarHandContactLogger object that has logged the output of the contact results output port of the plant. ''' builder = DiagramBuilder() tree = BuildHand(num_fingers, manipuland_sdf) num_finger_links = 3 # from sdf num_hand_q = num_finger_links * num_fingers # Generate the nominal posture for the hand # First link wide open, next links at right angles x_nom = np.zeros(2 * num_finger_links * num_fingers + 6) for i in range(num_fingers): if i < num_fingers / 2: x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \ np.array([2, 1.57, 1.57]) else: x_nom[(num_finger_links*i):(num_finger_links*(i+1))] = \ -np.array([2, 1.57, 1.57]) # Drop in the initial manipuland location x_nom[num_hand_q:(num_hand_q + 3)] = initial_manipuland_pose # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. It handles e.g. collision # modeling. plant = builder.AddSystem(RigidBodyPlant(tree)) # Alter the ground material used in simulation to make # it dissipate more energy (to make the object less bouncy) # and stickier (to make it easier to hold) and softer # (again to make it less bouncy) allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E6) # default 1E9 allmaterials.set_dissipation(1.0) # default 0.32 allmaterials.set_friction(0.9) # default 0.9. plant.set_default_compliant_material(allmaterials) # Spawn a controller and hook it up controller = builder.AddSystem( HandController( tree, x_nom=x_nom, num_fingers=num_fingers, mu=mu, n_grasp_search_iters=n_grasp_search_iters, manipuland_trajectory_callback=manipuland_trajectory_callback, control_period=control_period, print_period=print_period)) nq = controller.nq qinit, info = controller.ComputeTargetPosture(x_nom, x_nom[(nq - 3):nq], backoff_distance=0.0) if info != 1: print "Warning: initial condition IK solve returned info ", info xinit = np.zeros(x_nom.shape) xinit[0:(nq - 3)] = qinit[0:-3] xinit[(nq - 3):nq] = x_nom[(nq - 3):nq] builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) for i in range(num_fingers): builder.Connect(controller.get_output_port(i), plant.get_input_port(i)) # Create a state logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # And a contact result logger, same rate contact_log = builder.AddSystem(PlanarHandContactLogger(controller, plant)) contact_log.DeclarePeriodicPublish(0.0333, 0.0) builder.Connect(plant.contact_results_output_port(), contact_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. # simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.005) # Set the initial state state = simulator.get_mutable_context().\ get_mutable_continuous_state_vector() state.SetFromVector(xinit) # Simulate! simulator.StepTo(duration) return tree, plant, controller, state_log, contact_log
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() # Load in the hopper from a description file. # It's spawned with a fixed floating base because # the robot description file includes the world as its # root link -- it does this so that I can create a robot # system with planar dynamics manually. (Drake doesn't have # a planar floating base type accessible right now that I # know about -- it only has 6DOF floating base types.) tree = RigidBodyTree() AddModelInstancesFromSdfString( open("raibert_hopper_2d.sdf", 'r').read(), FloatingBaseType.kFixed, None, tree) # A RigidBodyPlant wraps a RigidBodyTree to allow # forward dynamical simulation. It handles e.g. collision # modeling. plant = builder.AddSystem(RigidBodyPlant(tree)) # Alter the ground material used in simulation to make # it dissipate more energy (to make the hopping more critical) # and stickier (to make the hopper less likely to slip). allmaterials = CompliantMaterial() allmaterials.set_youngs_modulus(1E8) # default 1E9 allmaterials.set_dissipation(1.0) # default 0.32 allmaterials.set_friction(1.0) # default 0.9. plant.set_default_compliant_material(allmaterials) # Spawn a controller and hook it up controller = builder.AddSystem( Hopper2dController(tree, desired_lateral_velocity=desired_lateral_velocity, print_period=print_period)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) # Create a logger to log at 30hz state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) state_log._DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) # Create a simulator diagram = builder.Build() simulator = Simulator(diagram) # Don't limit realtime rate for this sim, since we # produce a video to render it after simulating the whole thing. #simulator.set_target_realtime_rate(100.0) simulator.set_publish_every_time_step(False) # Force the simulator to use a fixed-step integrator, # which is much faster for this stiff system. (Due to the # spring-model of collision, the default variable-timestep # integrator will take very short steps. I've chosen the step # size here to be fast while still being stable in most situations.) integrator = simulator.get_mutable_integrator() integrator.set_fixed_step_mode(True) integrator.set_maximum_step_size(0.0005) # Set the initial state state = simulator.get_mutable_context( ).get_mutable_continuous_state_vector() state.SetFromVector(x0) # Simulate! simulator.StepTo(duration) return tree, controller, state_log
# Setup diagram for simulation diagram = makeDiagram(n_quadrotors, n_balls, use_visualizer=True, trajectory_u=u_opt_poly_all, trajectory_x=x_opt_poly_all, trajectory_K=K_poly_all) ################################################################################### # Animate # plt.figure(figsize=(20, 10)) # plot_system_graphviz(diagram) # plt.show() # Set up a simulator to run this diagram simulator = Simulator(diagram) integrator = simulator.get_mutable_integrator() integrator.set_maximum_step_size( 0.01) # Reduce the max step size so that we can always detect collisions context = simulator.get_mutable_context() context.SetAccuracy(1e-4) ##############################################3 # # Simulate duration = x_opt_poly_all.end_time() # context.SetTime(0.) # context.SetContinuousState(state_init) # simulator.Initialize() # simulator.AdvanceTo(duration) t_arr = np.linspace(0, duration, 100) context.SetTime(0.)
sys.plant.get_actuation_input_port()) logger = builder.AddSystem(SignalLogger(7)) builder.Connect(iLQR_ctrl.get_output_port(), logger.get_input_port()) diagram = builder.Build() context = diagram.CreateDefaultContext() sys.ports_init(context) sys.deriv = derivatives(sys) sys.meshcat.load() diagram.Publish(context) simulator = Simulator(diagram, context) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) simulator.get_mutable_integrator().set_fixed_step_mode(True) simulator.get_mutable_integrator().set_maximum_step_size(dt) iLQR_ctrl.simulator = simulator inf_opt = info_optimizer(iLQR_ctrl) print('Tuning LQR') iLQR_ctrl.run(max_iter=5, regu=5e-5, expected_cost_redu_thresh=1.0, do_final_plot=False) #print(inf_opt.grad_directed_info(0)) #print(inf_opt.grad_directed_info(1)) #print(inf_opt.grad_directed_info(2))
def draw(self, context): xy = self.EvalVectorInput(context, 0).CopyToVector() self.lines.set_xdata(xy[:self.num_particles]) self.lines.set_ydata(xy[self.num_particles:]) self.ax.set_title('t = ' + str(context.get_time())) builder = DiagramBuilder() num_particles = 5000 xlim = [-2.75, 2.75] ylim = [-3.25, 3.25] draw_timestep = .25 sys = builder.AddSystem(VanDerPolParticles(num_particles)) visualizer = builder.AddSystem(Particle2DVisualizer(num_particles, xlim, ylim, draw_timestep)) builder.Connect(sys.get_output_port(0), visualizer.get_input_port(0)) AddRandomInputs(.1, builder) # TODO(russt): Plot nominal limit cycle. diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) simulator.get_mutable_integrator().set_fixed_step_mode(True) simulator.get_mutable_integrator().set_maximum_step_size(0.1) simulator.StepTo(20) plt.show()