def main():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
body = add_body(plant, "welded_body")
add_geometry(plant, body)
plant.WeldFrames(plant.world_frame(), body.body_frame())
body = add_body(plant, "floating_body")
add_geometry(plant, body)
plant.SetDefaultFreeBodyPose(body, RigidTransform([1., 0, 1.]))
plant.Finalize()
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_context()
# plant.SetFreeBodyPose(context, body, X_WB)
# plant.SetFreeBodySpatialVelocity(body, V_WB, context)
# Should look at, show 40sec to 50sec.
# TODO(eric.cousineau): Without reset stats, it freezes? :(
# simulator.AdvanceTo(40.)
simulator.set_target_realtime_rate(100.)
# simulator.ResetStatistics()
dt = 0.1
while context.get_time() < 240.:
simulator.AdvanceTo(context.get_time() + dt)
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 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.AdvanceTo(args.duration)
def setup_manipulation_station(T_world_objectInitial, zmq_url, T_world_targetBin, manipuland="foam", include_bin=True, include_hoop=False):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=1e-3))
station.SetupClutterClearingStation()
if manipuland is "foam":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/061_foam_brick.sdf",
T_world_objectInitial)
elif manipuland is "ball":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/sphere.sdf",
T_world_objectInitial)
elif manipuland is "bball":
station.AddManipulandFromFile(
"drake/../../../../../../manipulation/sdfs/bball.sdf", # this is some path hackery
T_world_objectInitial)
elif manipuland is "rod":
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/rod.sdf",
T_world_objectInitial)
station_plant = station.get_multibody_plant()
parser = Parser(station_plant)
if include_bin:
parser.AddModelFromFile("extra_bin.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("extra_bin_base"), T_world_targetBin)
if include_hoop:
parser.AddModelFromFile("sdfs/hoop.sdf")
station_plant.WeldFrames(station_plant.world_frame(), station_plant.GetFrameByName("base_link_hoop"), T_world_targetBin)
station.Finalize()
frames_to_draw = {"gripper": {"body"}}
meshcat = None
if zmq_url is not None:
meshcat = ConnectMeshcatVisualizer(builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
delete_prefix_on_load=False,
frames_to_draw=frames_to_draw,
zmq_url=zmq_url)
diagram = builder.Build()
plant = station.get_multibody_plant()
context = plant.CreateDefaultContext()
gripper = plant.GetBodyByName("body")
initial_pose = plant.EvalBodyPoseInWorld(context, gripper)
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.01)
return initial_pose, meshcat
def simulate_scene_tree(scene_tree, T, timestep=0.001, with_meshcat=False):
builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg(
scene_tree, timestep=timestep)
mbp.Finalize()
if with_meshcat:
visualizer = ConnectMeshcatVisualizer(builder, scene_graph,
zmq_url="default")
diagram = builder.Build()
diag_context = diagram.CreateDefaultContext()
sim = Simulator(diagram)
sim.set_target_realtime_rate(1.0)
sim.AdvanceTo(T)
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 main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("-t1", default=0.05, help="Extend leg")
parser.add_argument("-t2", default=0.5, help="Dwell at top")
parser.add_argument("-t3", default=0.5, help="Contract leg")
parser.add_argument("-t4", default=0.1, help="Wait at bottom")
setup_argparse_for_setup_dot_diagram(parser)
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
t1 = float(args.t1)
t2 = float(args.t2)
t3 = float(args.t3)
t4 = float(args.t4)
q_crouch = np.array([
1600, 2100, 2000, 1600, 2100, 2000, 1400, 2100, 2000, 1400, 2100, 2000
])
q_extend = np.array([
1600, 1600, 2400, 1600, 1600, 2400, 1400, 1600, 2400, 1400, 1600, 2400
])
breaks = np.cumsum([0., t1, t2, t3, t4])
samples = np.stack([q_crouch, q_extend, q_extend, q_crouch, q_crouch]).T
trajectory = PiecewisePolynomial.FirstOrderHold(breaks, samples)
builder = DiagramBuilder()
plant, scene_graph, servo_controller = setup_dot_diagram(builder, args)
trajectory_source = builder.AddSystem(TrajectoryLooper(trajectory))
builder.Connect(trajectory_source.get_output_port(0),
servo_controller.get_input_port(0))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder,
output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat,
open_browser=args.open_browser)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
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 animate(plant, x_trajectory):
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(
MultibodyPositionToGeometryPose(plant, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2, 2],
ylim=[-1.25, 2],
ax=ax[0]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.AdvanceTo(x_trajectory.end_time())
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 simulate_drake_system(plant):
# Create a simple block diagram containing our system.
builder = DiagramBuilder()
system = builder.AddSystem(plant)
logger = LogOutput(system.get_output_port(0), builder)
diagram = builder.Build()
# Set the initial conditions, x(0).
context = diagram.CreateDefaultContext()
context.SetContinuousState([0, 0, 20, 10, 0, 0])
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram, context)
simulator.AdvanceTo(30)
# Plotting
x_sol = logger.data().T
plot_trj_3_wind(x_sol[:, 0:3], np.array([0, 0, 0]))
plt.show()
return 0
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 main():
parser = argparse.ArgumentParser(description=__doc__)
setup_argparse_for_setup_dot_diagram(parser)
parser.add_argument("--interactive", action='store_true')
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
q_init = np.array([
1700, 2000, 2000, 1700, 2000, 2000, 1300, 2000, 2000, 1300, 2000, 2000
])
builder = DiagramBuilder()
plant, scene_graph, servo_controller = setup_dot_diagram(builder, args)
if args.interactive:
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(ServoSliders(servo_controller))
sliders.set_position(q_init)
builder.Connect(sliders.get_output_port(0),
servo_controller.get_input_port(0))
else:
source = builder.AddSystem(
ConstantVectorSource(np.zeros(servo_controller.nu)))
builder.Connect(source.get_output_port(0),
servo_controller.get_input_port(0))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder,
output_port=scene_graph.get_query_output_port(),
zmq_url=args.meshcat,
open_browser=args.open_browser)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1E6)
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)
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 runManipulationExample(args):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation(time_step=0.005))
station.SetupManipulationClassStation()
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.AdvanceTo(args.duration)
# Use IK to find a nonpenetrating configuration of the scene from
# which to start simulation.
q0 = mbp.GetPositions(mbp_context).copy()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
constraint = ik.AddMinimumDistanceConstraint(0.01)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0])*1.0, q0, q_dec)
mbp.SetPositions(mbp_context, q0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
print("Initial guess: ", q0)
res = Solve(prog)
#print(prog.GetSolverId().name())
q0_proj = res.GetSolution(q_dec)
print("Final: ", q0_proj)
# Run a sim starting from whatever configuration IK figured out.
mbp.SetPositions(mbp_context, q0_proj)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
try:
simulator.AdvanceTo(2.0)
except Exception as e:
print("Exception in sim: ", e)
scene_k -= 1
# Set up a block diagram with the robot (dynamics) and a visualization block.
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
# Load the double pendulum from Universal Robot Description Format
parser = Parser(plant, scene_graph)
parser.AddModelFromFile(FindResource("double_pendulum/double_pendulum.urdf"))
plant.Finalize()
builder.ExportInput(plant.get_actuation_input_port())
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph,
xlim=[-2.8, 2.8],
ylim=[-2.8, 2.8]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.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)
# Set the initial conditions
context = simulator.get_mutable_context()
# state is (theta1, theta2, theta1dot, theta2dot)
context.SetContinuousState([1., 1., 0., 0.])
context.FixInputPort(0, [0., 0.]) # Zero input torques
# Simulate for 10 seconds
simulator.AdvanceTo(10)
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()
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree,
xlim=[-8., 8.],
ylim=[-2., 3.],
figsize_multiplier=3))
builder.Connect(rimless_wheel.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
diagram.GetMutableSubsystemContext(
rimless_wheel, context).get_numeric_parameter(0).set_slope(args.slope)
context.SetAccuracy(1e-4)
context.SetContinuousState([args.initial_angle, args.initial_angular_velocity])
simulator.AdvanceTo(args.duration)
u_logger = builder.AddSystem(SignalLogger(1))
builder.Connect(controller.get_output_port(0), u_logger.get_input_port(0))
diagram = builder.Build()
# Set up a simulator to run this diagram
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Simulate
duration = 20.0
context.SetTime(0.)
context.SetContinuousState(np.array([0.0, 0, 0.1, 0]))
simulator.Initialize()
simulator.AdvanceTo(duration)
data = np.copy(logger.data())
times = logger.sample_times()
u_data = np.copy(u_logger.data())
fig, axs = plt.subplots(5)
for i in range(data.shape[1]):
data[0, i] = (data[0, i]) % (2 * np.pi)
data[1, i] = (data[1, i]) % (2 * np.pi)
theta0_plt, = axs[0].plot(times, data[0, :])
theta1_plt, = axs[1].plot(times, data[1, :])
theta0_dot_plt, = axs[2].plot(times, data[2, :])
theta1_dot_plt, = axs[3].plot(times, data[3, :])
u_plt, = axs[4].plot(times, u_data[0, :])
# set the initial conditions
# do not change the initial conditions of the base
# since they must agree with the harmonic motion h*sin(omega*t)
context = simulator.get_mutable_context()
context.SetTime(0.) # reset current time
context.SetContinuousState((
0., # initial position of the base, DO NOT CHANGE!
theta, # initial angle of the pendulum
h*omega, # initial velocity of the base, DO NOT CHANGE!
theta_dot # initial angular velocity of the pendulum
))
# simulate from zero to sim_time
simulator.Initialize()
simulator.AdvanceTo(sim_time)
# stop the video and build the animation
visualizer.stop_recording()
ani = visualizer.get_recording_as_animation()
# play video
HTML(ani.to_jshtml())
"""We conclude by plotting the position of the base and the angular velocity of the pendulum as functions of time.
If we did our job correctly,
- the first should coincide with the desired position $h \sin (\omega t)$,
- the second should coincide with the response of the first-order system $\ddot \theta = f(\dot \theta)$ you came up with in point (a) of the exercise.
"""
# base position as a function of time
def project_tree_to_feasibility(tree,
constraints=[],
jitter_q=None,
do_forward_sim=False,
zmq_url=None,
prefix="projection",
timestep=0.001,
T=1.):
# Mutates tree into tree with bodies in closest
# nonpenetrating configuration.
builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \
compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep)
mbp.Finalize()
# Connect visualizer if requested. Wrap carefully to keep it
# from spamming the console.
if zmq_url is not None:
with open(os.devnull, 'w') as devnull:
with contextlib.redirect_stdout(devnull):
visualizer = ConnectMeshcatVisualizer(builder,
sg,
zmq_url=zmq_url,
prefix=prefix)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
q0 = mbp.GetPositions(mbp_context)
nq = len(q0)
if nq == 0:
logging.warn("Generated MBP had no positions.")
return tree
# Set up projection NLP.
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
# It's always a projection, so we always have this
# Euclidean norm error between the optimized q and
# q0.
prog.AddQuadraticErrorCost(np.eye(nq), q0, q_dec)
# Nonpenetration constraint.
ik.AddMinimumDistanceConstraint(0.001)
# Other requested constraints.
for constraint in constraints:
constraint.add_to_ik_prog(tree, ik, mbp, mbp_context,
node_to_free_body_ids_map)
# Initial guess, which can be slightly randomized by request.
q_guess = q0
if jitter_q:
q_guess = q_guess + np.random.normal(0., jitter_q, size=q_guess.size)
prog.SetInitialGuess(q_dec, q_guess)
# Solve.
solver = SnoptSolver()
options = SolverOptions()
logfile = "/tmp/snopt.log"
os.system("rm %s" % logfile)
options.SetOption(solver.id(), "Print file", logfile)
options.SetOption(solver.id(), "Major feasibility tolerance", 1E-3)
options.SetOption(solver.id(), "Major optimality tolerance", 1E-3)
options.SetOption(solver.id(), "Major iterations limit", 300)
result = solver.Solve(prog, None, options)
if not result.is_success():
logging.warn("Projection failed.")
print("Logfile: ")
with open(logfile) as f:
print(f.read())
qf = result.GetSolution(q_dec)
mbp.SetPositions(mbp_context, qf)
# If forward sim is requested, do a quick forward sim to get to
# a statically stable config.
if do_forward_sim:
sim = Simulator(diagram, diagram_context)
sim.set_target_realtime_rate(1000.)
sim.AdvanceTo(T)
# Reload poses back into tree
free_bodies = mbp.GetFloatingBaseBodies()
for body_id, node in body_id_to_node_map.items():
if body_id not in free_bodies:
continue
node.tf = drake_tf_to_torch_tf(
mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id)))
return tree
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
builder = DiagramBuilder()
source = builder.AddSystem(TrajectorySource(x_trajectory))
scene_graph = builder.AddSystem(SceneGraph())
AcrobotGeometry.AddToBuilder(builder, source.get_output_port(0), scene_graph)
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
simulator.AdvanceTo(x_trajectory.end_time())
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
plt.plot(times, u_values)
plt.xlabel("time (seconds)")
plt.ylabel("force (Newtons)")
plt.show()
def project_tree_to_feasibility_via_sim(tree,
constraints=[],
zmq_url=None,
prefix="projection",
timestep=0.0005,
T=10.):
# Mutates tree into tree with bodies in closest
# nonpenetrating configuration.
builder, mbp, sg, node_to_free_body_ids_map, body_id_to_node_map = \
compile_scene_tree_to_mbp_and_sg(tree, timestep=timestep)
mbp.Finalize()
# Connect visualizer if requested. Wrap carefully to keep it
# from spamming the console.
if zmq_url is not None:
with open(os.devnull, 'w') as devnull:
with contextlib.redirect_stdout(devnull):
visualizer = ConnectMeshcatVisualizer(builder,
sg,
zmq_url=zmq_url,
prefix=prefix)
# Forward sim under langevin forces
force_source = builder.AddSystem(
DecayingForceToDesiredConfigSystem(
mbp, mbp.GetPositions(mbp.CreateDefaultContext())))
builder.Connect(mbp.get_state_output_port(),
force_source.get_input_port(0))
builder.Connect(force_source.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diagram_context)
q0 = mbp.GetPositions(mbp_context)
nq = len(q0)
if nq == 0:
logging.warn("Generated MBP had no positions.")
return tree
# Make 'safe' initial configuration that randomly arranges objects vertically
#k = 0
#all_pos = []
#for node in tree:
# for body_id in node_to_free_body_ids_map[node]:
# body = mbp.get_body(body_id)
# tf = mbp.GetFreeBodyPose(mbp_context, body)
# tf = RigidTransform(p=tf.translation() + np.array([0., 0., 1. + k*0.5]), R=tf.rotation())
# mbp.SetFreeBodyPose(mbp_context, body, tf)
# k += 1
sim = Simulator(diagram, diagram_context)
sim.set_target_realtime_rate(1000)
sim.AdvanceTo(T)
# Reload poses back into tree
free_bodies = mbp.GetFloatingBaseBodies()
for body_id, node in body_id_to_node_map.items():
if body_id not in free_bodies:
continue
node.tf = drake_tf_to_torch_tf(
mbp.GetFreeBodyPose(mbp_context, mbp.get_body(body_id)))
return tree
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())
t_arr = np.linspace(0, duration, 100)
context.SetTime(0.)
context.SetContinuousState(state_init)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
visualizer = diagram.GetSubsystemByName('visualizer')
visualizer.start_recording()
# Plot
q_opt = np.zeros((100, 6 * n_quadrotors + 4 * n_balls))
q_actual = np.zeros((100, 6 * n_quadrotors + 4 * n_balls))
for i in range(100):
t = t_arr[i]
simulator.AdvanceTo(t_arr[i])
q_opt[i, :] = x_opt_poly_all.value(t).flatten()
q_actual[i, :] = context.get_continuous_state_vector().CopyToVector()
ani = visualizer.get_recording_as_animation()
Writer = animation.writers['ffmpeg']
writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800)
ani.save('animation.mp4', writer=writer)
#plotting
for i in range(n_quadrotors):
ind_i = 6 * i
ind_f = ind_i + 3
plt.figure(figsize=(6, 3))
plt.plot(t_arr, q_opt[:, ind_i:ind_f])
# plt.figure(figsize=(6, 3))
source = builder.AddSystem(TrajectorySource(x_opt_poly))
builder.AddSystem(scene_graph)
pos_to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(compass_gait, input_multibody_state=True))
builder.Connect(source.get_output_port(0), pos_to_pose.get_input_port())
builder.Connect(pos_to_pose.get_output_port(), scene_graph.get_source_pose_port(compass_gait.get_source_id()))
# add visualizer
xlim = [-.75, 1.]
ylim = [-.2, 1.5]
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph, xlim=xlim, ylim=ylim, show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
simulator = Simulator(builder.Build())
# generate and display animation
visualizer.start_recording()
simulator.AdvanceTo(x_opt_poly.end_time())
ani = visualizer.get_recording_as_animation()
HTML(ani.to_jshtml())
"""## Plot the Results
Here are two plots to visualize the results of the trajectory optimization.
In the first we plot the limit cycle we found in the plane of the leg angles.
To show a complete cycle, we "mirror" the trajectory of the first step and we plot it too ("Red leg swinging").
"""
# plot swing trajectories
# the second is the mirrored one
plt.plot(q_opt[:, 2], q_opt[:, 3], color='b', label='Blue leg swinging')
plt.plot(q_opt[:, 2] + q_opt[:, 3], - q_opt[:, 3], color='r', label='Red leg swinging')
#prog = opt.get_mutable_prog()
#AddUnitQuaternionConstraintOnPlant(
# mbp_ad, q_vars, prog)
#q_targ = mbp.GetPositions(mbp.CreateDefaultContext())
## Penalize deviation from target configuration
#prog.AddQuadraticErrorCost(np.eye(q_targ.shape[0]), q_targ, q_vars)
#prog.SetInitialGuess(q_vars, q_targ)
#solver = SnoptSolver()
#result = solver.Solve(opt.prog())
#print(result, result.is_success())
#q_0 = result.GetSolution(q_vars)
#print("Q found: ", q_0)
builder, mbp, scene_graph = build_mbp(seed=seed)
q_des = mbp.GetPositions(mbp.CreateDefaultContext())
forcer = builder.AddSystem(DecayingForceToDesiredConfigSystem(mbp, q_des))
builder.Connect(mbp.get_state_output_port(), forcer.get_input_port(0))
builder.Connect(forcer.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
visualizer = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url="default")
diagram = builder.Build()
diag_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diag_context)
mbp.SetPositions(mbp_context, np.random.random(q_des.shape) * 10.0)
sim = Simulator(diagram, diag_context)
#sim.set_target_realtime_rate(1.0)
sim.AdvanceTo(10.0)
