def test_visualizer(self):
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
state = GliderState(np.zeros((7,)))
state.x = -.3
state.z = 0.1
state.pitch = 0.2
state.elevator = .4
source = builder.AddSystem(ConstantVectorSource(state[:]))
geom = GliderGeometry.AddToBuilder(builder, source.get_output_port(0),
scene_graph)
# plt_vis = ConnectPlanarSceneGraphVisualizer(builder, scene_graph)
drake_viz = ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
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)
# but I just want to hack the Val planar viz into working...
set_initial_state = False
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),
setDefaultContactParams(cassie)
# precomputed standing fixed point state
# q = getNominalStandingConfiguration()
# qd = [0. for i in xrange(rtree.get_num_velocities())]
x = CassieFixedPointState()
# Setup visualizer
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(cassie.get_output_port(0), visualizer.get_input_port(0))
# Zero inputs -- passive forward simulation
u0 = ConstantVectorSource(np.zeros(rtree.get_num_actuators()))
null_controller = builder.AddSystem(u0)
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
# kinsol = rtree.doKinematics(q)
# com = rtree.centerOfMass(kinsol)
# print com
# nominal standing state
# state = simulator.get_mutable_context().get_mutable_continuous_state_vector()
# in current configuration. It's probably something simple,
# but I just want to hack the Val planar viz into working...
set_initial_state = False
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),
def make_box_flipup(generator,
observations="state",
meshcat=None,
time_limit=10):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
# TODO(russt): randomize parameters.
box = AddPlanarBinAndSimpleBox(plant)
finger = AddPointFinger(plant)
plant.Finalize()
plant.set_name("plant")
SetTransparency(scene_graph, alpha=0.5, source_id=plant.get_source_id())
controller_plant = MultibodyPlant(time_step=0.005)
AddPointFinger(controller_plant)
if meshcat:
MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
meshcat.Set2dRenderMode(xmin=-.35, xmax=.35, ymin=-0.1, ymax=0.3)
ContactVisualizer.AddToBuilder(
builder, plant, meshcat,
ContactVisualizerParams(radius=0.005, newtons_per_meter=40.0))
# Use the controller plant to visualize the set point geometry.
controller_scene_graph = builder.AddSystem(SceneGraph())
controller_plant.RegisterAsSourceForSceneGraph(controller_scene_graph)
SetColor(controller_scene_graph,
color=[1.0, 165.0 / 255, 0.0, 1.0],
source_id=controller_plant.get_source_id())
controller_vis = MeshcatVisualizerCpp.AddToBuilder(
builder, controller_scene_graph, meshcat,
MeshcatVisualizerParams(prefix="controller"))
controller_vis.set_name("controller meshcat")
controller_plant.Finalize()
# Stiffness control. (For a point finger with unit mass, the
# InverseDynamicsController is identical)
N = controller_plant.num_positions()
kp = [100] * N
ki = [1] * N
kd = [2 * np.sqrt(kp[0])] * N
controller = builder.AddSystem(
InverseDynamicsController(controller_plant, kp, ki, kd, False))
builder.Connect(plant.get_state_output_port(finger),
controller.get_input_port_estimated_state())
actions = builder.AddSystem(PassThrough(N))
positions_to_state = builder.AddSystem(Multiplexer([N, N]))
builder.Connect(actions.get_output_port(),
positions_to_state.get_input_port(0))
zeros = builder.AddSystem(ConstantVectorSource([0] * N))
builder.Connect(zeros.get_output_port(),
positions_to_state.get_input_port(1))
builder.Connect(positions_to_state.get_output_port(),
controller.get_input_port_desired_state())
builder.Connect(controller.get_output_port(),
plant.get_actuation_input_port())
if meshcat:
positions_to_poses = builder.AddSystem(
MultibodyPositionToGeometryPose(controller_plant))
builder.Connect(
positions_to_poses.get_output_port(),
controller_scene_graph.get_source_pose_port(
controller_plant.get_source_id()))
builder.ExportInput(actions.get_input_port(), "actions")
if observations == "state":
builder.ExportOutput(plant.get_state_output_port(), "observations")
# TODO(russt): Add 'time', and 'keypoints'
else:
raise ValueError("observations must be one of ['state']")
class RewardSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("box_state", 6)
self.DeclareVectorInputPort("finger_state", 4)
self.DeclareVectorInputPort("actions", 2)
self.DeclareVectorOutputPort("reward", 1, self.CalcReward)
def CalcReward(self, context, output):
box_state = self.get_input_port(0).Eval(context)
finger_state = self.get_input_port(1).Eval(context)
actions = self.get_input_port(2).Eval(context)
angle_from_vertical = (box_state[2] % np.pi) - np.pi / 2
cost = 2 * angle_from_vertical**2 # box angle
cost += 0.1 * box_state[5]**2 # box velocity
effort = actions - finger_state[:2]
cost += 0.1 * effort.dot(effort) # effort
# finger velocity
cost += 0.1 * finger_state[2:].dot(finger_state[2:])
# Add 10 to make rewards positive (to avoid rewarding simulator
# crashes).
output[0] = 10 - cost
reward = builder.AddSystem(RewardSystem())
builder.Connect(plant.get_state_output_port(box), reward.get_input_port(0))
builder.Connect(plant.get_state_output_port(finger),
reward.get_input_port(1))
builder.Connect(actions.get_output_port(), reward.get_input_port(2))
builder.ExportOutput(reward.get_output_port(), "reward")
# Set random state distributions.
uniform_random = Variable(name="uniform_random",
type=Variable.Type.RANDOM_UNIFORM)
box_joint = plant.GetJointByName("box")
x, y = box_joint.get_default_translation()
box_joint.set_random_pose_distribution([.2 * uniform_random - .1 + x, y], 0)
diagram = builder.Build()
simulator = Simulator(diagram)
# Termination conditions:
def monitor(context):
if context.get_time() > time_limit:
return EventStatus.ReachedTermination(diagram, "time limit")
return EventStatus.Succeeded()
simulator.set_monitor(monitor)
return simulator
import matplotlib.pyplot as plt
# from IPython import get_ipython
# from underactuated.jupyter import AdvanceToAndVisualize, SetupMatplotlibBackend
# plt_is_interactive = SetupMatplotlibBackend()
from pydrake.all import (ConstantVectorSource, DiagramBuilder,
PlanarSceneGraphVisualizer, SceneGraph, SignalLogger,
Simulator)
from pydrake.examples.compass_gait import (CompassGait, CompassGaitGeometry,
CompassGaitParams)
builder = DiagramBuilder()
compass_gait = builder.AddSystem(CompassGait())
hip_torque = builder.AddSystem(ConstantVectorSource([0.0]))
builder.Connect(hip_torque.get_output_port(0), compass_gait.get_input_port(0))
scene_graph = builder.AddSystem(SceneGraph())
CompassGaitGeometry.AddToBuilder(
builder, compass_gait.get_floating_base_state_output_port(), scene_graph)
# visualizer = builder.AddSystem(
# PlanarSceneGraphVisualizer(scene_graph, xlim=[-1., 5.], ylim=[-1., 2.],
# show=plt_is_interactive))
# builder.Connect(scene_graph.get_pose_bundle_output_port(),
# visualizer.get_input_port(0))
logger = builder.AddSystem(SignalLogger(14))
builder.Connect(compass_gait.get_output_port(1), logger.get_input_port(0))
diagram = builder.Build()
