def test_thigh_torque_return_type(self):
"""Verify the signature of ChooseThighTorque"""
from hopper_2d import Hopper2dController
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(0.0005))
parser = Parser(plant)
parser.AddModelFromFile("raibert_hopper_2d.sdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ground"))
plant.Finalize()
controller = Hopper2dController(plant, desired_lateral_velocity=0.0)
x0 = np.zeros(10)
x0[1] = 4. # in air
x0[4] = 0.5 # feasible leg length
x0[5] = 0.1 # initial speed
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
# Try from another desired velocity
controller.desired_lateral_velocity = -1.0
torquedes = controller.ChooseThighTorque(x0)
self.assertIsInstance(torquedes, float,
"ChooseThighTorque returned a type other than "\
"float for X0 = %s, desired_lateral_velocity = %f" %
(np.array_str(x0), controller.desired_lateral_velocity))
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 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 get_basic_manip_station():
"""Used for IK"""
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant = station.get_multibody_plant()
return plant, context
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 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 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 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 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 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 compile_scene_tree_clearance_geometry_to_mbp_and_sg(
scene_tree, timestep=0.001, alpha=0.25):
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=timestep))
parser = Parser(mbp)
parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH")
world_body = mbp.world_body()
free_body_poses = []
# For generating colors.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
for node in scene_tree.nodes:
if node.tf is not None and node.physics_geometry_info is not None:
# Don't have to do anything if this does not introduce geometry.
sanity_check_node_tf_and_physics_geom_info(node)
phys_geom_info = node.physics_geometry_info
has_clearance_geometry = len(phys_geom_info.clearance_geometry) > 0
if not has_clearance_geometry:
continue
# Add a body for this node and register the clearance geometry.
# TODO(gizatt) This tree body index is built in to disambiguate names.
# But I forsee a name-to-stuff resolution crisis when inference time comes...
# this might get resolved by the solution to that.
body = mbp.AddRigidBody(name=node.name,
M_BBo_B=phys_geom_info.spatial_inertia)
tf = torch_tf_to_drake_tf(node.tf)
mbp.SetDefaultFreeBodyPose(body, tf)
# Pick out a color for this class.
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = list(cmap(cmap_counter))
color[3] = alpha
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)
# Handle adding primitive geometry by adding it all to one
# mbp.
if len(phys_geom_info.clearance_geometry) > 0:
for k, (tf, geometry) in enumerate(
phys_geom_info.clearance_geometry):
mbp.RegisterCollisionGeometry(
body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name + "_col_%03d" % k,
coulomb_friction=default_friction)
mbp.RegisterVisualGeometry(body=body,
X_BG=torch_tf_to_drake_tf(tf),
shape=geometry,
name=node.name +
"_vis_%03d" % k,
diffuse_color=color)
return builder, mbp, scene_graph
def main():
state_rec = StateReceiver()
assert type(state_rec) is StateReceiver
assert state_rec.get_utime_output_port().size() == 1
assert state_rec.get_position_output_port().size() == kCassiePositions
assert state_rec.get_velocity_output_port().size() == kCassieVelocities
assert state_rec.get_contact_output_port().size() == 2
command_pub = CommandSender()
assert type(command_pub) is CommandSender
assert command_pub.get_utime_input_port().size() == 1
assert command_pub.get_torque_input_port().size() == kCassieActuators
assert command_pub.get_motor_pos_input_port().size() == kCassieActuators
assert command_pub.get_motor_vel_input_port().size() == kCassieActuators
assert command_pub.get_kp_input_port().size() == kCassieActuators
assert command_pub.get_kd_input_port().size() == kCassieActuators
assert command_pub.get_ki_input_port().size() == kCassieActuators
assert command_pub.get_leak_input_port().size() == kCassieActuators
assert command_pub.get_clamp_input_port().size() == kCassieActuators
lcm = DrakeLcm()
builder = DiagramBuilder()
state_rec = addLcmStateReceiver(builder, lcm)
assert type(state_rec) is StateReceiver
command_pub = addLcmCommandSender(builder, lcm, 0.0005)
assert type(command_pub) is CommandSender
state, command = addLcmControlComms(builder, lcm, 0.0005)
assert type(state) is StateReceiver
assert type(command) is CommandSender
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 build_block_diagram(boat, controller):
builder = DiagramBuilder()
boat = builder.AddSystem(boat)
boat.set_name('boat')
# logger to track trajectories
logger = LogOutput(boat.get_output_port(0), builder)
logger.set_name('logger')
# expose boats input as an input port
builder.ExportInput(boat.get_input_port(0))
# build diagram
diagram = builder.Build()
diagram.set_name('diagram')
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 gripper_forward_kinematics_example():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
frames_to_draw = {
"iiwa": {
"iiwa_link_1", "iiwa_link_2", "iiwa_link_3", "iiwa_link_4",
"iiwa_link_5", "iiwa_link_6", "iiwa_link_7"
},
"gripper": {"body"}
}
meshcat = ConnectMeshcatVisualizer(
builder,
station.get_scene_graph(),
output_port=station.GetOutputPort("pose_bundle"),
frames_to_draw=frames_to_draw,
axis_length=0.3,
axis_radius=0.01)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# xyz = Text(value="", description="gripper position (m): ", layout=Layout(width='500px'), style={'description_width':'initial'})
# rpy = Text(value="", description="gripper roll-pitch-yaw (rad): ", layout=Layout(width='500px'), style={'description_width':'initial'})
plant = station.get_multibody_plant()
gripper = plant.GetBodyByName("body")
def pose_callback(context):
pose = plant.EvalBodyPoseInWorld(context, gripper) # Important
# xyz.value = np.array2string(pose.translation(), formatter={'float': lambda x: "{:3.2f}".format(x)})
# rpy.value = np.array2string(RollPitchYaw(pose.rotation()).vector(), formatter={'float': lambda x: "{:3.2f}".format(x)})
meshcat.load()
MakeJointSlidersThatPublishOnCallback(station.get_multibody_plant(),
meshcat,
context,
my_callback=pose_callback)
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 grasp_poses_example():
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
parser = Parser(plant, scene_graph)
grasp = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf"
), "grasp")
brick = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
meshcat = ConnectMeshcatVisualizer(builder, scene_graph)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
B_O = plant.GetBodyByName("base_link", brick)
X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O)
B_Ggrasp = plant.GetBodyByName("body", grasp)
p_GgraspO = [0, 0.12, 0]
R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply(
RotationMatrix.MakeZRotation(np.pi / 2.0))
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
X_WGgrasp = X_WO.multiply(X_OGgrasp)
plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp)
# Open the fingers, too
plant.GetJointByName("left_finger_sliding_joint",
grasp).set_translation(plant_context, -0.054)
plant.GetJointByName("right_finger_sliding_joint",
grasp).set_translation(plant_context, 0.054)
meshcat.load()
diagram.Publish(context)
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(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_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 PendulumExample():
builder = DiagramBuilder()
plant = builder.AddSystem(PendulumPlant())
scene_graph = builder.AddSystem(SceneGraph())
PendulumGeometry.AddToBuilder(builder, plant.get_state_output_port(),
scene_graph)
MeshcatVisualizerCpp.AddToBuilder(
builder, scene_graph, meshcat,
MeshcatVisualizerParams(publish_period=np.inf))
builder.ExportInput(plant.get_input_port(), "torque")
# Note: The Pendulum-v0 in gym outputs [cos(theta), sin(theta), thetadot]
builder.ExportOutput(plant.get_state_output_port(), "state")
# Add a camera (I have sugar for this in the manip repo)
X_WC = RigidTransform(RotationMatrix.MakeXRotation(-np.pi / 2),
[0, -1.5, 0])
rgbd = AddRgbdSensor(builder, scene_graph, X_WC)
builder.ExportOutput(rgbd.color_image_output_port(), "camera")
diagram = builder.Build()
simulator = Simulator(diagram)
def reward(system, context):
plant_context = plant.GetMyContextFromRoot(context)
state = plant_context.get_continuous_state_vector()
u = plant.get_input_port().Eval(plant_context)[0]
theta = state[0] % 2 * np.pi # Wrap to 2*pi
theta_dot = state[1]
return (theta - np.pi)**2 + 0.1 * theta_dot**2 + 0.001 * u
max_torque = 3
env = DrakeGymEnv(simulator,
time_step=0.05,
action_space=gym.spaces.Box(low=np.array([-max_torque]),
high=np.array([max_torque])),
observation_space=gym.spaces.Box(
low=np.array([-np.inf, -np.inf]),
high=np.array([np.inf, np.inf])),
reward=reward,
render_rgb_port_id="camera")
check_env(env)
if show:
env.reset()
image = env.render(mode='rgb_array')
fig, ax = plt.subplots()
ax.imshow(image)
plt.show()
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 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 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 __init__(self, file=None, terrain=FlatTerrain(), dlevel=1):
"""
Initialize TimeSteppingMultibodyPlant with a model from a file and an arbitrary terrain geometry. Initialization also welds the first frame in the MultibodyPlant to the world frame
"""
self.builder = DiagramBuilder()
self.multibody, self.scene_graph = AddMultibodyPlantSceneGraph(
self.builder, 0.001)
# Store the terrain
self.terrain = terrain
self._dlevel = dlevel
# Build the MultibodyPlant from the file, if one exists
self.model_index = []
if file is not None:
# Parse the file
self.model_index = Parser(self.multibody).AddModelFromFile(
FindResource(file))
# Initialize the collision data
self.collision_frames = []
self.collision_poses = []
self.collision_radius = []
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 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 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)
