import numpy as np import matplotlib.pyplot as plt from pydrake.systems.analysis import Simulator from pydrake.systems.framework import DiagramBuilder from pydrake.systems.primitives import ConstantVectorSource, LogOutput from plant import SLIPState, SpringLoadedInvertedPendulum from visualizer import SLIPVisualizer builder = DiagramBuilder() plant = builder.AddSystem(SpringLoadedInvertedPendulum()) # Parameters from Geyer05, Figure 2.4 # Note: Geyer uses angle of attack = 90 - touchdown_angle touchdown_angle = np.deg2rad(30) Etilde = 1.61 s = SLIPState(np.zeros(8)) s.z = 0.9 s.theta = touchdown_angle s.r = 1 s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z) viz = builder.AddSystem(SLIPVisualizer()) builder.Connect(plant.get_output_port(0), viz.get_input_port(0)) log = LogOutput(plant.get_output_port(0), builder) command = builder.AddSystem(ConstantVectorSource([touchdown_angle]))
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
# TODO(russt): Replace this hard-coded camera serial number with a config
# file.
camera_ids = ["805212060544"]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
station.SetupDefaultStation()
parser = Parser(station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
object = parser.AddModelFromFile(
FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf"),
"object")
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
def test_drake_visualizer(self, T):
# Test visualization API.
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeLcm()
role = mut.Role.kIllustration
params = mut.DrakeVisualizerParams(
publish_period=0.1, role=mut.Role.kIllustration,
default_color=mut.Rgba(0.1, 0.2, 0.3, 0.4))
# Add some subscribers to detect message broadcast.
load_channel = "DRAKE_VIEWER_LOAD_ROBOT"
draw_channel = "DRAKE_VIEWER_DRAW"
load_subscriber = Subscriber(
lcm, load_channel, lcmt_viewer_load_robot)
draw_subscriber = Subscriber(
lcm, draw_channel, lcmt_viewer_draw)
# There are three ways to configure DrakeVisualizer.
def by_hand(builder, scene_graph, params):
visualizer = builder.AddSystem(
mut.DrakeVisualizer_[T](lcm=lcm, params=params))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.query_object_input_port())
def auto_connect_to_system(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(builder=builder,
scene_graph=scene_graph,
lcm=lcm, params=params)
def auto_connect_to_port(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(
builder=builder,
query_object_port=scene_graph.get_query_output_port(),
lcm=lcm, params=params)
for func in [by_hand, auto_connect_to_system, auto_connect_to_port]:
# Build the diagram.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
func(builder, scene_graph, params)
# Simulate to t = 0 to send initial load and draw messages.
diagram = builder.Build()
Simulator(diagram).AdvanceTo(0)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 1)
load_subscriber.clear()
draw_subscriber.clear()
# Ad hoc broadcasting.
scene_graph = SceneGraph()
mut.DrakeVisualizer_[T].DispatchLoadMessage(
scene_graph, lcm, params)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 0)
load_subscriber.clear()
draw_subscriber.clear()
from pydrake.systems.framework import DiagramBuilder
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--test",
action='store_true',
help="Causes the script to run without blocking for "
"user input.",
default=False)
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant())
cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
Parser(plant=cart_pole).AddModelFromFile(file_name)
cart_pole.AddForceElement(UniformGravityFieldElement())
cart_pole.Finalize()
assert cart_pole.geometry_source_is_registered()
builder.Connect(
scene_graph.get_query_output_port(),
cart_pole.get_geometry_query_input_port())
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
builder.ExportInput(cart_pole.get_actuation_input_port())
X_MS[1, 0] = np.sin(theta)
return X_MS
if __name__ == "__main__":
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--config_file",
required=True,
help="The path to a .yml camera config file")
args = parser.parse_args()
builder = DiagramBuilder()
# create the PointCloudToPoseSystem
pc_to_pose = builder.AddSystem(PointCloudToPoseSystem(
args.config_file, SegmentFoamBrick, GetFoamBrickPose))
# realsense serial numbers are >> 100
use_hardware = int(pc_to_pose.camera_configs["left_camera_serial"]) > 100
if use_hardware:
camera_ids = [
pc_to_pose.camera_configs["left_camera_serial"],
pc_to_pose.camera_configs["middle_camera_serial"],
pc_to_pose.camera_configs["right_camera_serial"]]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect()
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--num_samples",
type=int,
default=50,
help="Number of Monte Carlo samples to use to estimate performance.")
parser.add_argument("--torque_limit",
type=float,
default=2.0,
help="Torque limit of the pendulum.")
args = parser.parse_args()
if args.torque_limit < 0:
raise InvalidArgumentError("Please supply a nonnegative torque limit.")
# Assemble the Pendulum plant.
builder = DiagramBuilder()
pendulum = builder.AddSystem(MultibodyPlant())
file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")
Parser(pendulum).AddModelFromFile(file_name)
pendulum.AddForceElement(UniformGravityFieldElement())
pendulum.WeldFrames(pendulum.world_frame(),
pendulum.GetFrameByName("base_part2"))
pendulum.Finalize()
# Set the pendulum to start at uniformly random
# positions (but always zero velocity).
elbow = pendulum.GetMutableJointByName("theta")
upright_theta = np.pi
theta_expression = Variable(name="theta",
type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi
elbow.set_random_angle_distribution(theta_expression)
# Set up LQR, with high position gains to try to ensure the
# ROA is close to the theoretical torque-limited limit.
Q = np.diag([100., 1.])
R = np.identity(1) * 0.01
linearize_context = pendulum.CreateDefaultContext()
linearize_context.SetContinuousState(np.array([upright_theta, 0.]))
actuation_port_index = pendulum.get_actuation_input_port().get_index()
linearize_context.FixInputPort(actuation_port_index, np.zeros(1))
controller = builder.AddSystem(
LinearQuadraticRegulator(pendulum, linearize_context, Q, R,
np.zeros(0), actuation_port_index))
# Apply the torque limit.
torque_limit = args.torque_limit
torque_limiter = builder.AddSystem(
Saturation(min_value=np.array([-torque_limit]),
max_value=np.array([torque_limit])))
builder.Connect(controller.get_output_port(0),
torque_limiter.get_input_port(0))
builder.Connect(torque_limiter.get_output_port(0),
pendulum.get_actuation_input_port())
builder.Connect(pendulum.get_output_port(0), controller.get_input_port(0))
diagram = builder.Build()
# Perform the Monte Carlo simulation.
def make_simulator(generator):
''' Create a simulator for the system
using the given generator. '''
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(0)
simulator.Initialize()
return simulator
def calc_wrapped_error(system, context):
''' Given a context from the end of the simulation,
calculate an error -- which for this stabilizing
task is the distance from the
fixed point. '''
state = diagram.GetSubsystemContext(
pendulum, context).get_continuous_state_vector()
error = state.GetAtIndex(0) - upright_theta
# Wrap error to [-pi, pi].
return (error + np.pi) % (2 * np.pi) - np.pi
num_samples = args.num_samples
results = MonteCarloSimulation(make_simulator=make_simulator,
output=calc_wrapped_error,
final_time=1.0,
num_samples=num_samples,
generator=RandomGenerator())
# Compute results.
# The "success" region is fairly large since some "stabilized" trials
# may still be oscillating around the fixed point. Failed examples are
# consistently much farther from the fixed point than this.
binary_results = np.array([abs(res.output) < 0.1 for res in results])
passing_ratio = float(sum(binary_results)) / len(results)
# 95% confidence interval for the passing ratio.
passing_ratio_var = 1.96 * np.sqrt(passing_ratio *
(1. - passing_ratio) / len(results))
print("Monte-Carlo estimated performance across %d samples: "
"%.2f%% +/- %0.2f%%" %
(num_samples, passing_ratio * 100, passing_ratio_var * 100))
# Analytically compute the best possible ROA, for comparison, but
# calculating where the torque needed to lift the pendulum exceeds
# the torque limit.
arm_radius = 0.5
arm_mass = 1.0
# torque = r x f = r * (m * 9.81 * sin(theta))
# theta = asin(torque / (r * m))
if torque_limit <= (arm_radius * arm_mass * 9.81):
roa_half_width = np.arcsin(torque_limit /
(arm_radius * arm_mass * 9.81))
else:
roa_half_width = np.pi
roa_as_fraction_of_state_space = roa_half_width / np.pi
print("Max possible ROA = %0.2f%% of state space, which should"
" match with the above estimate." %
(100 * roa_as_fraction_of_state_space))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const",
type=float,
default=0.005,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
grab_focus = False
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
else:
grab_focus = True
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation()
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
if args.meshcat:
meshcat = builder.AddSystem(
MeshcatVisualizer(station.get_scene_graph(),
zmq_url=args.meshcat))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
# zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
# zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# 1.SceneGraph system
scene_graph = builder.AddSystem(SceneGraph())
scene_graph.RegisterSource('scene_graph_n')
plant = builder.AddSystem(MultibodyPlant())
plant_parser = Parser(plant, scene_graph)
plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body"))
# Add gravity to the model.
plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
# Model is completed
plant.Finalize()
builder.Connect(scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port())
builder.Connect(plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.get_continuous_state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_actuation_input_port())
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
# PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
# PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.get_continuous_state_output_port(),
logger.get_input_port(0))
return builder.Build(), logger, plant
def LittleDogSimulationDiagram(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
zero_config = np.zeros(
(rb_tree.get_num_actuators() * 2, 1)) # just for test
#zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0)
#zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)
zero_config = np.concatenate(
(np.array([0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5
]), np.zeros(12)),
axis=0)
# zero_config[0] = 1.7
# zero_config[1] = 1.7
# zero_config[2] = 1.7
# zero_config[3] = 1.7
# RigidBodyPlant
plant = builder.AddSystem(RigidBodyPlant(rb_tree, dt))
plant.set_name('plant')
# Visualizer
visualizer_publisher = builder.AddSystem(drake_visualizer)
visualizer_publisher.set_name('visualizer_publisher')
visualizer_publisher.set_publish_period(0.02)
builder.Connect(plant.state_output_port(),
visualizer_publisher.get_input_port(0))
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
builder.Connect(plant.state_output_port(),
robot_state_encoder.joint_state_results_input_port())
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantConverter(
rb_tree.actuators,
motor_gain=None)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_input_port(0))
# PID controller
kp, ki, kd = joints_PID_params(rb_tree)
#PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd))
#PIDcontroller.set_name('PID_controller')
rb = RigidBodyTree()
robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0],
[0, 0, 0])
# insert a robot from urdf files
pmap = PackageMap()
pmap.PopulateFromFolder(os.path.dirname(model_path))
AddModelInstanceFromUrdfStringSearchingInRosPackages(
open(model_path, 'r').read(),
pmap,
os.path.dirname(model_path),
FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw,
robot_frame,
rb)
PIDcontroller = builder.AddSystem(
RbtInverseDynamicsController(rb, kp, ki, kd, False))
PIDcontroller.set_name('PID_controller')
builder.Connect(robot_state_encoder.joint_state_outport_port(),
PIDcontroller.get_input_port(0))
builder.Connect(
PIDcontroller.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
# Ref trajectory
traj_src = builder.AddSystem(ConstantVectorSource(zero_config))
builder.Connect(traj_src.get_output_port(0),
PIDcontroller.get_input_port(1))
# Robot State handle
robot_state_handle = builder.AddSystem(
RobotStateHandle(rb_tree)) # force_sensor_info
robot_state_handle.set_name('robot_state_handle')
builder.Connect(plant.state_output_port(),
robot_state_handle.joint_state_results_input_port())
logger_N = 4
logger = []
for i in range(logger_N):
logger.append([])
# Signal logger
signalLogRate = 100
logger[0] = builder.AddSystem(SignalLogger(num_pos * 2))
logger[0]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(plant.get_output_port(0), logger[0].get_input_port(0))
# cotroller command
logger[1] = builder.AddSystem(SignalLogger(num_actuators))
logger[1]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(PIDcontroller.get_output_port(0),
logger[1].get_input_port(0))
# torque
# other
logger[2] = builder.AddSystem(SignalLogger(3))
builder.Connect(robot_state_handle.com_outport_port(),
logger[2].get_input_port(0))
logger[2]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
logger[3] = builder.AddSystem(SignalLogger(num_actuators))
builder.Connect(plant.torque_output_port(), logger[3].get_input_port(0))
logger[3]._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
return builder.Build(), logger, plant
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate", type=float, default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument(
"--duration", type=float, default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware", action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--test", action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--filter_time_const", type=float, default=0.1,
help="Time constant for the first order low pass filter applied to"
"the teleop commands")
parser.add_argument(
"--velocity_limit_factor", type=float, default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
parser.add_argument(
'--schunk_collision_model', type=str, default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
parser.add_argument(
"-w", "--open-window", dest="browser_new",
action="store_const", const=1, default=None,
help=(
"Open the MeshCat display in a new browser window. NOTE: the "
"slider controls are available in the meshcat viewer by clicking "
"'Open Controls' in the top-right corner."))
args = parser.parse_args()
builder = DiagramBuilder()
# NOTE: the meshcat instance is always created in order to create the
# teleop controls (orientation sliders and open/close gripper button). When
# args.hardware is True, the meshcat server will *not* display robot
# geometry, but it will contain the joint sliders and open/close gripper
# button in the "Open Controls" tab in the top-right of the viewing server.
meshcat = Meshcat()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(
schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation(
schunk_model=schunk_model)
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
station.Finalize()
# If using meshcat, don't render the cameras, since RgbdCamera
# rendering only works with drake-visualizer. Without this check,
# running this code in a docker container produces libGL errors.
geometry_query_port = station.GetOutputPort("geometry_query")
# Connect the meshcat visualizer.
meshcat_visualizer = MeshcatVisualizer.AddToBuilder(
builder=builder,
query_object_port=geometry_query_port,
meshcat=meshcat)
# Configure the planar visualization.
if args.setup == 'planar':
meshcat.Set2dRenderMode()
# Connect and publish to drake visualizer.
DrakeVisualizer.AddToBuilder(builder, geometry_query_port)
image_to_lcm_image_array = builder.AddSystem(
ImageToLcmImageArrayT())
image_to_lcm_image_array.set_name("converter")
for name in station.get_camera_names():
cam_port = (
image_to_lcm_image_array
.DeclareImageInputPort[PixelType.kRgba8U](
"camera_" + name))
builder.Connect(
station.GetOutputPort("camera_" + name + "_rgb_image"),
cam_port)
image_array_lcm_publisher = builder.AddSystem(
LcmPublisherSystem.Make(
channel="DRAKE_RGBD_CAMERA_IMAGES",
lcm_type=lcmt_image_array,
lcm=None,
publish_period=0.1,
use_cpp_serializer=True))
image_array_lcm_publisher.set_name("rgbd_publisher")
builder.Connect(
image_to_lcm_image_array.image_array_t_msg_output_port(),
image_array_lcm_publisher.get_input_port(0))
if args.browser_new is not None:
url = meshcat.web_url()
webbrowser.open(url=url, new=args.browser_new)
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
time_step = 0.005
params.set_timestep(time_step)
# True velocity limits for the IIWA14 (in rad, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
if args.setup == 'planar':
# Extract the 3 joints that are not welded in the planar version.
iiwa14_velocity_limits = iiwa14_velocity_limits[1:6:2]
# The below constant is in body frame.
params.set_end_effector_velocity_gain([1, 0, 0, 0, 1, 1])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
factor*iiwa14_velocity_limits))
differential_ik = builder.AddSystem(DifferentialIK(
robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
teleop = builder.AddSystem(EndEffectorTeleop(
meshcat, args.setup == 'planar'))
filter = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat))
builder.Connect(wsg_buttons.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
# When in regression test mode, log our joint velocities to later check
# that they were sufficiently quiet.
num_iiwa_joints = station.num_iiwa_joints()
if args.test:
iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints))
builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"),
iiwa_velocities.get_input_port(0))
else:
iiwa_velocities = None
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(num_iiwa_joints))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
diagram.GetMutableSubsystemContext(
filter, simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
differential_ik, simulator.get_mutable_context()), q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.AdvanceTo(args.duration)
# Ensure that our initialization logic was correct, by inspecting our
# logged joint velocities.
if args.test:
iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context())
for time, qdot in zip(iiwa_velocities_log.sample_times(),
iiwa_velocities_log.data().transpose()):
# TODO(jwnimmer-tri) We should be able to do better than a 40
# rad/sec limit, but that's the best we can enforce for now.
if qdot.max() > 0.1:
print(f"ERROR: large qdot {qdot} at time {time}")
sys.exit(1)
# y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree)
# y_traj.add_constraint(t=0, derivative_order=0, lb=START_STATE[:3]) # initial position
# y_traj.add_constraint(t=0, derivative_order=1, lb=[0, 0, 0]) # initial velocity
# y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0, 0]) # initial acceleration
# y_traj.add_constraint(t=1, derivative_order=0, lb=[0, 1.0, 0])
# y_traj.add_constraint(t=2, derivative_order=0, lb=[0, 2.0, 0])
# y_traj.add_constraint(t=3, derivative_order=0, lb=[0, 3.0, 0])
# y_traj.add_constraint(t=tf, derivative_order=0, lb=[0, 4.0, 0]) # end at zero
# y_traj.add_constraint(t=tf, derivative_order=1, lb=[0, 0, 0])
# y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0])
# y_traj.generate()
# Build
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
plant.RegisterGeometry(scene_graph)
builder.Connect(plant.get_geometry_pose_output_port(),
scene_graph.get_source_pose_port(plant.source_id()))
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = BasicVector([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = BasicVector([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2)
# Initialize integrator states.
def get_mutable_continuous_state(system):
return (diagram.GetMutableSubsystemState(system, context)
.get_mutable_continuous_state())
integrator_xc = get_mutable_continuous_state(integrator)
integrator_xc.get_mutable_vector().SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = (context_i.get_state().get_continuous_state()
.get_vector().CopyToVector())
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
def main():
# NOR gate RS flip flop example
# Input topics /S and /R are active high (true is logic 1 and false is logic 0)
# Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1)
# Input/Output table
# S: false R: false | Q: no change Q_not: no change
# S: true R: false | Q: false Q_not: true
# S: false R: true | Q: true Q_not: false
# S: true R: true | Q: invalid Q_not: invalid
builder = DiagramBuilder()
sys_ros_interface = builder.AddSystem(RosInterfaceSystem())
qos = QoSProfile(depth=10)
sys_pub_Q = builder.AddSystem(
RosPublisherSystem(Bool, "Q", qos,
sys_ros_interface.get_ros_interface()))
sys_pub_Q_not = builder.AddSystem(
RosPublisherSystem(Bool, "Q_not", qos,
sys_ros_interface.get_ros_interface()))
sys_sub_S = builder.AddSystem(
RosSubscriberSystem(Bool, "S", qos,
sys_ros_interface.get_ros_interface()))
sys_sub_R = builder.AddSystem(
RosSubscriberSystem(Bool, "R", qos,
sys_ros_interface.get_ros_interface()))
sys_nor_gate_1 = builder.AddSystem(NorGate())
sys_nor_gate_2 = builder.AddSystem(NorGate())
sys_memory = builder.AddSystem(Memory(Bool()))
builder.Connect(sys_nor_gate_1.GetOutputPort('Q'),
sys_memory.get_input_port(0))
builder.Connect(
sys_sub_S.get_output_port(0),
sys_nor_gate_1.GetInputPort('A'),
)
builder.Connect(
sys_nor_gate_2.GetOutputPort('Q'),
sys_nor_gate_1.GetInputPort('B'),
)
builder.Connect(
sys_memory.get_output_port(0),
sys_nor_gate_2.GetInputPort('A'),
)
builder.Connect(
sys_sub_R.get_output_port(0),
sys_nor_gate_2.GetInputPort('B'),
)
builder.Connect(sys_nor_gate_1.GetOutputPort('Q'),
sys_pub_Q.get_input_port(0))
builder.Connect(sys_nor_gate_2.GetOutputPort('Q'),
sys_pub_Q_not.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
while True:
simulator.AdvanceTo(simulator_context.get_time() + 0.1)
class BabySystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("the_ins", BasicVector(1))
self.DeclareVectorOutputPort("the_outs", BasicVector(1),
self.CalcOutput)
def CalcOutput(self, context, output):
# print(output)
output.SetFromVector(np.array([0.1]))
builder = DiagramBuilder()
controller = builder.AddSystem(BabySystem())
integrator = builder.AddSystem(Integrator(1))
builder.Connect(controller.get_output_port(), integrator.get_input_port())
# builder.Connect(integrator)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
simulator = Simulator(diagram)
# integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(np.array([2.3]))
g = Source(diagram.GetGraphvizString())
g.view()
# print(context)
int_context = integrator.GetMyContextFromRoot(simulator.get_mutable_context())
import matplotlib.pyplot as plt
from pydrake.systems.drawing import plot_system_graphviz
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import Adder
builder = DiagramBuilder()
size = 1
adders = [
builder.AddSystem(Adder(1, size)),
builder.AddSystem(Adder(1, size)),
]
for i, adder in enumerate(adders):
adder.set_name("adders[{}]".format(i))
builder.Connect(adders[0].get_output_port(0), adders[1].get_input_port(0))
builder.ExportInput(adders[0].get_input_port(0))
builder.ExportOutput(adders[1].get_output_port(0))
diagram = builder.Build()
diagram.set_name("graphviz_example")
plot_system_graphviz(diagram)
plt.show()
"joint velocities. It multiplies each of the seven pre-defined "
"joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument(
'--setup', type=str, default='default',
help="The manipulation station setup to simulate. ",
choices=['default', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'default':
station.SetupDefaultStation()
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateDefaultYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
def setUp(self):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupDefaultStation()
station.Finalize()
# create the PointCloudToPoseSystem
config_file = "perception/config/sim.yml"
self.pc_to_pose = builder.AddSystem(
PointCloudToPoseSystem(config_file, viz=False))
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = self.pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = \
self.pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = self.pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(
station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(
station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(
station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(
station.GetOutputPort(middle_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(
station.GetOutputPort(right_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("right_point_cloud"))
diagram = builder.Build()
simulator = Simulator(diagram)
self.context = diagram.GetMutableSubsystemContext(
self.pc_to_pose, simulator.get_mutable_context())
def test_geometry(self):
builder = DiagramBuilder()
quadrotor = builder.AddSystem(QuadrotorPlant())
scene_graph = builder.AddSystem(SceneGraph())
state_port = quadrotor.get_output_port(0)
QuadrotorGeometry.AddToBuilder(builder, state_port, scene_graph)
"joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='default',
help="The manipulation station setup to simulate. ",
choices=['default', 'clutter_clearing'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
# Initializes the chosen station type.
if args.setup == 'default':
station.SetupDefaultStation()
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation()
ycb_objects = CreateDefaultYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
def test_warnings_and_errors(self):
builder = DiagramBuilder()
sg = builder.AddSystem(SceneGraph())
v2 = builder.AddSystem(MeshcatVisualizer(scene_graph=sg))
builder.Connect(sg.get_query_output_port(),
v2.get_geometry_query_input_port())
v2.set_name("v2")
v4 = builder.AddSystem(MeshcatVisualizer(scene_graph=None))
builder.Connect(sg.get_query_output_port(),
v4.get_geometry_query_input_port())
v4.set_name("v4")
v5 = ConnectMeshcatVisualizer(builder, scene_graph=sg)
v5.set_name("v5")
v7 = ConnectMeshcatVisualizer(builder,
scene_graph=sg,
output_port=sg.get_query_output_port())
v7.set_name("v7")
with self.assertRaises(AssertionError):
v8 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=None)
v8.set_name("v8")
v10 = ConnectMeshcatVisualizer(builder,
scene_graph=None,
output_port=sg.get_query_output_port())
v10.set_name("v10")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
def PublishWithNoWarnings(v):
with warnings.catch_warnings(record=True) as w:
v.Publish(v.GetMyContextFromRoot(context))
self.assertEqual(len(w), 0, [x.message for x in w])
# Use geometry_query API
v2.load(v2.GetMyContextFromRoot(context))
PublishWithNoWarnings(v2)
v2.load()
PublishWithNoWarnings(v2)
# Use geometry_query API
v4.load(v4.GetMyContextFromRoot(context))
PublishWithNoWarnings(v4)
with self.assertRaises(RuntimeError):
v4.load() # Can't work without scene_graph nor context.
# Use geometry_query API
v5.load(v5.GetMyContextFromRoot(context))
PublishWithNoWarnings(v5)
v5.load()
PublishWithNoWarnings(v5)
# Use geometry_query API
v7.load(v7.GetMyContextFromRoot(context))
PublishWithNoWarnings(v7)
v7.load()
PublishWithNoWarnings(v7)
# Use geometry_query API
v10.load(v10.GetMyContextFromRoot(context))
PublishWithNoWarnings(v10)
with self.assertRaises(RuntimeError):
v10.load() # Can't work without scene_graph nor context.
def test_lcm_driven_loop(self):
"""Duplicates the test logic in `lcm_driven_loop_test.cc`."""
lcm_url = "udpm://239.255.76.67:7669"
t_start = 3.
t_end = 10.
def publish_loop():
# Publishes a set of messages for the driven loop. This should be
# run from a separate process.
# N.B. Because of this, care should be taken not to share C++
# objects between process boundaries.
t = t_start
while t <= t_end:
message = header_t()
message.utime = int(1e6 * t)
lcm.Publish("TEST_LOOP", message.encode())
time.sleep(0.1)
t += 1
class DummySys(LeafSystem):
# Converts message to time in seconds.
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("header_t",
AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def _calc_output(self, context, output):
message = self.EvalAbstractInput(context, 0).get_value()
y = output.get_mutable_value()
y[:] = message.utime / 1e6
# Construct diagram for LcmDrivenLoop.
lcm = DrakeLcm(lcm_url)
utime = mut.PyUtimeMessageToSeconds(header_t)
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
builder = DiagramBuilder()
builder.AddSystem(sub)
dummy = builder.AddSystem(DummySys())
builder.Connect(sub.get_output_port(0), dummy.get_input_port(0))
logger = LogOutput(dummy.get_output_port(0), builder)
logger.set_forced_publish_only()
diagram = builder.Build()
dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime)
dut.set_publish_on_every_received_message(True)
# N.B. Use `multiprocessing` instead of `threading` so that we may
# avoid issues with GIL deadlocks.
publish_proc = Process(target=publish_loop)
publish_proc.start()
# Initialize to first message.
first_msg = dut.WaitForMessage()
dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg))
# Run to desired amount. (Anything more will cause interpreter to
# "freeze".)
dut.RunToSecondsAssumingInitialized(t_end)
publish_proc.join()
# Check expected values.
log_t_expected = np.array([4, 5, 6, 7, 8, 9])
log_t = logger.sample_times()
self.assertTrue(np.allclose(log_t_expected, log_t))
log_y = logger.data()
self.assertTrue(np.allclose(log_t_expected, log_y))
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)
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
builder = DiagramBuilder()
plant = builder.AddSystem(QuadrotorPlant())
controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1]))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
# Set up visualization in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
QuadrotorGeometry.AddToBuilder(builder, plant.get_output_port(0), scene_graph)
meshcat = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=args.meshcat,
open_browser=args.open_browser))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
meshcat.get_input_port(0))
# end setup for visualization
def test_contact_force(self):
"""A block sitting on a table."""
object_file_path = FindResourceOrThrow(
"drake/examples/manipulation_station/models/061_foam_brick.sdf")
table_file_path = FindResourceOrThrow(
"drake/examples/kuka_iiwa_arm/models/table/"
"extra_heavy_duty_table_surface_only_collision.sdf")
# T: tabletop frame.
X_TObject = RigidTransform([0, 0, 0.2])
builder = DiagramBuilder()
plant = MultibodyPlant(0.002)
_, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
# Weld table to world.
plant.WeldFrames(
A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model))
plant.Finalize()
# Add meshcat visualizer.
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
# Add contact visualizer.
contact_viz = builder.AddSystem(
MeshcatContactVisualizer(
meshcat_viz=viz,
force_threshold=0,
contact_force_scale=10,
plant=plant))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(
plant.GetOutputPort("contact_results"),
contact_input_port)
builder.Connect(
scene_graph.get_pose_bundle_output_port(),
contact_viz.GetInputPort("pose_bundle"))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
X_WT = plant.CalcRelativeTransform(
mbp_context,
plant.world_frame(),
plant.GetFrameByName("top_center"))
plant.SetFreeBodyPose(
mbp_context,
plant.GetBodyByName("base_link", object_model),
X_WT.multiply(X_TObject))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(1.0)
contact_viz_context = (
diagram.GetMutableSubsystemContext(contact_viz, diagram_context))
contact_results = contact_viz.EvalAbstractInput(
contact_viz_context,
contact_input_port.get_index()).get_value()
self.assertGreater(contact_results.num_point_pair_contacts(), 0)
self.assertEqual(contact_viz._contact_key_counter, 4)
from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import ConstantVectorSource
from pydrake.systems.planar_scenegraph_visualizer import (
PlanarSceneGraphVisualizer)
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/urdf/planar_iiwa14_spheres_dense_elbow_collision.urdf"
)) # noqa
plant.Finalize()
visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
torque_command = builder.AddSystem(ConstantVectorSource(np.zeros(3)))
builder.Connect(torque_command.get_output_port(0),
plant.get_actuation_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(1)
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# Test __str__ methods.
self.assertRegexpMatches(str(context), "integrator")
self.assertEqual(str(input2), "[0.003, 0.004, 0.005]")
# Initialize integrator states.
integrator_xc = (diagram.GetMutableSubsystemState(
integrator, context).get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
self.assertTrue(np.allclose(xc, xc_expected))
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--duration",
type=float,
default=np.inf,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--hardware",
action='store_true',
help="Use the ManipulationStationHardwareInterface instead of an "
"in-process simulation.")
parser.add_argument(
"--joystick_id",
type=int,
default=None,
help="Joystick ID to use (0..N-1). If not specified, then only one "
"joystick must be plugged in, and that joystick will be used.")
parser.add_argument("--test",
action='store_true',
help="Disable opening the gui window for testing.")
parser.add_argument(
"--time_step",
type=float,
default=0.005,
help="Time constant for the differential IK solver and first order"
"low pass filter applied to the teleop commands")
parser.add_argument(
"--velocity_limit_factor",
type=float,
default=1.0,
help="This value, typically between 0 and 1, further limits the "
"iiwa14 joint velocities. It multiplies each of the seven "
"pre-defined joint velocity limits. "
"Note: The pre-defined velocity limits are specified by "
"iiwa14_velocity_limits, found in this python file.")
parser.add_argument('--setup',
type=str,
default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing'])
parser.add_argument(
'--schunk_collision_model',
type=str,
default='box',
help="The Schunk collision model to use for simulation. ",
choices=['box', 'box_plus_fingertip_spheres'])
MeshcatVisualizer.add_argparse_argument(parser)
args = parser.parse_args()
if args.test:
# Don't grab mouse focus during testing.
# See: https://stackoverflow.com/a/52528832/7829525
os.environ["SDL_VIDEODRIVER"] = "dummy"
builder = DiagramBuilder()
if args.hardware:
station = builder.AddSystem(ManipulationStationHardwareInterface())
station.Connect(wait_for_cameras=False)
else:
station = builder.AddSystem(ManipulationStation())
if args.schunk_collision_model == "box":
schunk_model = SchunkCollisionModel.kBox
elif args.schunk_collision_model == "box_plus_fingertip_spheres":
schunk_model = SchunkCollisionModel.kBoxPlusFingertipSpheres
# Initializes the chosen station type.
if args.setup == 'manipulation_class':
station.SetupManipulationClassStation(schunk_model=schunk_model)
station.AddManipulandFromFile(
("drake/examples/manipulation_station/models/"
"061_foam_brick.sdf"),
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
elif args.setup == 'clutter_clearing':
station.SetupClutterClearingStation(schunk_model=schunk_model)
ycb_objects = CreateClutterClearingYcbObjectList()
for model_file, X_WObject in ycb_objects:
station.AddManipulandFromFile(model_file, X_WObject)
station.Finalize()
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
if args.meshcat:
meshcat = ConnectMeshcatVisualizer(
builder,
output_port=station.GetOutputPort("geometry_query"),
zmq_url=args.meshcat,
open_browser=args.open_browser)
if args.setup == 'planar':
meshcat.set_planar_viewpoint()
robot = station.get_controller_plant()
params = DifferentialInverseKinematicsParameters(robot.num_positions(),
robot.num_velocities())
params.set_timestep(args.time_step)
# True velocity limits for the IIWA14 (in rad/s, rounded down to the first
# decimal)
iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
# Stay within a small fraction of those limits for this teleop demo.
factor = args.velocity_limit_factor
params.set_joint_velocity_limits(
(-factor * iiwa14_velocity_limits, factor * iiwa14_velocity_limits))
differential_ik = builder.AddSystem(
DifferentialIK(robot, robot.GetFrameByName("iiwa_link_7"), params,
args.time_step))
builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
station.GetInputPort("iiwa_position"))
joystick = initialize_joystick(args.joystick_id)
teleop = builder.AddSystem(DualShock4Teleop(joystick))
filter_ = builder.AddSystem(
FirstOrderLowPassFilter(time_constant=args.time_step, size=6))
builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
builder.Connect(filter_.get_output_port(0),
differential_ik.GetInputPort("rpy_xyz_desired"))
builder.Connect(teleop.GetOutputPort("position"),
station.GetInputPort("wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
station.GetInputPort("wsg_force_limit"))
diagram = builder.Build()
simulator = Simulator(diagram)
# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, np.zeros(7))
# If the diagram is only the hardware interface, then we must advance it a
# little bit so that first LCM messages get processed. A simulated plant is
# already publishing correct positions even without advancing, and indeed
# we must not advance a simulated plant until the sliders and filters have
# been initialized to match the plant.
if args.hardware:
simulator.AdvanceTo(1e-6)
q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)
teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter_.set_initial_output_value(
diagram.GetMutableSubsystemContext(filter_,
simulator.get_mutable_context()),
teleop.get_output_port(0).Eval(
diagram.GetMutableSubsystemContext(
teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(
diagram.GetMutableSubsystemContext(differential_ik,
simulator.get_mutable_context()),
q0)
simulator.set_target_realtime_rate(args.target_realtime_rate)
print_instructions()
simulator.AdvanceTo(args.duration)
def test_context_api(self):
# Capture miscellaneous functions not yet tested.
model_value = AbstractValue.Make("Hello")
model_vector = BasicVector([1., 2.])
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareContinuousState(1)
self.DeclareDiscreteState(2)
self.DeclareAbstractState(model_value.Clone())
self.DeclareAbstractParameter(model_value.Clone())
self.DeclareNumericParameter(model_vector.Clone())
system = TrivialSystem()
context = system.CreateDefaultContext()
self.assertTrue(context.get_state() is context.get_mutable_state())
self.assertEqual(context.num_continuous_states(), 1)
self.assertTrue(context.get_continuous_state_vector() is
context.get_mutable_continuous_state_vector())
self.assertEqual(context.num_discrete_state_groups(), 1)
self.assertTrue(context.get_discrete_state_vector() is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state().get_vector(0))
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state().get_vector(0))
self.assertEqual(context.num_abstract_states(), 1)
self.assertTrue(context.get_abstract_state() is
context.get_mutable_abstract_state())
self.assertTrue(
context.get_abstract_state(0) is
context.get_mutable_abstract_state(0))
self.assertEqual(
context.get_abstract_state(0).get_value(), model_value.get_value())
# Check abstract state API (also test AbstractValues).
values = context.get_abstract_state()
self.assertEqual(values.size(), 1)
self.assertEqual(
values.get_value(0).get_value(), model_value.get_value())
self.assertEqual(
values.get_mutable_value(0).get_value(), model_value.get_value())
values.SetFrom(values.Clone())
# Check parameter accessors.
self.assertEqual(system.num_abstract_parameters(), 1)
self.assertEqual(
context.get_abstract_parameter(index=0).get_value(),
model_value.get_value())
self.assertEqual(system.num_numeric_parameter_groups(), 1)
np.testing.assert_equal(
context.get_numeric_parameter(index=0).get_value(),
model_vector.get_value())
# Check diagram context accessors.
builder = DiagramBuilder()
builder.AddSystem(system)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Existence check.
self.assertIsNot(diagram.GetMutableSubsystemState(system, context),
None)
subcontext = diagram.GetMutableSubsystemContext(system, context)
self.assertIsNot(subcontext, None)
self.assertIs(diagram.GetSubsystemContext(system, context), subcontext)
subcontext2 = system.GetMyMutableContextFromRoot(context)
self.assertIsNot(subcontext2, None)
self.assertIs(subcontext2, subcontext)
self.assertIs(system.GetMyContextFromRoot(context), subcontext2)
def GetBrickPose(config_file, viz=False):
"""Estimates the pose of the foam brick in a ManipulationStation setup.
@param config_file str. The path to a camera configuration file.
@param viz bool. If True, save point clouds to numpy arrays.
@return An Isometry3 representing the pose of the brick.
"""
builder = DiagramBuilder()
# create the PointCloudToPoseSystem
pc_to_pose = builder.AddSystem(
PointCloudToPoseSystem(config_file, viz, SegmentFoamBrick,
GetFoamBrickPose))
# realsense serial numbers are >> 100
use_hardware = int(pc_to_pose.camera_configs["left_camera_serial"]) > 100
if use_hardware:
camera_ids = [
pc_to_pose.camera_configs["left_camera_serial"],
pc_to_pose.camera_configs["middle_camera_serial"],
pc_to_pose.camera_configs["right_camera_serial"]
]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect()
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = \
"drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = \
"camera_" + pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = \
"camera_" + pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = \
"camera_" + pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(station.GetOutputPort(middle_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(station.GetOutputPort(right_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("right_point_cloud"))
diagram = builder.Build()
simulator = Simulator(diagram)
if not use_hardware:
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow(
station.get_mutable_multibody_plant().GetBodyByName(
"base_link", brick), X_WObject,
station.GetMutableSubsystemContext(
station.get_mutable_multibody_plant(), station_context))
context = diagram.GetMutableSubsystemContext(
pc_to_pose, simulator.get_mutable_context())
# returns the pose of the brick, of type Isometry3
return pc_to_pose.GetOutputPort("X_WObject").Eval(context)
def test_render_engine_api(self):
class DummyRenderEngine(mut.render.RenderEngine):
"""Mirror of C++ DummyRenderEngine."""
# See comment below about `rgbd_sensor_test.cc`.
latest_instance = None
def __init__(self, render_label=None):
mut.render.RenderEngine.__init__(self)
# N.B. We do not hide these because this is a test class.
# Normally, you would want to hide this.
self.force_accept = False
self.registered_geometries = set()
self.updated_ids = {}
self.include_group_name = "in_test"
self.X_WC = RigidTransform_[float]()
self.color_count = 0
self.depth_count = 0
self.label_count = 0
self.color_camera = None
self.depth_camera = None
self.label_camera = None
def UpdateViewpoint(self, X_WC):
DummyRenderEngine.latest_instance = self
self.X_WC = X_WC
def ImplementGeometry(self, shape, user_data):
DummyRenderEngine.latest_instance = self
def DoRegisterVisual(self, id, shape, properties, X_WG):
DummyRenderEngine.latest_instance = self
mut.GetRenderLabelOrThrow(properties)
if self.force_accept or properties.HasGroup(
self.include_group_name
):
self.registered_geometries.add(id)
return True
return False
def DoUpdateVisualPose(self, id, X_WG):
DummyRenderEngine.latest_instance = self
self.updated_ids[id] = X_WG
def DoRemoveGeometry(self, id):
DummyRenderEngine.latest_instance = self
self.registered_geometries.remove(id)
def DoClone(self):
DummyRenderEngine.latest_instance = self
new = DummyRenderEngine()
new.force_accept = copy.copy(self.force_accept)
new.registered_geometries = copy.copy(
self.registered_geometries)
new.updated_ids = copy.copy(self.updated_ids)
new.include_group_name = copy.copy(self.include_group_name)
new.X_WC = copy.copy(self.X_WC)
new.color_count = copy.copy(self.color_count)
new.depth_count = copy.copy(self.depth_count)
new.label_count = copy.copy(self.label_count)
new.color_camera = copy.copy(self.color_camera)
new.depth_camera = copy.copy(self.depth_camera)
new.label_camera = copy.copy(self.label_camera)
return new
def DoRenderColorImage(self, camera, color_image_out):
DummyRenderEngine.latest_instance = self
self.color_count += 1
self.color_camera = camera
def DoRenderDepthImage(self, camera, depth_image_out):
DummyRenderEngine.latest_instance = self
self.depth_count += 1
self.depth_camera = camera
def DoRenderLabelImage(self, camera, label_image_out):
DummyRenderEngine.latest_instance = self
self.label_count += 1
self.label_camera = camera
engine = DummyRenderEngine()
self.assertIsInstance(engine, mut.render.RenderEngine)
self.assertIsInstance(engine.Clone(), DummyRenderEngine)
# Test implementation of C++ interface by using RgbdSensor.
renderer_name = "renderer"
builder = DiagramBuilder()
scene_graph = builder.AddSystem(mut.SceneGraph())
# N.B. This passes ownership.
scene_graph.AddRenderer(renderer_name, engine)
sensor = builder.AddSystem(RgbdSensor(
parent_id=scene_graph.world_frame_id(),
X_PB=RigidTransform(),
depth_camera=mut.render.DepthRenderCamera(
mut.render.RenderCameraCore(
renderer_name, CameraInfo(640, 480, np.pi/4),
mut.render.ClippingRange(0.1, 5.0), RigidTransform()),
mut.render.DepthRange(0.1, 5.0))))
builder.Connect(
scene_graph.get_query_output_port(),
sensor.query_object_input_port(),
)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyContextFromRoot(diagram_context)
image = sensor.color_image_output_port().Eval(sensor_context)
# N.B. Because there's context cloning going on under the hood, we
# won't be interacting with our originally registered instance.
# See `rgbd_sensor_test.cc` as well.
current_engine = DummyRenderEngine.latest_instance
self.assertIsNot(current_engine, engine)
self.assertIsInstance(image, ImageRgba8U)
self.assertEqual(current_engine.color_count, 1)
image = sensor.depth_image_32F_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth32F)
self.assertEqual(current_engine.depth_count, 1)
image = sensor.depth_image_16U_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth16U)
self.assertEqual(current_engine.depth_count, 2)
image = sensor.label_image_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageLabel16I)
self.assertEqual(current_engine.label_count, 1)
def __init__(self):
#import pdb; pdb.set_trace()
# Create a block diagram containing our system.
builder = DiagramBuilder()
# add the two decoupled plants (x(s)=u/s^2; y(s)=u/s^2)
plant_x = builder.AddSystem(DoubleIntegrator())
plant_y = builder.AddSystem(DoubleIntegrator())
plant_x.set_name("double_integrator_x")
plant_y.set_name("double_integrator_y")
# add the controller, lead compensator for now just to stablize it
controller_x = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_y = builder.AddSystem(
Controller(tau_num=2., tau_den=.2, k=1.))
controller_x.set_name("controller_x")
controller_y.set_name("controller_y")
# the perception's "Beam" model with the four sources of noise
x_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
x_meas_fb.set_name("x_fb")
y_meas_fb = builder.AddSystem(Adder(num_inputs=4, size=1))
y_meas_fb.set_name("y_fb")
y_perception = builder.AddSystem(Adder(num_inputs=2, size=1))
y_perception.set_name("range_measurment_y")
neg_y_meas = builder.AddSystem(Gain(k=-1., size=1))
neg_y_meas.set_name("neg_y_meas")
wall_position = builder.AddSystem(ConstantVectorSource([Y_wall]))
p_hit = builder.AddSystem(RandomSource(distribution=RandomDistribution.kGaussian, num_outputs=2,\
sampling_interval_sec=0.01))
p_hit.set_name("GaussionNoise(0,1)")
p_short = builder.AddSystem(RandomSource(distribution=RandomDistribution.kExponential, num_outputs=2,\
sampling_interval_sec=0.01))
p_short.set_name("ExponentialNoise(1)")
#p_max = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=1,\
# sampling_interval_sec=0.01))
p_rand = builder.AddSystem(RandomSource(distribution=RandomDistribution.kUniform, num_outputs=2,\
sampling_interval_sec=0.01))
p_rand.set_name("UniformNoise(0,1)")
#import pdb; pdb.set_trace()
p_hit_Dx = builder.AddSystem(Demultiplexer(size=2))
p_hit_Dx.set_name('Dmux1')
p_short_Dx = builder.AddSystem(Demultiplexer(size=2))
p_short_Dx.set_name('Dmux2')
p_rand_Dx = builder.AddSystem(Demultiplexer(size=2))
p_rand_Dx.set_name('Dmux3')
normgain_x = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_x.set_name("Sigma_x")
normgain_y = builder.AddSystem(Gain(k=normal_coeff, size=1))
normgain_y.set_name("Sigma_y")
expgain_x = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_x.set_name("Exp_x")
expgain_y = builder.AddSystem(Gain(k=exp_coeff, size=1))
expgain_y.set_name("Exp_y")
randgain_x = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_x.set_name("Rand_x")
randgain_y = builder.AddSystem(Gain(k=rand_coeff, size=1))
randgain_y.set_name("Rand_y")
#maxgain_x = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_x.set_name("Max_x")
#maxgain_y = builder.AddSystem(Adder(num_inputs=2, size=1))
#maxgain_y.set_name("Max_y")
# the summation to get the error (closing the loop)
summ_x = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_y = builder.AddSystem(Adder(num_inputs=2, size=1))
summ_x.set_name("summation_x")
summ_y.set_name("summation_y")
neg_x = builder.AddSystem(Gain(k=-1., size=1))
neg_y = builder.AddSystem(Gain(k=-1., size=1))
neg_uy = builder.AddSystem(Gain(k=-1., size=1))
neg_x.set_name("neg_x")
neg_y.set_name("neg_y")
neg_uy.set_name("neg_uy")
# wire up all the blocks (summation to the controller to the plant ...)
builder.Connect(summ_x.get_output_port(0),
controller_x.get_input_port(0)) # e_x
builder.Connect(summ_y.get_output_port(0),
controller_y.get_input_port(0)) # e_y
builder.Connect(controller_x.get_output_port(0),
plant_x.get_input_port(0)) # u_x
builder.Connect(
controller_y.get_output_port(0),
neg_uy.get_input_port(0)) # u_y (to deal with directions)
builder.Connect(neg_uy.get_output_port(0),
plant_y.get_input_port(0)) # u_y
builder.Connect(
plant_x.get_output_port(0),
x_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(wall_position.get_output_port(0),
y_perception.get_input_port(0)) # perception
builder.Connect(plant_y.get_output_port(0),
neg_y_meas.get_input_port(0)) # perception
builder.Connect(neg_y_meas.get_output_port(0),
y_perception.get_input_port(1)) # perception, z meas
builder.Connect(
y_perception.get_output_port(0),
y_meas_fb.get_input_port(0)) # perception, nominal state meas
builder.Connect(p_hit.get_output_port(0),
p_hit_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_hit_Dx.get_output_port(0),
normgain_x.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_x.get_output_port(0),
x_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_hit_Dx.get_output_port(1),
normgain_y.get_input_port(0)) # normalize Normal dist
builder.Connect(normgain_y.get_output_port(0),
y_meas_fb.get_input_port(1)) # Normal dist
builder.Connect(p_short.get_output_port(0),
p_short_Dx.get_input_port(0)) # demux the noise
builder.Connect(p_short_Dx.get_output_port(0),
expgain_x.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_x.get_output_port(0),
x_meas_fb.get_input_port(2)) # Exp dist
builder.Connect(p_short_Dx.get_output_port(1),
expgain_y.get_input_port(0)) # normalize Exp dist
builder.Connect(expgain_y.get_output_port(0),
y_meas_fb.get_input_port(2)) # Exp dist
#builder.Connect(p_max.get_output_port(0), x_meas_fb.get_input_port(3)) # Uniform dist
#builder.Connect(p_max.get_output_port(0), y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand.get_output_port(0),
p_rand_Dx.get_input_port(0)) # normalize Uniform dist
builder.Connect(p_rand_Dx.get_output_port(0),
randgain_x.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_x.get_output_port(0),
x_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(p_rand_Dx.get_output_port(1),
randgain_y.get_input_port(0)) # normalize Uniform dist
builder.Connect(randgain_y.get_output_port(0),
y_meas_fb.get_input_port(3)) # Uniform dist
builder.Connect(x_meas_fb.get_output_port(0),
neg_x.get_input_port(0)) # -x
builder.Connect(y_meas_fb.get_output_port(0),
neg_y.get_input_port(0)) # -y
builder.Connect(neg_x.get_output_port(0),
summ_x.get_input_port(1)) # r-x
builder.Connect(neg_y.get_output_port(0),
summ_y.get_input_port(1)) # r-y
# Make the desired_state input of the controller, an input to the diagram.
builder.ExportInput(summ_x.get_input_port(0))
builder.ExportInput(summ_y.get_input_port(0))
# get telemetry
logger_x = LogOutput(plant_x.get_output_port(0), builder)
logger_noise_x = LogOutput(x_meas_fb.get_output_port(0), builder)
logger_y = LogOutput(plant_y.get_output_port(0), builder)
logger_noise_y = LogOutput(y_meas_fb.get_output_port(0), builder)
logger_x.set_name("logger_x_state")
logger_y.set_name("logger_y_state")
logger_noise_x.set_name("x_state_meas")
logger_noise_y.set_name("y_meas")
# compile the system
diagram = builder.Build()
diagram.set_name("diagram")
# Create the simulator, and simulate for 10 seconds.
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# First we extract the subsystem context for the plant
plant_context_x = diagram.GetMutableSubsystemContext(plant_x, context)
plant_context_y = diagram.GetMutableSubsystemContext(plant_y, context)
# Then we can set the pendulum state, which is (x, y).
z0 = X_0
plant_context_x.get_mutable_continuous_state_vector().SetFromVector(z0)
z0 = Y_0
plant_context_y.get_mutable_continuous_state_vector().SetFromVector(z0)
# The diagram has a single input port (port index 0), which is the desired_state.
context.FixInputPort(
0,
[X_d]) # here we assume no perception, closing feedback on state X
context.FixInputPort(
1, [Z_d]) #Z_y, keep 3m away, basically we want to be at Y=0
# run the simulation
simulator.AdvanceTo(10)
t = logger_x.sample_times()
#import pdb; pdb.set_trace()
# Plot the results.
plt.figure()
plot_system_graphviz(diagram, max_depth=2)
plt.figure()
plt.plot(t, logger_noise_x.data().transpose(), label='x_state_meas')
plt.plot(t, logger_noise_y.data().transpose(), label='y_meas (z(y))')
plt.plot(t, logger_x.data().transpose(), label='x_state')
plt.plot(t, logger_y.data().transpose(), label='y_state')
plt.xlabel('t')
plt.ylabel('x(t),y(t)')
plt.legend()
plt.show(block=True)
