def Simulate2dRamone(x0, duration, desired_goal = 0.0, print_period = 0.0): builder = DiagramBuilder() tree = RigidBodyTree() AddModelInstanceFromUrdfFile("ramone_act.urdf", FloatingBaseType.kRollPitchYaw, None, tree) plant = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem( Ramone2dController(tree, desired_goal=desired_goal, 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)) state_log = builder.AddSystem(SignalLogger(plant.get_num_states())) builder.Connect(plant.get_output_port(0), state_log.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) simulator.get_mutable_context().set_accuracy(1e-4) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector(x0) simulator.StepTo(duration) return tree, controller, state_log, plant
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort( plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def runPendulumExample(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant) parser.AddModelFromFile(FindResource("pendulum/pendulum.urdf")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, Tview=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.StepTo(args.duration)
def Simulate2dHopper(x0, duration, actuators_off=False, desired_lateral_velocity=0.0, desired_height=3.0, print_period=0.0): # The diagram, plant and contorller diagram = build_block_diagram(actuators_off, desired_lateral_velocity, desired_height, print_period) # Start visualizer recording visualizer = diagram.GetSubsystemByName('visualizer') visualizer.start_recording() simulator = Simulator(diagram) simulator.Initialize() plant = diagram.GetSubsystemByName('plant') plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) potential = plant.CalcPotentialEnergy(plant_context) simulator.AdvanceTo(duration) visualizer.stop_recording() animation = visualizer.get_recording_as_animation() controller = diagram.GetSubsystemByName('controller') state_log = diagram.GetSubsystemByName('state_log') return plant, controller, state_log, animation
def simulate(): # Animate the resulting policy. builder = DiagramBuilder() plant = builder.AddSystem(DoubleIntegrator()) vi_policy = builder.AddSystem(policy) builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0)) builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) logger = LogOutput(plant.get_output_port(0), builder) logger.set_name("logger") simulator.get_mutable_context().SetContinuousState(env.reset()) simulator.AdvanceTo(10. if get_ipython() is not None else 5.) return logger
def simulateRobot(time, B, v_command): tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() robot = builder.AddSystem(RigidBodyPlant(tree)) controller = builder.AddSystem(DController(tree, B, v_command)) builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # Set the initial conditions context = simulator.get_mutable_context() state = context.get_mutable_continuous_state_vector() start1 = 3 * np.pi / 16 start2 = 15 * np.pi / 16 #np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps state.SetFromVector( (start1, start2, -start1, -start2, np.pi + start1, start2, np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(time) #import pdb; pdb.set_trace() return (logger.data()[8:11, :], logger.data()[:8, :], logger.data()[19:22, :], logger.data()[11:19, :], logger.sample_times())
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 jacobian_controller_example(): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation()) station.SetupClutterClearingStation() station.Finalize() controller = builder.AddSystem( PseudoInverseController(station.get_multibody_plant())) integrator = builder.AddSystem(Integrator(7)) builder.Connect(controller.get_output_port(), integrator.get_input_port()) builder.Connect(integrator.get_output_port(), station.GetInputPort("iiwa_position")) builder.Connect(station.GetOutputPort("iiwa_position_measured"), controller.get_input_port()) meshcat = ConnectMeshcatVisualizer( builder, station.get_scene_graph(), output_port=station.GetOutputPort("pose_bundle")) diagram = builder.Build() simulator = Simulator(diagram) station_context = station.GetMyContextFromRoot( simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros((7, 1))) station.GetInputPort("wsg_position").FixValue(station_context, [0.1]) integrator.GetMyContextFromRoot(simulator.get_mutable_context( )).get_mutable_continuous_state_vector().SetFromVector( station.GetIiwaPosition(station_context)) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(0.01) return simulator
def simulate_acrobot(): builder = DiagramBuilder() acrobot = builder.AddSystem(AcrobotPlant()) saturation = builder.AddSystem(Saturation(min_value=[-10], max_value=[10])) builder.Connect(saturation.get_output_port(0), acrobot.get_input_port(0)) wrapangles = WrapToSystem(4) wrapangles.set_interval(0, 0, 2. * math.pi) wrapangles.set_interval(1, -math.pi, math.pi) wrapto = builder.AddSystem(wrapangles) builder.Connect(acrobot.get_output_port(0), wrapto.get_input_port(0)) controller = builder.AddSystem(BalancingLQR()) builder.Connect(wrapto.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), saturation.get_input_port(0)) tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"), FloatingBaseType.kFixed) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])) builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) context = simulator.get_mutable_context() state = context.get_mutable_continuous_state_vector() parser = argparse.ArgumentParser() parser.add_argument("-N", "--trials", type=int, help="Number of trials to run.", default=5) parser.add_argument("-T", "--duration", type=float, help="Duration to run each sim.", default=4.0) args = parser.parse_args() print(AcrobotPlant) for i in range(args.trials): context.set_time(0.) state.SetFromVector(UprightState().CopyToVector() + 0.05 * np.random.randn(4, )) simulator.StepTo(args.duration)
def Simulate2dHopper(x0, duration, desired_lateral_velocity=0.0, print_period=0.0): builder = DiagramBuilder() plant = builder.AddSystem(MultibodyPlant(0.0005)) scene_graph = builder.AddSystem(SceneGraph()) plant.RegisterAsSourceForSceneGraph(scene_graph) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) # Build the plant parser = Parser(plant) parser.AddModelFromFile("raibert_hopper_2d.sdf") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground")) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Create a logger to log at 30hz state_dim = plant.num_positions() + plant.num_velocities() state_log = builder.AddSystem(SignalLogger(state_dim)) state_log.DeclarePeriodicPublish(0.0333, 0.0) # 30hz logging builder.Connect(plant.get_continuous_state_output_port(), state_log.get_input_port(0)) # The controller controller = builder.AddSystem( Hopper2dController(plant, desired_lateral_velocity=desired_lateral_velocity, print_period=print_period)) builder.Connect(plant.get_continuous_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) # The diagram diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) simulator.StepTo(duration) return plant, controller, state_log
def animate_cartpole(policy, duration=10.): # Animate the resulting policy. builder = DiagramBuilder() tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf", FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) plant_system = builder.AddSystem(plant) # TODO(russt): add wrap-around logic to barycentric mesh # (so the policy has it, too) class WrapTheta(VectorSystem): def __init__(self): VectorSystem.__init__(self, 4, 4) def _DoCalcVectorOutput(self, context, input, state, output): output[:] = input twoPI = 2.*math.pi output[1] = output[1] - twoPI * math.floor(output[1] / twoPI) wrap = builder.AddSystem(WrapTheta()) builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0)) vi_policy = builder.AddSystem(policy) builder.Connect(wrap.get_output_port(0), vi_policy.get_input_port(0)) builder.Connect(vi_policy.get_output_port(0), plant_system.get_input_port(0)) logger = builder.AddSystem(SignalLogger(4)) logger._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant_system.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector([-1., math.pi-1, 1., -1.]) # Do the sim. simulator.StepTo(duration) # Visualize the result as a video. vis = PlanarRigidBodyVisualizer(tree, xlim=[-12.5, 12.5], ylim=[-1, 2.5]) ani = vis.animate(logger, repeat=True) # plt.show() # Things added to get visualizations in an ipynb plt.close(vis.fig) HTML(ani.to_html5_video())
def RunPendulumSimulation(pendulum_plant, pendulum_controller, x0=[0.9, 0.0], duration=10): ''' Accepts a pendulum_plant (which should be a DampedOscillatingPendulumPlant) and simulates it for 'duration' seconds from initial state `x0`. Returns a logger object which can return simulated timesteps ` logger.sample_times()` (N-by-1) and simulated states `logger.data()` (2-by-N). ''' # Create a simple block diagram containing the plant in feedback # with the controller. builder = DiagramBuilder() plant = builder.AddSystem(pendulum_plant) controller = builder.AddSystem(pendulum_controller) 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 capture the simulation of our plant # (We tell the logger to expect a 3-variable input, # and hook it up to the pendulum plant's 3-variable output.) logger = builder.AddSystem(SignalLogger(3)) logger.DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) simulator.Initialize() simulator.set_publish_every_time_step(False) # Set the initial conditions for the simulation. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() state.SetFromVector(x0) # Simulate for the requested duration. simulator.AdvanceTo(duration) # Return the logger, which stores the output of the # plant across the time steps of the simulation. return logger
def simulate(initial_state, const_input, boat, controller, duration): diagram = build_block_diagram(boat, controller) # set up a simulator simulator = Simulator(diagram) simulator_context = simulator.get_mutable_context() # set the initial conditions boat = diagram.GetSubsystemByName('boat') boat_context = diagram.GetMutableSubsystemContext(boat, simulator_context) boat_context.get_mutable_continuous_state_vector().SetFromVector(initial_state) # set inputs to boat simulator_context.FixInputPort(0, const_input) # simulate from time zero to duration simulator.AdvanceTo(duration) return diagram.GetSubsystemByName('logger')
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()
def runManipulationExample(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupDefaultStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, Tview=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.StepTo(args.duration)
def simulate(q0, qdot0, sim_time, controller): # initialize block diagram builder = DiagramBuilder() # add system and controller double_integrator = builder.AddSystem(get_double_integrator()) controller = builder.AddSystem(controller) # wirw system and controller builder.Connect(double_integrator.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), double_integrator.get_input_port(0)) # measure double-integrator state and input state_logger = LogOutput(double_integrator.get_output_port(0), builder) input_logger = LogOutput(controller.get_output_port(0), builder) # finalize block diagram diagram = builder.Build() # instantiate simulator simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) # makes sim faster # set initial conditions context = simulator.get_mutable_context() context.SetContinuousState([q0, qdot0]) # run simulation simulator.AdvanceTo(sim_time) # unpack sim results q_sim, qdot_sim = state_logger.data() u_sim = input_logger.data().flatten() t_sim = state_logger.sample_times() return q_sim, qdot_sim, u_sim, t_sim
def main(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) path = subprocess.check_output([ 'venv/share/drake/common/resource_tool', '-print_resource_path', 'drake/examples/multibody/cart_pole/cart_pole.sdf', ]).strip() Parser(plant).AddModelFromFile(path) plant.Finalize() # Add to visualizer. DrakeVisualizer.AddToBuilder(builder, scene_graph) # Add controller. controller = builder.AddSystem(BalancingLQR(plant)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) diagram = builder.Build() # Set up a simulator to run this diagram. simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() # Simulate. duration = 8.0 for i in range(5): initial_state = UPRIGHT_STATE + 0.1 * np.random.randn(4) print(f"Iteration: {i}. Initial: {initial_state}") context.SetContinuousState(initial_state) context.SetTime(0.0) simulator.Initialize() simulator.AdvanceTo(duration)
controller = builder.AddSystem(QuadIlqrMpcController()) logger_x = builder.AddSystem(SignalLogger(n)) logger_u = builder.AddSystem(SignalLogger(m)) builder.Connect(controller.get_output_port(0), quad.get_input_port(0)) builder.Connect(quad.get_output_port(0), logger_x.get_input_port(0)) builder.Connect(quad.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), logger_u.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) # if __name__ == '__main__': # Set the initial conditions, x(0). state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector(traj_specs.x0) input_vector = simulator.get_mutable_context( ).get_mutable_discrete_state_vector() input_vector.SetFromVector(traj_specs.u0) #%% Simulate simulator.StepTo(h * 300) #%% plot PlotTraj(logger_x.data().T, dt=None, xw_list=traj_specs.xw_list, t=logger_x.sample_times()) #%% open meshcat vis = meshcat.Visualizer()
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # kinsol = rtree.doKinematics(q) # com = rtree.centerOfMass(kinsol) # print com # nominal standing state # state = simulator.get_mutable_context().get_mutable_continuous_state_vector() # state.SetFromVector(q+qd) cassie.set_state_vector(simulator.get_mutable_context(), x) simulator.StepTo(1.0) # simulator.Initialize(); # from bot_lcmgl import lcmgl # import lcm as true_lcm # viz_lcmgl = lcmgl("Visualize-Points", true_lcm.LCM()) # cache = rtree.doKinematics(x[:23], x[23:]) # thigh_l = rtree.transformPoints(cache, GetFourBarHipMountPoint(), # rtree.FindBodyIndex("thigh_left"), 0) # heel_l = rtree.transformPoints(cache, GetFourBarHeelMountPoint(), # rtree.FindBodyIndex("heel_spring_left"), 0) # thigh_r = rtree.transformPoints(cache, -GetFourBarHipMountPoint(), # rtree.FindBodyIndex("thigh_right"), 0)
# Create a simulator for it. simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Simulator time steps will be very small, so don't # force the rest of the system to update every single time. simulator.set_publish_every_time_step(False) # From iiwa_wsg_simulation.cc: # When using the default RK3 integrator, the simulation stops # advancing once the gripper grasps the box. Grasping makes the # problem computationally stiff, which brings the default RK3 # integrator to its knees. timestep = 0.00005 integrator = RungeKutta2Integrator(diagram, timestep, simulator.get_mutable_context()) simulator.reset_integrator(integrator) # The simulator simulates forward from a given Context, # so we adjust the simulator's initial Context to set up # the initial state. state = simulator.get_mutable_context().\ get_mutable_continuous_state_vector() initial_state = np.zeros(x.shape) initial_state[0:x.shape[0]] = x.copy() state.SetFromVector(initial_state) simulator.get_mutable_context().set_time(t) # This kicks off simulation. rbt_new = None try:
import matplotlib.pyplot as plt from pydrake.all import (DiagramBuilder, SignalLogger, Simulator) from discrete_time_system import * # Create a simple block diagram containing our system. builder = DiagramBuilder() system = builder.AddSystem(SimpleDiscreteTimeSystem()) logger = builder.AddSystem(SignalLogger(1)) builder.Connect(system.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) # Set the initial conditions, x(0). state = simulator.get_mutable_context().get_mutable_discrete_state_vector() state.SetFromVector([0.9]) # Simulate for 10 seconds. simulator.StepTo(10) # Plot the results. plt.stem(logger.sample_times(), logger.data().transpose()) plt.xlabel('t') plt.ylabel('x(t)') plt.show()
tree.compile() builder = DiagramBuilder() compass_gait = builder.AddSystem(CompassGait()) parser = argparse.ArgumentParser() parser.add_argument("-T", "--duration", type=float, help="Duration to run sim.", default=10.0) args = parser.parse_args() visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, xlim=[-1., 5.], ylim=[-1., 2.], figsize_multiplier=2)) builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() diagram.Publish(context) # draw once to get the window open context.set_accuracy(1e-4) context.SetContinuousState([0., 0., 0.4, -2.]) simulator.StepTo(args.duration)
def lyapunov_simulation( x0, xf, uref=0, create_dynamics=create_symbolic_dynamics, to_point=False, a=0.1, u_max=0.5, eps=2.): ''' Simulates and plots our Lyapunov Controller in different scenarios. Parameters: x0: initial state (x, y, yaw) x1: final state (x, y, yaw) uref: reference input create_dynamics: symbolic dynamics creation function a, eps: lyapunov controller parameters u_max: control limit (u_max = 1/r) ''' params = np.array([a, u_max, eps]) uavPlant = create_dynamics() # construction site for our closed-loop system builder = DiagramBuilder() # add the robot to the diagram # the method .AddSystem() simply returns a pointer to the system # we passed as input, so it's ok to give it the same name uav = builder.AddSystem(uavPlant) # add the controller if not to_point: controller = builder.AddSystem(LyapunovController(lyapunov_controller, params, xf)) else: controller = builder.AddSystem(LyapunovController(lyapunov_controller_to_point, uref, xf)) # wire the controller with the system builder.Connect(uavPlant.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), uavPlant.get_input_port(0)) # add a logger to the diagram # this will store the state trajectory logger = LogOutput(uavPlant.get_output_port(0), builder) control_logger = LogOutput(controller.get_output_port(0), builder) # complete the construction of the diagram diagram = builder.Build() # reset logger to delete old trajectories # this is needed if you want to simulate the system multiple times logger.reset() # set up a simulation environment simulator = Simulator(diagram) # simulator.set_target_realtime_rate(1) # set the initial cartesian state to a random initial position # try initial_state = np.random.randn(3) for a random initial state context = simulator.get_mutable_context() context.SetContinuousState(x0) # simulate from zero to sim_time # the trajectory will be stored in the logger if not to_point: sim_time = 400 else: sim_time = 0.1 simulator.AdvanceTo(sim_time) # print('done!') # draw_simulation(logger.data()) return (simulator, logger.data(), control_logger.data())
controller = builder.AddSystem(Diff_Drive_Controller(plant)) builder.Connect(plant.get_continuous_state_output_port(), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), plant.get_actuation_input_port()) #Data Logger logger = LogOutput(plant.get_continuous_state_output_port(), builder) #Run Simulation diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.) plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) print plant_context.get_mutable_discrete_state_vector() plant_context.get_mutable_discrete_state_vector().SetFromVector(x0) simulator.Initialize() simulator.StepTo(duration) #print('sample_times',logger.sample_times()) #nx1 array print('Final State: ', logger.data()[:, -1]) #nxm array x_log = logger.data()[4, :] data = logger.data() theta_log = np.asarray([ math.asin(2 * (data[0, i] * data[2, i] - data[1, i] * data[3, i])) for i in range(data.shape[1]) ]) #theta = math.asin(2*(x[0]*x[2] - x[1]*x[3]))
def getPredictedMotion(B, v_command, time): #object_positions = object_positions + 0.1 #manipulator_positions = manipulator_positions + 0.1 #object_velocities = object_velocities + 0.1 #manipulator_velocities = manipulator_velocities + 0.1 #object_positions = object_positions[:, range(0,object_positions.shape[1],2)] #manipulator_positions = manipulator_positions[:, range(0,manipulator_positions.shape[1],2)] #object_velocities = object_velocities[:, range(0,object_velocities.shape[1],2)] #manipulator_velocities = manipulator_velocities[:, range(0,manipulator_velocities.shape[1],2)] #times = times[range(0,times.size,2)] #import pdb; pdb.set_trace() step = 0.01 A = 10 * np.eye(3) tree = RigidBodyTree( FindResource( os.path.dirname(os.path.realpath(__file__)) + "/block_pusher2.urdf"), FloatingBaseType.kFixed) # Set up a block diagram with the robot (dynamics), the controller, and a # visualization block. builder = DiagramBuilder() #robot = builder.AddSystem(RigidBodyPlant(tree)) robot = builder.AddSystem( QuasiStaticRigidBodyPlant(tree, step, A, np.linalg.inv(B))) controller = builder.AddSystem(QController(tree, v_command, step)) #builder.Connect(robot.get_output_port(0), controller.get_input_port(0)) builder.Connect(controller.get_output_port(0), robot.get_input_port(0)) Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarRigidBodyVisualizer(tree, Tview, xlim=[-2.8, 4.8], ylim=[-2.8, 10])) builder.Connect(robot.get_output_port(0), visualizer.get_input_port(0)) logger = builder.AddSystem( SignalLogger(tree.get_num_positions() + tree.get_num_velocities())) builder.Connect(robot.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Set up a simulator to run this diagram simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(True) # Set the initial conditions context = simulator.get_mutable_context() state = context.get_mutable_discrete_state_vector() start1 = 3 * np.pi / 16 start2 = 15 * np.pi / 16 #np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps, np.pi/6 - eps, 2*np.pi/3 + eps, -np.pi/6 + eps, -2*np.pi/3 - eps state.SetFromVector( (start1, start2, -start1, -start2, np.pi + start1, start2, np.pi - start1, -start2, 1, 1, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.)) # (theta1, theta2, theta1dot, theta2dot) # Simulate for 10 seconds simulator.StepTo(time) #import pdb; pdb.set_trace() return (logger.data()[8:11, :], logger.data()[:8, :], logger.data()[19:22, :], logger.data()[11:19, :], logger.sample_times())
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
builder = DiagramBuilder() acrobot = builder.AddSystem(RigidBodyPlant(tree)) parser = argparse.ArgumentParser() parser.add_argument("-T", "--duration", type=float, help="Duration to run sim.", default=10000.0) args = parser.parse_args() visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])) builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0)) ax = visualizer.fig.add_axes([.2, .95, .6, .025]) torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.)) builder.Connect(torque_system.get_output_port(0), acrobot.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) context = simulator.get_mutable_context() context.SetContinuousState([1., 0., 0., 0.]) simulator.StepTo(args.duration)
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
builder = DiagramBuilder() compass_gait = builder.AddSystem(CompassGait()) parser = argparse.ArgumentParser() parser.add_argument("-T", "--duration", type=float, help="Duration to run sim.", default=10.0) args = parser.parse_args() logger = builder.AddSystem(SignalLogger(14)) builder.Connect(compass_gait.get_output_port(1), logger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(True) simulator.get_mutable_context().SetAccuracy(1e-4) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() state.SetFromVector([0., 0., 0.4, -2.]) simulator.StepTo(args.duration) plt.figure() plt.plot(logger.data()[4, :], logger.data()[11, :]) plt.xlabel('left leg angle') plt.ylabel('left leg angular velocity') plt.show()
import matplotlib.pyplot as plt from pydrake.all import DiagramBuilder, SignalLogger, Simulator from discrete_time_system import * # Create a simple block diagram containing our system. builder = DiagramBuilder() system = builder.AddSystem(SimpleDiscreteTimeSystem()) logger = builder.AddSystem(SignalLogger(1)) builder.Connect(system.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() # Create the simulator. simulator = Simulator(diagram) # Set the initial conditions, x(0). state = simulator.get_mutable_context().get_mutable_discrete_state_vector() state.SetFromVector([0.9]) # Simulate for 10 seconds. simulator.AdvanceTo(10) # Plot the results. plt.stem(logger.sample_times(), logger.data().transpose()) plt.xlabel("t") plt.ylabel("x(t)") plt.show()
# And also log signalLogRate = 60 signalLogger = builder.AddSystem(SignalLogger(nx)) signalLogger.DeclarePeriodicPublish(1. / signalLogRate, 0.0) builder.Connect(rbplant_sys.get_output_port(0), signalLogger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) # TODO(russt): Clean up state vector access below. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() if set_initial_state: initial_state = np.zeros((nx, 1)) initial_state[0] = 1.0 state.SetFromVector(initial_state) simulator.StepTo(args.duration) print(state.CopyToVector()) # Generate an animation of whatever happened ani = visualizer.animate(signalLogger, repeat=True) if (args.animate): print("Animating the simulation on repeat -- "
# Done! Compile it all together and visualize it. diagram = builder.Build() kuka_utils.render_system_with_graphviz(diagram, "view.gv") # Create a simulator for it. simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Simulator time steps will be very small, so don't # force the rest of the system to update every single time. simulator.set_publish_every_time_step(False) # The simulator simulates forward from a given Context, # so we adjust the simulator's initial Context to set up # the initial state. state = simulator.get_mutable_context().\ get_mutable_continuous_state_vector() initial_state = np.zeros(state.size()) initial_state[0:q0.shape[0]] = q0 state.SetFromVector(initial_state) # From iiwa_wsg_simulation.cc: # When using the default RK3 integrator, the simulation stops # advancing once the gripper grasps the box. Grasping makes the # problem computationally stiff, which brings the default RK3 # integrator to its knees. timestep = 0.0002 simulator.reset_integrator( RungeKutta2Integrator(diagram, timestep, simulator.get_mutable_context())) # This kicks off simulation. Most of the run time will be spent
def simulate_and_log_policy_system(policy_system, expmt, ic=None): global tree global logger expmt_settings = { "pendulum": { 'constructor_or_path': PendulumPlant, 'state_dim': 2, 'twoPI_boundary_condition_state_idxs': (0,), 'initial_state': [0.1, 0.0], }, "acrobot": { 'constructor_or_path': "/opt/underactuated/src/acrobot/acrobot.urdf", 'state_dim': 4, 'twoPI_boundary_condition_state_idxs': (0, 1), 'initial_state': [0.5, 0.5, 0.0, 0.0], }, "cartpole": { 'constructor_or_path': "/opt/underactuated/src/cartpole/cartpole.urdf", 'state_dim': 4, 'twoPI_boundary_condition_state_idxs': (1,), 'initial_state': [0.5, 0.5, 0.0, 0.0], }, } assert expmt in expmt_settings.keys() # Animate the resulting policy. settings = expmt_settings[expmt] builder = DiagramBuilder() constructor_or_path = settings['constructor_or_path'] if not isinstance(constructor_or_path, str): plant = constructor_or_path() else: tree = RigidBodyTree(constructor_or_path, FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) plant_system = builder.AddSystem(plant) # TODO(russt): add wrap-around logic to barycentric mesh # (so the policy has it, too) class WrapTheta(VectorSystem): def __init__(self): VectorSystem.__init__(self, settings['state_dim'], settings['state_dim']) def _DoCalcVectorOutput(self, context, input, state, output): output[:] = input twoPI = 2.*math.pi for idx in settings['twoPI_boundary_condition_state_idxs']: # output[idx] += math.pi output[idx] = output[idx]# - twoPI * math.floor(output[idx] / twoPI) # output[idx] -= math.pi wrap = builder.AddSystem(WrapTheta()) builder.Connect(plant_system.get_output_port(0), wrap.get_input_port(0)) vi_policy = builder.AddSystem(policy_system) builder.Connect(wrap.get_output_port(0), vi_policy.get_input_port(0)) builder.Connect(vi_policy.get_output_port(0), plant_system.get_input_port(0)) x_logger = builder.AddSystem(SignalLogger(settings['state_dim'])) x_logger._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(plant_system.get_output_port(0), x_logger.get_input_port(0)) u_logger = builder.AddSystem(SignalLogger(1)) u_logger._DeclarePeriodicPublish(0.033333, 0.0) builder.Connect(vi_policy.get_output_port(0), u_logger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) state = simulator.get_mutable_context().get_mutable_continuous_state_vector() if ic is None: ic = settings['initial_state'] state.SetFromVector(ic) return simulator, tree, x_logger, u_logger
# And also log signalLogRate = 60 signalLogger = builder.AddSystem(SignalLogger(nx)) signalLogger.DeclarePeriodicPublish(1. / signalLogRate, 0.0) builder.Connect(rbplant_sys.get_output_port(0), signalLogger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) # TODO(russt): Clean up state vector access below. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() if set_initial_state: initial_state = np.zeros((nx, 1)) initial_state[0] = 1.0 state.SetFromVector(initial_state) simulator.StepTo(args.duration) if (args.animate): # Generate an animation of whatever happened ani = visualizer.animate(signalLogger) if meshcat_server_p is not None: meshcat_server_p.kill() meshcat_server_p.wait()