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.StepTo(args.duration)
def RunSimulation(quadrotor_plant,
control_law,
x0=np.random.random((8, 1)),
duration=30,
control_period=0.0333,
print_period=1.0,
simulation_period=0.0333):
quadrotor_controller = QuadrotorController(control_law,
control_period=control_period,
print_period=print_period)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
QuadrotorPendulum(mb=quadrotor_plant.mb,
lb=quadrotor_plant.lb,
m1=quadrotor_plant.m1,
l1=quadrotor_plant.l1,
g=quadrotor_plant.g,
input_max=quadrotor_plant.input_max))
controller = builder.AddSystem(quadrotor_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
input_log = builder.AddSystem(SignalLogger(2))
input_log._DeclarePeriodicPublish(control_period, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(8))
state_log._DeclarePeriodicPublish(control_period, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
simulator = Simulator(diagram, context)
simulator.Initialize()
simulator.set_publish_every_time_step(True)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(control_period)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
def RunSimulation(pendulum_plant,
control_law,
x0=np.random.random((4, 1)),
duration=30):
pendulum_controller = PendulumController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
InertialWheelPendulum(m1=pendulum_plant.m1,
l1=pendulum_plant.l1,
m2=pendulum_plant.m2,
l2=pendulum_plant.l2,
r=pendulum_plant.r,
g=pendulum_plant.g,
input_max=pendulum_plant.input_max))
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
input_log = builder.AddSystem(SignalLogger(1))
input_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(4))
state_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
simulator = Simulator(diagram, context)
simulator.Initialize()
simulator.set_publish_every_time_step(False)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(0.005)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
def RunSimulation(self, real_time_rate=1.0):
'''
Here we test using the NNSystem in a Simulator to drive
an acrobot.
'''
sdf_file = "assets/acrobot.sdf"
urdf_file = "assets/acrobot.urdf"
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
parser = Parser(plant=plant, scene_graph=scene_graph)
parser.AddModelFromFile(sdf_file)
plant.Finalize(scene_graph)
# Add
nn_system = NNSystem(self.pytorch_nn_object)
builder.AddSystem(nn_system)
# NN -> plant
builder.Connect(nn_system.NN_out_output_port,
plant.get_actuation_input_port())
# plant -> NN
builder.Connect(plant.get_continuous_state_output_port(),
nn_system.NN_in_input_port)
# Add meshcat visualizer
meshcat = MeshcatVisualizer(scene_graph)
builder.AddSystem(meshcat)
# builder.Connect(scene_graph.GetOutputPort("lcm_visualization"),
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.GetInputPort("lcm_visualization"))
# build diagram
diagram = builder.Build()
meshcat.load()
# time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
# context = diagram.GetMutableSubsystemContext(
# self.station, simulator.get_mutable_context())
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 5.
simulator.StepTo(sim_duration)
print("stepping complete")
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 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 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 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)
# Visualize
visualizer = builder.AddSystem(pbrv)
builder.Connect(rbplant_sys.get_output_port(0),
visualizer.get_input_port(0))
# 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())
def RunSimulation(robobee_plantBS,
control_law,
x0=np.random.random((15, 1)),
duration=30):
robobee_controller = RobobeeController(control_law)
# Create a simple block diagram containing the plant in feedback
# with the controller.
builder = DiagramBuilder()
# The last pendulum plant we made is now owned by a deleted
# system, so easiest path is for us to make a new one.
plant = builder.AddSystem(
RobobeePlantBS(m=robobee_plantBS.m,
Ixx=robobee_plantBS.Ixx,
Iyy=robobee_plantBS.Iyy,
Izz=robobee_plantBS.Izz,
g=robobee_plantBS.g,
input_max=robobee_plantBS.input_max))
Rigidbody_selector = builder.AddSystem(RigidBodySelection())
print("1. Connecting plant and controller\n")
controller = builder.AddSystem(robobee_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
print("2. Connecting plant to the logger\n")
set_time_interval = 0.001
time_interval_multiple = 1000
publish_period = set_time_interval * time_interval_multiple
input_log = builder.AddSystem(SignalLogger(4))
# input_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(controller.get_output_port(0), input_log.get_input_port(0))
state_log = builder.AddSystem(SignalLogger(15))
# state_log._DeclarePeriodicPublish(0.033333, 0.0)
builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))
# Drake visualization
print("3. Connecting plant output to DrakeVisualizer\n")
rtree = RigidBodyTree(
FindResourceOrThrow("drake/examples/robobee/robobee_arena.urdf"),
FloatingBaseType.kQuaternion) #robobee_twobar
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(plant.get_output_port(0),
Rigidbody_selector.get_input_port(0))
builder.Connect(Rigidbody_selector.get_output_port(0),
visualizer.get_input_port(0))
print("4. Building diagram\n")
diagram = builder.Build()
# Set the initial conditions for the simulation.
context = diagram.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
state.SetFromVector(x0)
# Create the simulator.
print("5. Create simulation\n")
simulator = Simulator(diagram, context)
simulator.Initialize()
# simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.get_integrator().set_fixed_step_mode(True)
simulator.get_integrator().set_maximum_step_size(0.05)
# Simulate for the requested duration.
simulator.StepTo(duration)
return input_log, state_log
def perform_iou_testing(model_file, test_specific_temp_directory,
pose_directory):
random_poses = {}
# Read camera translation calculated and applied on gazebo
# we read the random positions file as it contains everything:
with open(
test_specific_temp_directory + "/pics/" + pose_directory +
"/poses.txt", "r") as datafile:
for line in datafile:
if line.startswith("Translation:"):
line_split = line.split(" ")
# we make the value negative since gazebo moved the robot
# and in drakewe move the camera
trans_x = float(line_split[1])
trans_y = float(line_split[2])
trans_z = float(line_split[3])
elif line.startswith("Scaling:"):
line_split = line.split(" ")
scaling = float(line_split[1])
else:
line_split = line.split(" ")
if line_split[1] == "nan":
random_poses[line_split[0][:-1]] = 0
else:
random_poses[line_split[0][:-1]] = float(line_split[1])
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
parser = Parser(plant)
model = make_parser(plant).AddModelFromFile(model_file)
model_bodies = me.get_bodies(plant, {model})
frame_W = plant.world_frame()
frame_B = model_bodies[0].body_frame()
if len(plant.GetBodiesWeldedTo(plant.world_body())) < 2:
plant.WeldFrames(frame_W,
frame_B,
X_PC=plant.GetDefaultFreeBodyPose(frame_B.body()))
# Creating cameras:
renderer_name = "renderer"
scene_graph.AddRenderer(renderer_name,
MakeRenderEngineVtk(RenderEngineVtkParams()))
# N.B. These properties are chosen arbitrarily.
depth_camera = DepthRenderCamera(
RenderCameraCore(
renderer_name,
CameraInfo(
width=960,
height=540,
focal_x=831.382036787,
focal_y=831.382036787,
center_x=480,
center_y=270,
),
ClippingRange(0.01, 10.0),
RigidTransform(),
),
DepthRange(0.01, 10.0),
)
world_id = plant.GetBodyFrameIdOrThrow(plant.world_body().index())
# Creating perspective cam
X_WB = xyz_rpy_deg(
[
1.6 / scaling + trans_x, -1.6 / scaling + trans_y,
1.2 / scaling + trans_z
],
[-120, 0, 45],
)
sensor_perspective = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating top cam
X_WB = xyz_rpy_deg([0 + trans_x, 0 + trans_y, 2.2 / scaling + trans_z],
[-180, 0, -90])
sensor_top = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating front cam
X_WB = xyz_rpy_deg([2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z],
[-90, 0, 90])
sensor_front = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating side cam
X_WB = xyz_rpy_deg([0 + trans_x, 2.2 / scaling + trans_y, 0 + trans_z],
[-90, 0, 180])
sensor_side = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
# Creating back cam
X_WB = xyz_rpy_deg([-2.2 / scaling + trans_x, 0 + trans_y, 0 + trans_z],
[-90, 0, -90])
sensor_back = create_camera(builder, world_id, X_WB, depth_camera,
scene_graph)
DrakeVisualizer.AddToBuilder(builder, scene_graph)
# Remove gravity to avoid extra movements of the model when running the simulation
plant.gravity_field().set_gravity_vector(
np.array([0, 0, 0], dtype=np.float64))
# Switch off collisions to avoid problems with random positions
collision_filter_manager = scene_graph.collision_filter_manager()
model_inspector = scene_graph.model_inspector()
geometry_ids = GeometrySet(model_inspector.GetAllGeometryIds())
collision_filter_manager.Apply(
CollisionFilterDeclaration().ExcludeWithin(geometry_ids))
plant.Finalize()
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
dofs = plant.num_actuated_dofs()
if dofs != plant.num_positions():
raise ValueError(
"Error on converted model: Num positions is not equal to num actuated dofs."
)
if pose_directory == "random_pose":
joint_positions = [0] * dofs
for joint_name, pose in random_poses.items():
# check if NaN
if pose != pose:
pose = 0
# drake will add '_joint' when there's a name collision
if plant.HasJointNamed(joint_name):
joint = plant.GetJointByName(joint_name)
else:
joint = plant.GetJointByName(joint_name + "_joint")
joint_positions[joint.position_start()] = pose
sim_plant_context = plant.GetMyContextFromRoot(
simulator.get_mutable_context())
plant.get_actuation_input_port(model).FixValue(sim_plant_context,
np.zeros((dofs, 1)))
plant.SetPositions(sim_plant_context, model, joint_positions)
simulator.AdvanceTo(1)
generate_images_and_iou(simulator, sensor_perspective,
test_specific_temp_directory, pose_directory, 1)
generate_images_and_iou(simulator, sensor_top,
test_specific_temp_directory, pose_directory, 2)
generate_images_and_iou(simulator, sensor_front,
test_specific_temp_directory, pose_directory, 3)
generate_images_and_iou(simulator, sensor_side,
test_specific_temp_directory, pose_directory, 4)
generate_images_and_iou(simulator, sensor_back,
test_specific_temp_directory, pose_directory, 5)
