def main():
args_parser = argparse.ArgumentParser(
description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
args_parser.add_argument("filename",
type=str,
help="Path to an SDF or URDF file.")
args_parser.add_argument(
"--package_path",
type=str,
default=None,
help="Full path to the root package for reading in SDF resources.")
position_group = args_parser.add_mutually_exclusive_group()
position_group.add_argument(
"--position",
type=float,
nargs="+",
default=[],
help="A list of positions which must be the same length as the number "
"of positions in the sdf model. Note that most models have a "
"floating-base joint by default (unless the sdf explicitly welds "
"the base to the world, and so have 7 positions corresponding to "
"the quaternion representation of that floating-base position.")
position_group.add_argument(
"--joint_position",
type=float,
nargs="+",
default=[],
help="A list of positions which must be the same length as the number "
"of positions ASSOCIATED WITH JOINTS in the sdf model. This "
"does not include, e.g., floating-base coordinates, which will "
"be assigned a default value.")
args_parser.add_argument(
"--test",
action='store_true',
help="Disable opening the gui window for testing.")
# TODO(russt): Add option to weld the base to the world pending the
# availability of GetUniqueBaseBody requested in #9747.
args = args_parser.parse_args()
filename = args.filename
if not os.path.isfile(filename):
args_parser.error("File does not exist: {}".format(filename))
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Construct a MultibodyPlant.
plant = MultibodyPlant()
plant.RegisterAsSourceForSceneGraph(scene_graph)
# Create the parser.
parser = Parser(plant)
# Get the package pathname.
if args.package_path:
# Verify that package.xml is found in the designated path.
package_path = os.path.abspath(args.package_path)
if not os.path.isfile(os.path.join(package_path, "package.xml")):
parser.error("package.xml not found at: {}".format(package_path))
# Get the package map and populate it using the package path.
package_map = parser.package_map()
package_map.PopulateFromFolder(package_path)
# Add the model from the file and finalize the plant.
parser.AddModelFromFile(filename)
plant.Finalize(scene_graph)
# Add sliders to set positions of the joints.
sliders = builder.AddSystem(JointSliders(robot=plant))
to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
builder.Connect(to_pose.get_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()))
# Connect this to drake_visualizer.
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
if len(args.position):
sliders.set_position(args.position)
elif len(args.joint_position):
sliders.set_joint_position(args.joint_position)
# Make the diagram and run it.
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_publish_every_time_step(False)
if args.test:
sliders.window.withdraw()
simulator.StepTo(0.1)
else:
simulator.set_target_realtime_rate(1.0)
simulator.StepTo(np.inf)
def setUp(self):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
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 generate(
model_manifest,
seed_value,
get_num_model_instances,
use_height_heuristic=False,
min_realtime_rate=1.,
max_sim_time=1.,
visualize=False,
visualize_height_heuristic=False,
sim_rate=0.,
sim_dt=0.05,
):
"""Generates poses for a given scene."""
# TOOD(eric.cousineau): This may not be fully deterministic? Random state
# may be leaking in from somewhere else?
seed(seed_value)
timing = pd.DataFrame(np.zeros(1, timing_dtype))
time_construction = make_timer(timing, 'construction')
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=5e-4)
frame_O_list, frame_Sink = add_sink_scene(model_manifest, plant,
scene_graph,
get_num_model_instances)
num_manipulands = len(frame_O_list)
if num_manipulands == 0:
raise RetryError(time_construction(), "No manipulands (empty sink)")
plant.Finalize()
if visualize:
ConnectDrakeVisualizer(builder, scene_graph)
diagram = builder.Build()
d_context = diagram.CreateDefaultContext()
context = plant.GetMyContextFromRoot(d_context)
time_construction()
time_posturing = make_timer(timing, 'posturing')
# Try to sample different configurations that are collision-free.
X_SinkO_list = generate_manipuland_sink_poses(num_manipulands)
set_manipuland_poses(plant, context, frame_Sink, frame_O_list,
X_SinkO_list)
# If there are any existing collisions, reject this sample.
query_object = plant.get_geometry_query_input_port().Eval(context)
if query_object.HasCollisions():
raise RetryError(time_posturing(), f"Collision in initial config")
time_posturing()
# Initilialize simulator here so that the visualization can be initialized
# (if enabled).
simulator = Simulator(diagram, d_context)
simulator.Initialize()
time_projection = make_timer(timing, 'projection')
if use_height_heuristic:
def height_heuristic_show(debug_name):
if visualize_height_heuristic:
print(f" height_heuristic_show: {debug_name}")
diagram.Publish(d_context)
input(f" Press Enter...\n")
# All objects in this pose should be collision free. Record their
# heights.
zs_free = get_frame_heights(plant, context, frame_O_list)
# Specify a configuration that should have collisions. We use
# something slightly below zero because if there are only plates, their
# frame is defined such that z=0 may not collide with the sink.
# We purposely make things collide so that we can briefly search for
# the closest collision free configuration.
zs_colliding = np.full(num_manipulands, -0.01)
zs = height_heuristic(plant,
context,
frame_O_list,
zs_colliding,
zs_free,
show=height_heuristic_show)
# Use returned heights in our context.
set_frame_heights(plant, context, frame_O_list, zs)
diagram.Publish(d_context)
time_projection()
time_simulation = make_timer(timing, 'simulation')
simulator.set_target_realtime_rate(sim_rate)
try:
# TODO(eric.cousineau): Also use settling velocities / height as
# terminating criteria (to reject or use a simulation)?
realtime_rates = []
while d_context.get_time() < max_sim_time:
t_real_start = time.time()
simulator.AdvanceTo(d_context.get_time() + sim_dt)
dt_real = time.time() - t_real_start
realtime_rates.append(sim_dt / dt_real)
if np.mean(realtime_rates) < min_realtime_rate:
raise RetryError(time_simulation(), "Sim too slow")
zs = get_frame_heights(plant, context, frame_O_list)
if np.any(zs < 0.):
raise RetryError(time_simulation(), "Manipuland(s) fell")
except RuntimeError as e:
# Try to mitigate solver failures.
is_solver_failure = ("discrete update solver failed to converge"
in str(e))
if is_solver_failure:
raise RetryError(time_simulation(), "Solver failure")
else:
raise
time_simulation()
# Compute poses w.r.t. sink and return.
X_SinkO_list = []
for i, frame_O in enumerate(frame_O_list):
X_SinkO = plant.CalcRelativeTransform(context, frame_Sink, frame_O)
X_SinkO_list.append(X_SinkO)
return X_SinkO_list, timing
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)
table_model = Parser(plant=plant).AddModelFromFile(table_file_path)
object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
# Weld table to world.
plant.WeldFrames(
A=plant.world_frame(),
B=plant.GetFrameByName("link", table_model),
X_AB=RigidTransform(RotationMatrix.MakeXRotation(0.2)))
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=1,
plant=plant,
contact_force_radius=0.005))
contact_input_port = contact_viz.GetInputPort("contact_results")
builder.Connect(
plant.GetOutputPort("contact_results"),
contact_input_port)
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(len(contact_viz._published_contacts), 4)
# y = x
def CopyStateOut(self, context, output):
x = context.get_continuous_state_vector().CopyToVector()
output.SetFromVector(x)
continuous_system = SimpleContinuousTimeSystem()
# Simulation
# Create a simple block diagram containig our system
builder = DiagramBuilder()
# system = builder.AddSystem(SimpleContinuousTimeSystem())
system = builder.AddSystem(continuous_vector_system)
logger = LogOutput(system.get_output_port(0), builder)
diagram = builder.Build()
# Set the initial conditions, x(0)
context = diagram.CreateDefaultContext()
context.SetContinuousState([0.9])
# Create the simulator, and simulate for 10 seconds
simulator = Simulator(diagram, context)
simulator.AdvanceTo(10)
# Plot the results
plt.figure()
plt.plot(logger.sample_times(), logger.data().transpose())
plt.xlabel('t')
plt.ylabel('y(t)')
plt.show()
def test_leaf_system_overrides(self):
test = self
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.called_publish = False
self.called_feedthrough = False
self.called_continuous = False
self.called_discrete = False
self.called_initialize = False
self.called_per_step = False
self.called_periodic = False
# Ensure we have desired overloads.
self._DeclarePeriodicPublish(0.1)
self._DeclarePeriodicPublish(0.1, 0)
self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.1,
offset_sec=0.)
self._DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=self._on_initialize))
self._DeclarePerStepEvent(
event=PublishEvent(trigger_type=TriggerType.kPerStep,
callback=self._on_per_step))
self._DeclarePeriodicEvent(
period_sec=0.1,
offset_sec=0.0,
event=PublishEvent(trigger_type=TriggerType.kPeriodic,
callback=self._on_periodic))
self._DeclareContinuousState(2)
self._DeclareDiscreteState(1)
# Ensure that we have inputs / outputs to call direct
# feedthrough.
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareVectorInputPort(name="test_input",
model_vector=BasicVector(1),
random_type=None)
self._DeclareVectorOutputPort(BasicVector(1), noop)
def _DoPublish(self, context, events):
# Call base method to ensure we do not get recursion.
LeafSystem._DoPublish(self, context, events)
# N.B. We do not test for a singular call to `DoPublish`
# (checking `assertFalse(self.called_publish)` first) because
# the above `_DeclareInitializationEvent` will call both its
# callback and this event when invoked via
# `Simulator::Initialize` from `call_leaf_system_overrides`,
# even when we explicitly say not to publish at initialize.
self.called_publish = True
def _DoHasDirectFeedthrough(self, input_port, output_port):
# Test inputs.
test.assertIn(input_port, [0, 1])
test.assertEqual(output_port, 0)
# Call base method to ensure we do not get recursion.
base_return = LeafSystem._DoHasDirectFeedthrough(
self, input_port, output_port)
test.assertTrue(base_return is None)
# Return custom methods.
self.called_feedthrough = True
return False
def _DoCalcTimeDerivatives(self, context, derivatives):
# Note: Don't call base method here; it would abort because
# derivatives.size() != 0.
test.assertEqual(derivatives.get_vector().size(), 2)
self.called_continuous = True
def _DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
self.called_discrete = True
def _on_initialize(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_initialize)
self.called_initialize = True
def _on_per_step(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
self.called_per_step = True
def _on_periodic(self, context, event):
test.assertIsInstance(context, Context)
test.assertIsInstance(event, PublishEvent)
test.assertFalse(self.called_periodic)
self.called_periodic = True
system = TrivialSystem()
self.assertFalse(system.called_publish)
self.assertFalse(system.called_feedthrough)
self.assertFalse(system.called_continuous)
self.assertFalse(system.called_discrete)
self.assertFalse(system.called_initialize)
results = call_leaf_system_overrides(system)
self.assertTrue(system.called_publish)
self.assertTrue(system.called_feedthrough)
self.assertFalse(results["has_direct_feedthrough"])
self.assertTrue(system.called_continuous)
self.assertTrue(system.called_discrete)
self.assertTrue(system.called_initialize)
self.assertEqual(results["discrete_next_t"], 0.1)
self.assertFalse(system.HasAnyDirectFeedthrough())
self.assertFalse(system.HasDirectFeedthrough(output_port=0))
self.assertFalse(
system.HasDirectFeedthrough(input_port=0, output_port=0))
# Test explicit calls.
system = TrivialSystem()
context = system.CreateDefaultContext()
system.Publish(context)
self.assertTrue(system.called_publish)
context_update = context.Clone()
system.CalcTimeDerivatives(
context, context_update.get_mutable_continuous_state())
self.assertTrue(system.called_continuous)
# Test per-step and periodic call backs
system = TrivialSystem()
simulator = Simulator(system)
simulator.StepTo(0.1)
self.assertTrue(system.called_per_step)
self.assertTrue(system.called_periodic)
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 1)
self.DeclareContinuousState(2)
# qqdot(t) = u(t)
def DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
xdot[0] = x[1]
xdot[1] = u
# y(t) = x(t)
def DoCalcVectorOutput(self, context, u, x, y):
y[:] = x
plant = DoubleIntegrator()
simulator = Simulator(plant)
options = DynamicProgrammingOptions()
def min_time_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
if x.dot(x) < .05:
return 0.
return 1.
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + 20 * u.dot(u)
def test_simulator_api(self):
"""Tests basic Simulator API."""
# TODO(eric.cousineau): Migrate tests from `general_test.py` to here.
system = ConstantVectorSource([1.])
simulator = Simulator(system)
self.assertIs(simulator.get_system(), system)
def build(self, real_time_rate=0, is_visualizing=False):
# Create manipulation station simulator
self.manip_station_sim = ManipulationStationSimulator(
time_step=5e-3,
object_file_path=object_file_path,
object_base_link_name="base_link",
)
# Create plan runner
plan_runner, self.plan_scheduler = CreateManipStationPlanRunnerDiagram(
station=self.manip_station_sim.station,
kuka_plans=[],
gripper_setpoint_list=[],
rl_environment=True)
self.manip_station_sim.plan_runner = plan_runner
# Create builder and add systems
builder = DiagramBuilder()
builder.AddSystem(self.manip_station_sim.station)
builder.AddSystem(plan_runner)
# Connect systems
builder.Connect(
plan_runner.GetOutputPort("gripper_setpoint"),
self.manip_station_sim.station.GetInputPort("wsg_position"))
builder.Connect(
plan_runner.GetOutputPort("force_limit"),
self.manip_station_sim.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(
demux.get_output_port(0),
self.manip_station_sim.station.GetInputPort("iiwa_position"))
builder.Connect(
demux.get_output_port(1),
self.manip_station_sim.station.GetInputPort(
"iiwa_feedforward_torque"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(
self.manip_station_sim.station.GetOutputPort(
"iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add meshcat visualizer
if is_visualizing:
scene_graph = self.manip_station_sim.station.get_mutable_scene_graph(
)
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=False,
plant=self.manip_station_sim.plant)
builder.AddSystem(viz)
builder.Connect(
self.manip_station_sim.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
# Build diagram
self.diagram = builder.Build()
if is_visualizing:
print("Setting up visualizer...")
viz.load()
time.sleep(2.0)
# Construct Simulator
self.simulator = Simulator(self.diagram)
self.manip_station_sim.simulator = self.simulator
self.simulator.set_publish_every_time_step(False)
self.simulator.set_target_realtime_rate(real_time_rate)
self.context = self.diagram.GetMutableSubsystemContext(
self.manip_station_sim.station,
self.simulator.get_mutable_context())
self.left_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"left_door_hinge")
self.right_hinge_joint = self.manip_station_sim.plant.GetJointByName(
"right_door_hinge")
# Goal for training
self.goal_position = np.array([0.85, 0, 0.31])
# Object body
self.obj = self.manip_station_sim.plant.GetBodyByName(
self.manip_station_sim.object_base_link_name,
self.manip_station_sim.object)
# Properties for RL
max_action = np.ones(8) * 0.1
max_action[-1] = 0.03
low_action = -1 * max_action
low_action[-1] = 0
self.action_space = ActionSpace(low=low_action, high=max_action)
self.state_dim = self._getObservation().shape[0]
self._episode_steps = 0
self._max_episode_steps = 75
# Set initial state of the robot
self.reset_sim = False
self.reset()
def RunRealRobot(self, plan_list, gripper_setpoint_list, sim_duration=None, extra_time=2.0,
is_plan_runner_diagram=False,):
"""
Constructs a Diagram that sends commands to ManipulationStationHardwareInterface.
@param plan_list: A list of Plans to be executed.
@param gripper_setpoint_list: A list of gripper setpoints. Each setpoint corresponds to a Plan.
@param sim_duration: the duration of simulation in seconds. If unset, it is set to the sum of the durations of all
plans in plan_list plus extra_time.
@param extra_time: the amount of time for which commands are sent, in addition to the duration of all plans.
@param is_plan_runner_diagram: True: use the diagram version of PlanRunner; False: use the leaf version
of PlanRunner.
@return: logs of robot configuration and torque, decoded from LCM messges sent by the robot's driver.
Logs are SignalLogger systems, whose data can be accessed by SignalLogger.data().
"""
builder = DiagramBuilder()
camera_ids = ["805212060544"]
station_hardware = ManipulationStationHardwareInterface(camera_ids)
station_hardware.Connect(wait_for_cameras=False)
builder.AddSystem(station_hardware)
# Add plan runner.
if is_plan_runner_diagram:
plan_runner = CreateManipStationPlanRunnerDiagram(
station=self.station,
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,)
else:
plan_runner = ManipStationPlanRunner(
station=self.station,
kuka_plans=plan_list,
gripper_setpoint_list=gripper_setpoint_list,
print_period=0,)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.GetOutputPort("gripper_setpoint"),
station_hardware.GetInputPort("wsg_position"))
builder.Connect(plan_runner.GetOutputPort("force_limit"),
station_hardware.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
station_hardware.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
station_hardware.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(station_hardware.GetOutputPort("iiwa_position_measured"),
plan_runner.GetInputPort("iiwa_position"))
builder.Connect(station_hardware.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.GetInputPort("iiwa_velocity"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0), builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
station_hardware.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
station_hardware.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
# build diagram
diagram = builder.Build()
# RenderSystemWithGraphviz(diagram)
# construct simulator
simulator = Simulator(diagram)
self.simulator = simulator
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
t_plan = GetPlanStartingTimes(plan_list)
if sim_duration is None:
sim_duration = t_plan[-1] + extra_time
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, \
iiwa_position_measured_log, iiwa_external_torque_log
# One input, one output, two state variables.
VectorSystem.__init__(self, 1, 2, direct_feedthrough=False)
self.DeclareContinuousState(2)
# qqdot(t) = u(t)
def DoCalcVectorTimeDerivatives(self, context, u, x, xdot):
xdot[0] = x[1]
xdot[1] = u
# y(t) = x(t)
def DoCalcVectorOutput(self, context, u, x, y):
y[:] = x
plant = DoubleIntegrator()
simulator = Simulator(plant)
options = DynamicProgrammingOptions()
def min_time_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
if x.dot(x) < .05:
return 0.
return 1.
def quadratic_regulator_cost(context):
x = context.get_continuous_state_vector().CopyToVector()
u = plant.EvalVectorInput(context, 0).CopyToVector()
return x.dot(x) + 20 * u.dot(u)
def main():
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupClutterClearingStation()
station.Finalize()
package_map = PackageMap()
package_map.PopulateFromEnvironment('AMENT_PREFIX_PATH')
sdf_file_path = os.path.join(package_map.GetPath('iiwa14_description'),
'iiwa14_no_collision.sdf')
joint_names = [
'iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4',
'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'
]
joints = []
for name in joint_names:
joints.append(station.get_controller_plant().GetJointByName(name))
rclpy.init()
node = rclpy.create_node('interactive_demo')
# Publish SDF content on robot_description topic
latching_qos = QoSProfile(depth=1,
durability=DurabilityPolicy.TRANSIENT_LOCAL)
description_publisher = node.create_publisher(StringMsg,
'robot_description',
qos_profile=latching_qos)
msg = StringMsg()
with open(sdf_file_path, 'r') as sdf_file:
msg.data = sdf_file.read()
description_publisher.publish(msg)
clock_system = SystemClock()
builder.AddSystem(clock_system)
# Transform broadcaster for TF frames
tf_broadcaster = TransformBroadcaster(node)
# Connect to system that publishes TF transforms
tf_system = builder.AddSystem(
TFPublisher(tf_broadcaster=tf_broadcaster, joints=joints))
tf_buffer = Buffer(node=node)
tf_listener = TransformListener(tf_buffer, node)
server = InteractiveMarkerServer(node, 'joint_targets')
joint_target_system = builder.AddSystem(
MovableJoints(server, tf_buffer, joints))
fk_system = builder.AddSystem(
ForwardKinematics(station.get_controller_plant()))
builder.Connect(station.GetOutputPort('iiwa_position_measured'),
fk_system.GetInputPort('joint_positions'))
builder.Connect(fk_system.GetOutputPort('transforms'),
tf_system.GetInputPort('body_poses'))
builder.Connect(clock_system.GetOutputPort('clock'),
tf_system.GetInputPort('clock'))
builder.Connect(joint_target_system.GetOutputPort('joint_states'),
station.GetInputPort('iiwa_position'))
ConnectDrakeVisualizer(builder, station.get_scene_graph(),
station.GetOutputPort("pose_bundle"))
constant_sys = builder.AddSystem(ConstantVectorSource(numpy.array([0.107
])))
builder.Connect(constant_sys.get_output_port(0),
station.GetInputPort("wsg_position"))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
station_context = station.GetMyMutableContextFromRoot(simulator_context)
num_iiwa_joints = station.num_iiwa_joints()
station.GetInputPort("iiwa_feedforward_torque").FixValue(
station_context, numpy.zeros(num_iiwa_joints))
while simulator_context.get_time() < 12345:
simulator.AdvanceTo(simulator_context.get_time() + 0.01)
# TODO(sloretz) really need a spin_some in rclpy
rclpy.spin_once(node, timeout_sec=0)
rclpy.shutdown()
def do_main():
rospy.init_node('run_dishrack_interaction', anonymous=False)
#np.random.seed(42)
for outer_iter in range(200):
try:
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.0025))
# Add ground
world_body = mbp.world_body()
ground_shape = Box(2., 2., 2.)
ground_body = mbp.AddRigidBody(
"ground",
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform(p=[0, 0, -1]))
mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(),
ground_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(ground_body,
RigidTransform.Identity(),
ground_shape, "ground_col",
CoulombFriction(0.9, 0.8))
parser = Parser(mbp, scene_graph)
dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf"
cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf"
candidate_model_files = {
#"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf",
"mug_1":
"/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf",
#"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf",
}
# Decide how many of each object to add
max_num_objs = 6
num_objs = [
np.random.randint(0, max_num_objs)
for k in range(len(candidate_model_files.keys()))
]
# Actually produce their initial poses + add them to the sim
poses = [] # [quat, pos]
all_object_instances = []
all_manipulable_body_ids = []
total_num_objs = sum(num_objs)
object_ordering = list(range(total_num_objs))
k = 0
random.shuffle(object_ordering)
print("ordering: ", object_ordering)
for class_k, class_entry in enumerate(
candidate_model_files.items()):
for model_instance_k in range(num_objs[class_k]):
class_name, class_path = class_entry
model_name = "%s_%d" % (class_name, model_instance_k)
all_object_instances.append([class_name, model_name])
model_id = parser.AddModelFromFile(class_path,
model_name=model_name)
all_manipulable_body_ids += mbp.GetBodyIndices(model_id)
# Put them in a randomly ordered line, for placing
y_offset = (object_ordering[k] / float(total_num_objs) -
0.5) # RAnge -0.5 to 0.5
poses.append([
RollPitchYaw(np.random.uniform(
0., 2. * np.pi, size=3)).ToQuaternion().wxyz(),
[-0.25, y_offset, 0.1]
])
k += 1
#$poses.append([
# RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(),
# [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]])
# Build a desk
parser.AddModelFromFile(cupboard_model)
mbp.WeldFrames(world_body.body_frame(),
mbp.GetBodyByName("shelf_origin_body").body_frame(),
RigidTransform(p=[0.0, 0, 0.0]))
#parser.AddModelFromFile(dish_bin_model)
#mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(),
# RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.)))
mbp.AddForceElement(UniformGravityFieldElement())
mbp.Finalize()
hydra_sg_spy = builder.AddSystem(
HydraInteractionLeafSystem(
mbp,
scene_graph,
all_manipulable_body_ids=all_manipulable_body_ids))
#builder.Connect(scene_graph.get_query_output_port(),
# hydra_sg_spy.get_input_port(0))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
hydra_sg_spy.get_input_port(0))
builder.Connect(mbp.get_state_output_port(),
hydra_sg_spy.get_input_port(1))
builder.Connect(hydra_sg_spy.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url="tcp://127.0.0.1:6000",
draw_period=0.01))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
mbp, diagram_context)
sg_context = diagram.GetMutableSubsystemContext(
scene_graph, diagram_context)
q0 = mbp.GetPositions(mbp_context).copy()
for k in range(len(poses)):
offset = k * 7
q0[(offset):(offset + 4)] = poses[k][0]
q0[(offset + 4):(offset + 7)] = poses[k][1]
mbp.SetPositions(mbp_context, q0)
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
simulator.Initialize()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
for k in range(len(poses)):
# Quaternion norm
prog.AddConstraint(squaredNorm, [1], [1],
q_dec[(k * 7):(k * 7 + 4)])
# Trivial quaternion bounds
prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4),
q_dec[(k * 7):(k * 7 + 4)])
# Conservative bounds on on XYZ
prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]),
np.array([2., 2., 2.]),
q_dec[(k * 7 + 4):(k * 7 + 7)])
def vis_callback(x):
vis_diagram_context = diagram.CreateDefaultContext()
mbp.SetPositions(
diagram.GetMutableSubsystemContext(mbp,
vis_diagram_context), x)
pose_bundle = scene_graph.get_pose_bundle_output_port().Eval(
diagram.GetMutableSubsystemContext(scene_graph,
vis_diagram_context))
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(pose_bundle))
visualizer.Publish(context)
prog.AddVisualizationCallback(vis_callback, q_dec)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec)
ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
# print "Initial guess: ", q0
start_time = time.time()
solver = SnoptSolver()
#solver = NloptSolver()
sid = solver.solver_type()
# SNOPT
prog.SetSolverOption(sid, "Print file", "test.snopt")
prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2)
prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Scale option", 0)
#prog.SetSolverOption(sid, "Elastic weight", 1e1)
#prog.SetSolverOption(sid, "Elastic mode", "Yes")
# NLOPT
#prog.SetSolverOption(sid, "initial_step", 0.1)
#prog.SetSolverOption(sid, "xtol_rel", 1E-2)
#prog.SetSolverOption(sid, "xtol_abs", 1E-2)
#prog.SetSolverOption(sid, "Major step limit", 2)
print("Solver opts: ", prog.GetSolverOptions(solver.solver_type()))
result = mp.Solve(prog)
print("Solve info: ", result)
print("Solved in %f seconds" % (time.time() - start_time))
#print(IpoptSolver().Solve(prog))
print(result.get_solver_id().name())
q0_proj = result.GetSolution(q_dec)
# print "Final: ", q0_proj
mbp.SetPositions(mbp_context, q0_proj)
simulator.StepTo(1000)
raise StopIteration()
except StopIteration:
print(colored("Stopped, saving and restarting", 'yellow'))
qf = mbp.GetPositions(mbp_context)
# Decide whether to accept: all objs within bounds
save = True
for k in range(len(all_object_instances)):
offset = k * 7
q = qf[offset:(offset + 7)]
if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[
5] >= 0.2 or q[6] <= -0.1:
save = False
break
if save:
print(colored("Saving", "green"))
save_config(all_object_instances, qf,
"mug_rack_environments_human.yaml")
else:
print(
colored("Not saving due to bounds violation: " + str(q),
"yellow"))
except Exception as e:
print(colored("Suffered other exception " + str(e), "red"))
sys.exit(-1)
except:
print(
colored("Suffered totally unknown exception! Probably sim.",
"red"))
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 = ManipStationPlanRunner(
station=self.station,
kuka_plans=plans_list,
gripper_setpoint_list=gripper_setpoint_list)
builder.AddSystem(plan_runner)
builder.Connect(plan_runner.hand_setpoint_output_port,
self.station.GetInputPort("wsg_position"))
builder.Connect(plan_runner.gripper_force_limit_output_port,
self.station.GetInputPort("wsg_force_limit"))
demux = builder.AddSystem(Demultiplexer(14, 7))
builder.Connect(
plan_runner.GetOutputPort("iiwa_position_and_torque_command"),
demux.get_input_port(0))
builder.Connect(demux.get_output_port(0),
self.station.GetInputPort("iiwa_position"))
builder.Connect(demux.get_output_port(1),
self.station.GetInputPort("iiwa_feedforward_torque"))
builder.Connect(self.station.GetOutputPort("iiwa_position_measured"),
plan_runner.iiwa_position_input_port)
builder.Connect(self.station.GetOutputPort("iiwa_velocity_estimated"),
plan_runner.iiwa_velocity_input_port)
# Add meshcat visualizer
scene_graph = self.station.get_mutable_scene_graph()
viz = MeshcatVisualizer(scene_graph,
is_drawing_contact_force=True,
plant=self.plant)
self.viz = viz
builder.AddSystem(viz)
builder.Connect(self.station.GetOutputPort("pose_bundle"),
viz.GetInputPort("lcm_visualization"))
builder.Connect(self.station.GetOutputPort("contact_results"),
viz.GetInputPort("contact_results"))
# Add logger
iiwa_position_command_log = LogOutput(demux.get_output_port(0),
builder)
iiwa_position_command_log._DeclarePeriodicPublish(0.005)
iiwa_external_torque_log = LogOutput(
self.station.GetOutputPort("iiwa_torque_external"), builder)
iiwa_external_torque_log._DeclarePeriodicPublish(0.005)
iiwa_position_measured_log = LogOutput(
self.station.GetOutputPort("iiwa_position_measured"), builder)
iiwa_position_measured_log._DeclarePeriodicPublish(0.005)
wsg_state_log = LogOutput(
self.station.GetOutputPort("wsg_state_measured"), builder)
wsg_state_log._DeclarePeriodicPublish(0.1)
wsg_command_log = LogOutput(plan_runner.hand_setpoint_output_port,
builder)
wsg_command_log._DeclarePeriodicPublish(0.1)
# 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.SetWsgPosition(0.05, context)
self.station.SetWsgVelocity(0, context)
# set initial hinge angles of the cupboard.
# setting hinge angle to exactly 0 or 90 degrees will result in intermittent contact
# with small contact forces between the door and the cupboard body.
left_hinge_joint = self.plant.GetJointByName("left_door_hinge")
left_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=-0.001)
right_hinge_joint = self.plant.GetJointByName("right_door_hinge")
right_hinge_joint.set_angle(
context=self.station.GetMutableSubsystemContext(
self.plant, context),
angle=0.001)
# set initial pose of the object
self.plant.tree().SetFreeBodyPoseOrThrow(
self.plant.GetBodyByName(self.object_base_link_name, self.object),
self.X_WObject,
self.station.GetMutableSubsystemContext(self.plant, context))
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, \
iiwa_position_measured_log, \
iiwa_external_torque_log, \
wsg_state_log, \
wsg_command_log
