def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger_per_step = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))
# Add a redundant logger via the helper method.
logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder)
# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger(kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
kTime = 1.
simulator.AdvanceTo(kTime)
# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger_per_step_2.data())
# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)
logger_per_step.reset()
# Verify that t and x retain their values after systems are deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())
def build_diagram(mbp,
scene_graph,
robot,
gripper,
meshcat=False,
controllers=False):
builder = DiagramBuilder()
builder.AddSystem(scene_graph)
builder.AddSystem(mbp)
# Connect scene_graph to MBP for collision detection.
builder.Connect(mbp.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(mbp.get_source_id()))
builder.Connect(scene_graph.get_query_output_port(),
mbp.get_geometry_query_input_port())
if meshcat:
vis = add_meshcat_visualizer(scene_graph, builder)
else:
vis = add_drake_visualizer(scene_graph, builder)
state_log = builder.AddSystem(
SignalLogger(mbp.get_continuous_state_output_port().size()))
state_log._DeclarePeriodicPublish(0.02)
builder.Connect(mbp.get_continuous_state_output_port(),
state_log.get_input_port(0))
state_machine = None
if controllers:
state_machine = connect_controllers(builder, mbp, robot, gripper)
return builder.Build(), state_machine
def main():
# Simulate with doubles.
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
adder = builder.AddSystem(SimpleAdder(100.))
builder.Connect(source.get_output_port(0), adder.get_input_port(0))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(adder.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.AdvanceTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 110.)
# Compute outputs with AutoDiff and Symbolic.
for T in (AutoDiffXd, Expression):
adder_T = SimpleAdder_[T](100.)
context = adder_T.CreateDefaultContext()
adder_T.get_input_port().FixValue(context, [10.])
output = adder_T.AllocateOutput()
adder_T.CalcOutput(context, output)
# N.B. At present, you cannot get a reference to existing AutoDiffXd
# or Expression numpy arrays, so we will explictly copy the vector:
# https://github.com/RobotLocomotion/drake/issues/8116
value, = output.get_vector_data(0).CopyToVector()
assert isinstance(value, T)
print("Output from {}: {}".format(type(adder_T), repr(value)))
def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger.get_input_port(0))
# Add a redundant logger via the helper method.
logger2 = LogOutput(integrator.get_output_port(0), builder)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
t = logger.sample_times()
x = logger.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger2.data())
logger.reset()
def PlotJointLog(iiwa_joint_log: SignalLogger, legend: str, y_label: str):
'''
Plots per-joint quantities from its signal logger system.
'''
fig = plt.figure(figsize=(8, 14), dpi=100)
t = iiwa_joint_log.sample_times()
num_plot_poins = 1000
n = len(t)
indices = subsample_from_length_to_n(num_plot_poins, n)
for i, signal in enumerate(iiwa_joint_log.data()):
ax = fig.add_subplot(711 + i)
ax.plot(t[indices], signal[indices], label=legend + str(i + 1))
ax.set_xlabel("t(s)")
ax.set_ylabel(y_label)
ax.legend()
ax.grid(True)
plt.tight_layout()
plt.show()
def main():
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource([10.]))
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(source.get_output_port(0), logger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
x = logger.data()
print("Output values: {}".format(x))
assert np.allclose(x, 10.)
def test_simple_visualizer(self):
builder = DiagramBuilder()
system = builder.AddSystem(SimpleContinuousTimeSystem())
logger = builder.AddSystem(SignalLogger(1))
builder.Connect(system.get_output_port(0), logger.get_input_port(0))
visualizer = builder.AddSystem(TestVisualizer(1))
builder.Connect(system.get_output_port(0),
visualizer.get_input_port(0))
diagram = builder.Build()
context = diagram.CreateDefaultContext()
context.SetContinuousState([0.9])
simulator = Simulator(diagram, context)
simulator.AdvanceTo(.1)
ani = visualizer.animate(logger, repeat=True)
self.assertIsInstance(ani, animation.FuncAnimation)
# Sanity check on the availability of the optional source id before using it.
assert robot_plant.geometry_source_is_registered()
assert robot_plant.num_positions() == 7
print('get_source_id : ', robot_plant.get_source_id())
# Boilerplate used to connect the plant to a SceneGraph for visualization.
lcm = DrakeLcm
builder.Connect(scene_graph.get_query_output_port(),
robot_plant.get_geometry_query_input_port())
builder.Connect(robot_plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(robot_plant.get_source_id()))
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
# 3.Signal logger
logger = builder.AddSystem(SignalLogger(14))
builder.Connect(robot_plant.get_continuous_state_output_port(), logger.get_input_port(0))
class PoseOutput(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.num_vec = 3
# self._DeclareContinuousState(num_joints, num_particles, 0)
self._DeclareVectorOutputPort(BasicVector(self.num_vec),
self.CalcOutput)
def CalcOutput(self, context, output):
vec = robot_plant.tree().EvalBodyPoseInWorld(plant_context, robot_plant.GetBodyByName('iiwa_link_7')).matrix()
def RunRealRobot(self, plans_list, gripper_setpoint_list):
'''
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
from pydrake.examples.manipulation_station import ManipulationStationHardwareInterface
builder = DiagramBuilder()
self.station_hardware = ManipulationStationHardwareInterface()
builder.AddSystem(self.station_hardware)
# Add plan runner
plan_runner = KukaPlanRunner(self.plant)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.get_output_port(0),
self.station_hardware.GetInputPort("iiwa_position"))
# Add state machine.
state_machine = ManipStateMachine(
plant=self.plant,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(state_machine)
builder.Connect(state_machine.kuka_plan_output_port,
plan_runner.plan_input_port)
builder.Connect(state_machine.hand_setpoint_output_port,
self.station_hardware.GetInputPort("wsg_position"))
builder.Connect(state_machine.gripper_force_limit_output_port,
self.station_hardware.GetInputPort("wsg_force_limit"))
builder.Connect(
self.station_hardware.GetOutputPort("iiwa_position_measured"),
state_machine.iiwa_position_input_port)
# Add logger
iiwa_position_command_log = builder.AddSystem(
SignalLogger(
self.station_hardware.GetInputPort("iiwa_position").size()))
builder.Connect(plan_runner.get_output_port(0),
iiwa_position_command_log.get_input_port(0))
# build diagram
diagram = builder.Build()
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station_hardware, simulator.get_mutable_context())
context.FixInputPort(
self.station.GetInputPort("iiwa_feedforward_torque").get_index(),
np.zeros(7))
simulator.set_target_realtime_rate(1.0)
simulator.Initialize()
# simulator.set_publish_at_initialization(False)
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
print "sending trajectories in 2 seconds..."
time.sleep(1.0)
print "sending trajectories in 1 second..."
time.sleep(1.0)
print "sending trajectories now!"
simulator.StepTo(sim_duration)
return iiwa_position_command_log
def LittleDogSimulationDiagram(lcm, rb_tree, dt, drake_visualizer):
builder = DiagramBuilder()
num_pos = rb_tree.get_num_positions()
num_actuators = rb_tree.get_num_actuators()
# RigidBodyPlant
plant = builder.AddSystem(RigidBodyPlant(rb_tree, dt))
plant.set_name('plant')
# Robot command subscriber
robot_command_subscriber = builder.AddSystem(
LcmSubscriberSystem.Make('ROBOT_COMMAND', littledog_command_t, lcm))
robot_command_subscriber.set_name('robot_command_subscriber')
# Robot command to Plant Converter
robot_command_to_rigidbodyplant_converter = builder.AddSystem(
RobotCommandToRigidBodyPlantLCMConverter(
rb_tree.actuators)) # input argu: rigidbody actuators
robot_command_to_rigidbodyplant_converter.set_name(
'robot_command_to_rigidbodyplant_converter')
# Visualizer
visualizer_publisher = builder.AddSystem(drake_visualizer)
visualizer_publisher.set_name('visualizer_publisher')
visualizer_publisher.set_publish_period(0.02)
# Visualize
#visualizer_publisher = builder.AddSystem(MeshcatRigidBodyVisualizer(rb_tree))
# Robot State Encoder
robot_state_encoder = builder.AddSystem(
RobotStateLCMEncoder(rb_tree)) # force_sensor_info
robot_state_encoder.set_name('robot_state_encoder')
# Robot State Publisher
robot_state_publisher = builder.AddSystem(
LcmPublisherSystem.Make('EST_ROBOT_STATE', robot_state_t, lcm))
robot_state_publisher.set_name('robot_state_publisher')
robot_state_publisher.set_publish_period(0.005)
# Connect everything
builder.Connect(
robot_command_subscriber.get_output_port(0),
robot_command_to_rigidbodyplant_converter.robot_command_input_port())
builder.Connect(
robot_command_to_rigidbodyplant_converter.desired_effort_output_port(),
plant.get_input_port(0))
builder.Connect(plant.state_output_port(),
visualizer_publisher.get_input_port(0))
# builder.Connect(plant.get_output_port(0),
# visualizer_publisher.get_input_port(0))
builder.Connect(plant.state_output_port(),
robot_state_encoder.joint_state_results_input_port())
builder.Connect(robot_state_encoder.lcm_message_output_port(),
robot_state_publisher.get_input_port(0))
# Signal logger
logger = builder.AddSystem(SignalLogger(num_pos * 2))
builder.Connect(plant.state_output_port(), logger.get_input_port(0))
return builder.Build(), logger, plant
def RunSimulation(self,
plans_list,
gripper_setpoint_list,
extra_time=0,
real_time_rate=1.0,
q0_kuka=np.zeros(7)):
'''
Constructs a Diagram that sends commands to ManipulationStation.
:param plans_list:
:param gripper_setpoint_list:
:param extra_time:
:param real_time_rate:
:param q0_kuka:
:return:
'''
builder = DiagramBuilder()
builder.AddSystem(self.station)
# Add plan runner
plan_runner = KukaPlanRunner(self.plant)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
# Add state machine.
state_machine = ManipStateMachine(
plant=self.plant,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(state_machine)
builder.Connect(state_machine.kuka_plan_output_port,
plan_runner.plan_input_port)
builder.Connect(state_machine.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(state_machine.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
state_machine.iiwa_position_input_port)
# Add meshcat visualizer
from underactuated.meshcat_visualizer import MeshcatVisualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph)
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.get_input_port(0))
# Add logger
iiwa_position_command_log = builder.AddSystem(
SignalLogger(self.station.GetInputPort("iiwa_position").size()))
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
builder.Connect(plan_runner.get_output_port(0),
iiwa_position_command_log.get_input_port(0))
# build diagram
diagram = builder.Build()
viz.load()
time.sleep(2.0)
RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
context = diagram.GetMutableSubsystemContext(
self.station, simulator.get_mutable_context())
# set initial state of the robot
self.station.SetIiwaPosition(q0_kuka, context)
self.station.SetIiwaVelocity(np.zeros(7), context)
self.station.SetWsgState(0.05, 0, context)
# set initial hinge angles of the cupboard.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-np.pi / 2)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=np.pi / 2)
# set initial pose of the object
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
# fix feedforward torque input to 0.
context.FixInputPort(
self.station.GetInputPort("iiwa_feedforward_torque").get_index(),
np.zeros(7))
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(real_time_rate)
simulator.Initialize()
sim_duration = 0.
for plan in plans_list:
sim_duration += plan.get_duration() * 1.1
sim_duration += extra_time
print "simulation duration", sim_duration
simulator.StepTo(sim_duration)
return iiwa_position_command_log
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'])
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())
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.
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()
elif args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
else:
DrakeVisualizer.AddToBuilder(builder,
station.GetOutputPort("query_object"))
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=image_array_t,
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))
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(args.setup == 'planar'))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
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(teleop.window))
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(SignalLogger(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:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.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)
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
ref_conf = np.concatenate(
(np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]), np.zeros(9)), axis=0)
traj_src = builder.AddSystem(ConstantVectorSource(ref_conf))
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.state_output_port(), logger.get_input_port(0))
diagram = builder.Build()
# init_state = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)#.reshape((-1,1))
# print(init_state.shape)
# simulation setting
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)
#plant.set_state_vector(diagram_context, init_state)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
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 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
torque_system = builder.AddSystem(
ConstantVectorSource(
np.ones((rbt.get_num_actuators(), 1)) * torque))
builder.Connect(torque_system.get_output_port(0),
rbplant_sys.get_input_port(0))
print('Simulating with constant torque = ' + str(torque) +
' Newton-meters')
# Visualize
visualizer = builder.AddSystem(pbrv)
builder.Connect(rbplant_sys.get_output_port(0),
visualizer.get_input_port(0))
# And also log
signalLogRate = 60
signalLogger = builder.AddSystem(SignalLogger(nx))
signalLogger._DeclarePeriodicPublish(1. / signalLogRate, 0.0)
builder.Connect(rbplant_sys.get_output_port(0),
signalLogger.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
# TODO(russt): Clean up state vector access below.
state = simulator.get_mutable_context().get_mutable_state()\
.get_mutable_continuous_state().get_mutable_vector()
if set_initial_state:
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(
'--setup', type=str, default='manipulation_class',
help="The manipulation station setup to simulate. ",
choices=['manipulation_class', 'clutter_clearing', 'planar'])
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())
# 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)
elif args.setup == 'planar':
station.SetupPlanarIiwaStation()
station.AddManipulandFromFile(
"drake/examples/manipulation_station/models/"
+ "061_foam_brick.sdf",
RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0]))
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,
open_browser=args.open_browser))
builder.Connect(station.GetOutputPort("pose_bundle"),
meshcat.get_input_port(0))
if args.setup == 'planar':
pyplot_visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(
station.get_scene_graph()))
builder.Connect(station.GetOutputPort("pose_bundle"),
pyplot_visualizer.get_input_port(0))
teleop = builder.AddSystem(JointSliders(station.get_controller_plant(),
length=800))
if args.test:
teleop.window.withdraw() # Don't display the window when testing.
num_iiwa_joints = station.num_iiwa_joints()
filter = builder.AddSystem(FirstOrderLowPassFilter(
time_constant=2.0, size=num_iiwa_joints))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
station.GetInputPort("iiwa_position"))
wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
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.
if args.test:
iiwa_velocities = builder.AddSystem(SignalLogger(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)
# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
station_context)
teleop.set_position(q0)
filter.set_initial_output_value(diagram.GetMutableSubsystemContext(
filter, 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:
for time, qdot in zip(iiwa_velocities.sample_times(),
iiwa_velocities.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)