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 playbackMotion(data1, data2, data3, data4, times):
data = np.concatenate((data2, data1, data4, data3), axis=0)
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(Player(data, times))
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]))
#print(robot.get_output_port(0).size())
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)
# Simulate for 10 seconds
simulator.StepTo(times[-1] + 0.5)
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 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 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 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 plant_system(self):
'''
Implements the plant_system method in DrakeEnv by constructing a RigidBodyPlant
'''
# Add Systems
builder = DiagramBuilder()
self.scene_graph = builder.AddSystem(SceneGraph())
self.mbp = builder.AddSystem(MultibodyPlant())
# Load the model from the file
AddModelFromSdfFile(file_name=self.model_path,
plant=self.mbp,
scene_graph=self.scene_graph)
self.mbp.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
self.mbp.Finalize(self.scene_graph)
assert self.mbp.geometry_source_is_registered()
# Visualizer must be initialized after Finalize() and before CreateDefaultContext()
self.init_visualizer()
builder.Connect(
self.mbp.get_geometry_poses_output_port(),
self.scene_graph.get_source_pose_port(self.mbp.get_source_id()))
# Expose the inputs and outputs and build the diagram
self._input_port_index_action = builder.ExportInput(
self.mbp.get_actuation_input_port())
self._output_port_index_state = builder.ExportOutput(
self.mbp.get_continuous_state_output_port())
self.diagram = builder.Build()
self._output = self.mbp.AllocateOutput()
return self.diagram
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 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 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 cartPoleTest(args):
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(
file_name=file_name, plant=cart_pole, scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
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():
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 kukaTest(args):
file_name = FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision"
".sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
kuka = builder.AddSystem(MultibodyPlant())
AddModelFromSdfFile(
file_name=file_name, plant=kuka, scene_graph=scene_graph)
kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
kuka.Finalize(scene_graph)
assert kuka.geometry_source_is_registered()
builder.Connect(
kuka.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(kuka.get_source_id()))
visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
visualizer.load()
diagram_context = diagram.CreateDefaultContext()
kuka_context = diagram.GetMutableSubsystemContext(
kuka, diagram_context)
kuka_context.FixInputPort(
kuka.get_actuation_input_port().get_index(), np.zeros(
kuka.get_actuation_input_port().size()))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.duration)
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 build_block_diagram(actuators_off=False,
desired_lateral_velocity=0.0,
desired_height=3.0,
print_period=0.0):
builder = DiagramBuilder()
# Build the plant
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 robot
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
plant.set_name('plant')
# 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_state_output_port(), state_log.get_input_port(0))
state_log.set_name('state_log')
# The controller
controller = builder.AddSystem(
Hopper2dController(plant,
desired_lateral_velocity=desired_lateral_velocity,
desired_height=desired_height,
actuators_off=actuators_off,
print_period=print_period))
builder.Connect(plant.get_state_output_port(),
controller.get_input_port(0))
builder.Connect(controller.get_output_port(0),
plant.get_actuation_input_port())
controller.set_name('controller')
# Create visualizer
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph,
xlim=[-1, 10],
ylim=[-.2, 4.5],
show=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
visualizer.set_name('visualizer')
diagram = builder.Build()
return diagram
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 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 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
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)
else:
print("Unrecognized model %s." % model)
parser.print_usage()
exit(1)
rbplant = RigidBodyPlant(rbt, timestep)
nx = rbt.get_num_positions() + rbt.get_num_velocities()
builder = DiagramBuilder()
rbplant_sys = builder.AddSystem(rbplant)
torque = args.torque
torque_system = builder.AddSystem(
ConstantVectorSource(
np.ones((rbt.get_num_actuators(), 1)) * torque))
builder.Connect(torque_system.get_output_port(0),
rbplant_sys.get_input_port(0))
print('Simulating with constant torque = ' + str(torque) +
' Newton-meters')
# 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))
def MakeManipulationStation(time_step=0.002,
plant_setup_callback=None,
camera_prefix="camera"):
"""
Sets up a manipulation station with the iiwa + wsg + controllers [+
cameras]. Cameras will be added to each body with a name starting with
"camera".
Args:
time_step: the standard MultibodyPlant time step.
plant_setup_callback: should be a python callable that takes one
argument: `plant_setup_callback(plant)`. It will be called after the
iiwa and WSG are added to the plant, but before the plant is
finalized. Use this to add additional geometry.
camera_prefix: Any bodies in the plant (created during the
plant_setup_callback) starting with this prefix will get a camera
attached.
"""
builder = DiagramBuilder()
# Add (only) the iiwa, WSG, and cameras to the scene.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=time_step)
iiwa = AddIiwa(plant)
wsg = AddWsg(plant, iiwa)
if plant_setup_callback:
plant_setup_callback(plant)
plant.Finalize()
num_iiwa_positions = plant.num_positions(iiwa)
# I need a PassThrough system so that I can export the input port.
iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions))
builder.ExportInput(iiwa_position.get_input_port(), "iiwa_position")
builder.ExportOutput(iiwa_position.get_output_port(),
"iiwa_position_command")
# Export the iiwa "state" outputs.
demux = builder.AddSystem(
Demultiplexer(2 * num_iiwa_positions, num_iiwa_positions))
builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port())
builder.ExportOutput(demux.get_output_port(0), "iiwa_position_measured")
builder.ExportOutput(demux.get_output_port(1), "iiwa_velocity_estimated")
builder.ExportOutput(plant.get_state_output_port(iiwa),
"iiwa_state_estimated")
# Make the plant for the iiwa controller to use.
controller_plant = MultibodyPlant(time_step=time_step)
controller_iiwa = AddIiwa(controller_plant)
AddWsg(controller_plant, controller_iiwa, welded=True)
controller_plant.Finalize()
# Add the iiwa controller
iiwa_controller = builder.AddSystem(
InverseDynamicsController(controller_plant,
kp=[100] * num_iiwa_positions,
ki=[1] * num_iiwa_positions,
kd=[20] * num_iiwa_positions,
has_reference_acceleration=False))
iiwa_controller.set_name("iiwa_controller")
builder.Connect(plant.get_state_output_port(iiwa),
iiwa_controller.get_input_port_estimated_state())
# Add in the feed-forward torque
adder = builder.AddSystem(Adder(2, num_iiwa_positions))
builder.Connect(iiwa_controller.get_output_port_control(),
adder.get_input_port(0))
# Use a PassThrough to make the port optional (it will provide zero values
# if not connected).
torque_passthrough = builder.AddSystem(PassThrough([0]
* num_iiwa_positions))
builder.Connect(torque_passthrough.get_output_port(),
adder.get_input_port(1))
builder.ExportInput(torque_passthrough.get_input_port(),
"iiwa_feedforward_torque")
builder.Connect(adder.get_output_port(),
plant.get_actuation_input_port(iiwa))
# Add discrete derivative to command velocities.
desired_state_from_position = builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
num_iiwa_positions, time_step, suppress_initial_transient=True))
desired_state_from_position.set_name("desired_state_from_position")
builder.Connect(desired_state_from_position.get_output_port(),
iiwa_controller.get_input_port_desired_state())
builder.Connect(iiwa_position.get_output_port(),
desired_state_from_position.get_input_port())
# Export commanded torques.
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_commanded")
builder.ExportOutput(adder.get_output_port(), "iiwa_torque_measured")
builder.ExportOutput(plant.get_generalized_contact_forces_output_port(iiwa),
"iiwa_torque_external")
# Wsg controller.
wsg_controller = builder.AddSystem(SchunkWsgPositionController())
wsg_controller.set_name("wsg_controller")
builder.Connect(wsg_controller.get_generalized_force_output_port(),
plant.get_actuation_input_port(wsg))
builder.Connect(plant.get_state_output_port(wsg),
wsg_controller.get_state_input_port())
builder.ExportInput(wsg_controller.get_desired_position_input_port(),
"wsg_position")
builder.ExportInput(wsg_controller.get_force_limit_input_port(),
"wsg_force_limit")
wsg_mbp_state_to_wsg_state = builder.AddSystem(
MakeMultibodyStateToWsgStateSystem())
builder.Connect(plant.get_state_output_port(wsg),
wsg_mbp_state_to_wsg_state.get_input_port())
builder.ExportOutput(wsg_mbp_state_to_wsg_state.get_output_port(),
"wsg_state_measured")
builder.ExportOutput(wsg_controller.get_grip_force_output_port(),
"wsg_force_measured")
# Cameras.
AddRgbdSensors(builder,
plant,
scene_graph,
model_instance_prefix=camera_prefix)
# Export "cheat" ports.
builder.ExportOutput(scene_graph.get_query_output_port(), "geometry_query")
builder.ExportOutput(plant.get_contact_results_output_port(),
"contact_results")
builder.ExportOutput(plant.get_state_output_port(),
"plant_continuous_state")
builder.ExportOutput(plant.get_body_poses_output_port(), "body_poses")
diagram = builder.Build()
diagram.set_name("ManipulationStation")
return diagram
import matplotlib.pyplot as plt
from pydrake.all import (DiagramBuilder, SignalLogger, Simulator)
from simple_continuous_time_system import *
# Create a simple block diagram containing our system.
builder = DiagramBuilder()
system = builder.AddSystem(SimpleContinuousTimeSystem())
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_continuous_state_vector()
state.SetFromVector([0.9])
# Simulate for 10 seconds.
simulator.StepTo(10)
# Plot the results.
plt.plot(logger.sample_times(), logger.data().transpose())
plt.xlabel('t')
plt.ylabel('x(t)')
plt.show()
from underactuated import FindResource, PlanarSceneGraphVisualizer
# 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)
