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()
